Fix cube coll detection

test_sdl.c

@@ -346,8 +346,8 @@ int main()
 //  addBody(TPE_SHAPE_CAPSULE,256,0,0);
 //addBody(TPE_SHAPE_CAPSULE,300,1024,0);
 
-  addBody(TPE_SHAPE_CUBOID,1024,1024,1024);
-  addBody(TPE_SHAPE_CUBOID,512,3048,3048);
+  addBody(TPE_SHAPE_CUBOID,512,1224,1024);
+  addBody(TPE_SHAPE_CUBOID,4000,512,4000);
 
 bodies[0].body.mass = 3 * TPE_FRACTIONS_PER_UNIT;
 bodies[1].body.mass = TPE_INFINITY;
@@ -366,9 +366,9 @@ bodies[1].body.mass = TPE_INFINITY;
 
   TPE_Unit frame = 0;
 
-bodies[0].body.position = TPE_vec4(500,0,0,0);
-bodies[1].body.position = TPE_vec4(-950,0,0,0);
-bodies[0].body.velocity = TPE_vec4(50,0,0,0);
+bodies[0].body.position = TPE_vec4(0,1600,0,0);
+bodies[1].body.position = TPE_vec4(0,-1000,0,0);
+bodies[0].body.velocity = TPE_vec4(0,-30,0,0);
 
 //TPE_bodyApplyImpulse(&(bodies[0].body),TPE_vec4(256,0,0,0),TPE_vec4(-1,-1,-1,0));
 
@@ -380,7 +380,9 @@ bodies[0].body.velocity = TPE_vec4(50,0,0,0);
 //TPE_bodySetRotation(&(bodies[1].body),TPE_vec4(210,50,1,0),5);
 
 TPE_Vec4 qqq;
-TPE_rotationToQuaternion(TPE_vec4(130,300,50,0),400,&qqq);
+//TPE_rotationToQuaternion(TPE_vec4(80,90,93,0),243,&qqq);
+
+qqq = TPE_vec4(350,270,128,224);
 
 TPE_bodySetOrientation(&(bodies[0].body),qqq);
 
