Fix a few bugs

programs/test.c

@@ -3,7 +3,7 @@
 #include "../tinyphysicsengine.h"
 #include <stdio.h>
 
-#define ass(cond,text) { printf(text ": "); if (!(cond)) { puts("ERROR"); return 1; } else puts("OK"); }
+#define ass(cond,text) { printf("[CHECKING] " text ": "); if (!(cond)) { puts("ERROR"); return 1; } else puts("OK"); }
 
 TPE_Unit rampPoits[6] =
 {
@@ -186,6 +186,12 @@ int main(void)
 
     TPE_worldInit(&w,b,4,envSimple);
 
+    int16_t bi;
+
+    TPE_castBodyRay(TPE_vec3(-1857,3743,-4800),TPE_vec3(0,0,100),-1,&w,&bi,0);
+
+    ass(bi >= 0,"body ray hit");
+
     puts("dropping bodies onto a ramp...");
 
     for (int i = 0; i < 300; ++i)
@@ -193,14 +199,53 @@ int main(void)
       for (uint8_t j = 0; j < w.bodyCount; ++j)
         TPE_bodyApplyGravity(&w.bodies[j],8);
 
+      if (i == 100)
+        ass(TPE_worldGetNetSpeed(&w) > 100,"world net speed");
+
       TPE_worldStep(&w);
     }
 
+    puts("simulation finished");
+
+    uint32_t hash = TPE_worldHash(&w);
+
+    printf("world hash: %lu\n",hash);
+ 
+    // change the hash if functionality changes:
+    ass(hash == 3862131191,"world hash");
+
     for (int i = 0; i < w.bodyCount; ++i)
     {
       ass(TPE_bodyGetCenterOfMass(&w.bodies[i]).x > 0,"x position > 0");
       ass(w.bodies[i].flags & TPE_BODY_FLAG_DEACTIVATED,"deactivated?");
     }
+
+    ass(TPE_worldGetNetSpeed(&w) < 100,"world net speed");
+
+    TPE_bodyAccelerate(&w.bodies[0],TPE_vec3(200,300,-20));
+    TPE_bodyAccelerate(&w.bodies[1],TPE_vec3(-700,400,0));
+    TPE_bodyAccelerate(&w.bodies[2],TPE_vec3(20,-300,-100));
+    TPE_bodyAccelerate(&w.bodies[3],TPE_vec3(0,30,-900));
+
+    puts("exploding bodies...");
+
+    for (int i = 0; i < 100; ++i)
+    {
+      for (uint8_t j = 0; j < w.bodyCount; ++j)
+        TPE_bodyApplyGravity(&w.bodies[j],8);
+
+      TPE_worldStep(&w);
+    }
+
+    // check if within environment
+
+    for (int i = 0; i < w.bodyCount; ++i)
+    {
+      TPE_Vec3 p = TPE_bodyGetCenterOfMass(&w.bodies[i]);
+
+      ass(p.x < 5000 && p.x > -5000 && p.y < 5000 && p.y > -5000 &&
+          p.z < 5000 && p.z > -5000,"body within environment");
+    }
   }
 
   return 0;

tinyphysicsengine.h

@@ -2672,6 +2672,12 @@ TPE_Vec3 TPE_castBodyRay(TPE_Vec3 rayPos, TPE_Vec3 rayDir, int16_t excludeBody,
             {
               // joint hit, compute exact coordinates:
 
+              if (bodyIndex != 0)
+                *bodyIndex = i;
+
+              if (jointIndex != 0)
+                *jointIndex = j;
+
               c = TPE_vec3Times(rayDir,TPE_sqrt(js * js - d * d));
               // ^ offset vector to two intersections
               p = TPE_vec3Plus(p,rayPos);
@@ -2832,12 +2838,12 @@ uint32_t TPE_jointHash(const TPE_Joint *joint)
 {
   uint32_t 
     r = _TPE_hash(joint->position.x);
-    r = _TPE_hash(r + joint->position.y);
-    r = _TPE_hash(r + joint->position.z);
-    r = _TPE_hash(r +
+    r = _TPE_hash(r ^ joint->position.y);
+    r = _TPE_hash(r ^ joint->position.z);
+    r = _TPE_hash(r ^
       (((uint32_t) joint->velocity[0]) |
       (((uint32_t) joint->velocity[1]) << 16)));
-    r = _TPE_hash(r + 
+    r = _TPE_hash(r ^ 
       (((uint32_t) joint->velocity[2]) |
       ((uint32_t) joint->sizeDivided)));
   
@@ -2863,20 +2869,20 @@ uint32_t TPE_bodyHash(const TPE_Body *body)
     (((uint32_t) body->elasticity) << 16));
 
   for (uint8_t i = 0; i < body->jointCount; ++i)
-    r = _TPE_hash(r + TPE_jointHash(&body->joints[i]));
+    r = _TPE_hash(r ^ TPE_jointHash(&body->joints[i]));
 
   for (uint8_t i = 0; i < body->connectionCount; ++i)
-    r = _TPE_hash(r + TPE_connectionHash(&body->connections[i]));
+    r = _TPE_hash(r ^ TPE_connectionHash(&body->connections[i]));
 
   return r;
 }
 
 uint32_t TPE_worldHash(const TPE_World *world)
 {
-  uint8_t r = 0;
+  uint32_t r = 0;
 
   for (uint8_t i = 0; i < world->bodyCount; ++i)
-    r = _TPE_hash(r + TPE_bodyHash(&world->bodies[i]));
+    r = _TPE_hash(r ^ TPE_bodyHash(&world->bodies[i]));
 
   return r;
 }