@@ -395,9 +397,8 @@ int collided = 0;
   while (running)
   {
 
-//bodies[0].body.velocity.x -= 1;
 
-bodies[0].body.velocity.x -= 1;
+bodies[0].body.velocity.y -= 1;
 
     for (uint32_t i = 0; i < PIXELS_SIZE; ++i)
       pixels[i] = 0;
@@ -410,7 +411,7 @@ bodies[0].body.velocity.x -= 1;
 
     TPE_Vec4 p, n;
 
-#define BOUND 1000
+#define BOUND 2000
 
 for (int i = 0; i < bodyCount; ++i)
 {
@@ -431,20 +432,27 @@ for (int i = 0; i < bodyCount; ++i)
 printf("\nkin. energy: %d\n",
   TPE_bodyGetKineticEnergy(&bodies[0].body) +
   TPE_bodyGetKineticEnergy(&bodies[1].body));
+*/
+/*
+qqq = TPE_bodyGetOrientation(&(bodies[0].body));
+TPE_PRINTF_VEC4(qqq)
+printf("\n");
 */
     TPE_Unit collDepth = TPE_bodyCollides(&(bodies[1].body),&(bodies[0].body),&p,&n);
 
     if (collDepth)
     {
-
 //if (collided < 3)
 {
+
 TPE_resolveCollision(&(bodies[1].body),&(bodies[0].body), 
-  p,n,collDepth,512);
+  p,n,collDepth,340);
 
+/*
 printf("\nkin. energy: %d\n",
   TPE_bodyGetKineticEnergy(&bodies[0].body) +
   TPE_bodyGetKineticEnergy(&bodies[1].body));
+*/
 }
 
 collided++;
@@ -525,7 +533,7 @@ TPE_vec3Add
         scene.camera.transform.rotation.y -= SHIFT_STEP;
     }
 
-#define SHIFT_STEP 50
+#define SHIFT_STEP 10
 
     if (state[SDL_SCANCODE_L])
       bodies[1].body.position.x += SHIFT_STEP;

tinyphysicsengine.h

@@ -971,7 +971,7 @@ TPE_Unit TPE_bodyCollides(const TPE_Body *body1, const TPE_Body *body2,
           0x10, // -+- --- |
           0x34  // ++- +-- |
         };
- 
+
         for (uint8_t j = 0; j < 12; ++j) // for each edge
         {
           // we check the edge against all sides of the other cuboid
@@ -1092,67 +1092,62 @@ TPE_Unit TPE_bodyCollides(const TPE_Body *body1, const TPE_Body *body2,
         collisionPoint->y /= 2;
         collisionPoint->z /= 2;
         collisionPoint->w = 0;
+          
+        /* We'll find the "closest" side to collision point, compute the
+        penetration depth for both bodies (can't do just one) and return the
+        bigger one. */
 
-        // compute the coll. normal as the axis closest to the coll. point
-
-        TPE_Vec4 bestAxis = TPE_vec4(1,0,0,0);
-        TPE_Unit bestDot = -1;
-        TPE_Unit currentDot;
-
-        uint8_t currentBody = 0;
-        uint8_t bestBody = 0;
-
-        // TODO: optimize this shit? create array instead of aX1, aX2 etc.?
+        TPE_Unit result = -TPE_INFINITY;
 
-        collisionExtentMin = TPE_vec3Minus(*collisionPoint,body1->position); // reuse
+        for (int i = 0; i < 2; ++i) // for each body
+        {
+          TPE_Vec4 bestAxis = TPE_vec4(1,0,0,0);
+          TPE_Unit bestDot = -1;
+          TPE_Unit currentDot;
+                
+          collisionExtentMin = TPE_vec3Minus(*collisionPoint,
+            i == 0 ? body1->position : body2->position); // reuse
+
+          #define checkAxis(a) \
+            currentDot = (TPE_vec3DotProduct(a,collisionExtentMin) * TPE_FRACTIONS_PER_UNIT) / \
+              TPE_nonZero(TPE_vec3DotProduct(a,a)); \
+            if (currentDot > bestDot) \
+              { bestDot = currentDot; bestAxis = a; } \
+            else { \
+              currentDot *= -1; \
+              if (currentDot > bestDot) { \
+                bestDot = currentDot; bestAxis = a; \
+                TPE_vec3MultiplyPlain(bestAxis,-1,&bestAxis); } \
+            }
+
+          checkAxis(aX1)
+          checkAxis(aY1)
+          checkAxis(aZ1)
+
+          #undef checkAxis
+                
+          TPE_Unit len = TPE_nonZero(TPE_vec3Len(bestAxis));
+
+          len = len - TPE_vec3DotProductPlain(bestAxis,
+            TPE_vec3Minus(*collisionPoint,
+            i == 0 ? body1->position : body2->position)) / len;
+
+          if (len > result)
+          {
+            result = len;
+            *collisionNormal = bestAxis;
+            TPE_vec3Normalize(collisionNormal);
 
-        #define checkAxis(a) \
-          currentDot = (TPE_vec3DotProduct(a,collisionExtentMin) * TPE_FRACTIONS_PER_UNIT) / \
-            TPE_nonZero(TPE_vec3DotProduct(a,a)); \
-          if (currentDot > bestDot) \
-            { bestDot = currentDot; bestAxis = a; bestBody = currentBody; } \
-          else { \
-            currentDot *= -1; \
-            if (currentDot > bestDot) { \
-              bestDot = currentDot; bestAxis = a; bestBody = currentBody; \
-              TPE_vec3MultiplyPlain(bestAxis,-1,&bestAxis); } \
+            if (i == 0)
+              TPE_vec3MultiplyPlain(*collisionNormal,-1,collisionNormal); 
           }
 
-        checkAxis(aX1)
-        checkAxis(aY1)
-        checkAxis(aZ1)
-
-        collisionExtentMin = TPE_vec3Minus(*collisionPoint,body2->position);
-
-        currentBody = 1;
-
-        checkAxis(aX2)
-        checkAxis(aY2)
-        checkAxis(aZ2)
-
-        #undef checkAxis
-
-// TODO: optimize/refactor this mess
-
-        *collisionNormal = bestAxis;
-
-        if (bestBody == 0)
-          TPE_vec3MultiplyPlain(*collisionNormal,-1,collisionNormal); 
-
-        TPE_vec3Normalize(collisionNormal);
-
-        TPE_Unit len = TPE_nonZero(TPE_vec3Len(bestAxis));
-
-        TPE_Unit result = len -
-          TPE_vec3DotProductPlain(bestAxis,
-          TPE_vec3Minus(*collisionPoint,
-            bestBody == 0 ? body1->position : body2->position)) / len;
-
-        /* TODO: for some reason negative result is sometimes computed and the
-           next line kind of fixes it, but I don't know why this happens --
-           FIGURE THIS OUT!!! */
+          aX1 = aX2; // check the second body's axes in next iteration
+          aY1 = aY2;
+          aZ1 = aZ2;
+        }
 
-        return result >= 1 ? result : result * -1;
+        return result > 1 ? result : 1;
       }
 
       break;