Continue collisions

test.c

@@ -62,12 +62,13 @@ int testColl(const TPE_Body *b1, const TPE_Body *b2,
 #endif
 
   if (!tolerance(ret,expRet) ||
-      !tolerance(p.x,expX) ||
-      !tolerance(p.y,expY) ||
-      !tolerance(p.z,expZ) ||
-      !tolerance(n.x,expNX) ||
-      !tolerance(n.y,expNY) ||
-      !tolerance(n.z,expNZ))
+      ((ret != 0) &&
+       (!tolerance(p.x,expX) ||
+        !tolerance(p.y,expY) ||
+        !tolerance(p.z,expZ) ||
+        !tolerance(n.x,expNX) ||
+        !tolerance(n.y,expNY) ||
+        !tolerance(n.z,expNZ))))
     return 0;
 
   puts("OK");
@@ -90,39 +91,51 @@ int main(void)
   }
 
   {
-    TPE_Body b1, b2;
+    puts("collisions:");
+
+    TPE_Body sphere, cylinder;
     TPE_Vec4 collPoint, collNorm;
   
-    TPE_bodyInit(&b1);
-    TPE_bodyInit(&b2);
+    TPE_bodyInit(&sphere);
+    TPE_bodyInit(&cylinder);
+
+    // sphere, sphere:
 
-    b1.shape = TPE_SHAPE_SPHERE;
-    b2.shape = TPE_SHAPE_SPHERE;
+    sphere.shape = TPE_SHAPE_SPHERE;
+    cylinder.shape = TPE_SHAPE_SPHERE;
     
-    b1.shapeParams[0] = TPE_FRACTIONS_PER_UNIT;
-    b2.shapeParams[1] = TPE_FRACTIONS_PER_UNIT;
+    sphere.shapeParams[0] = TPE_FRACTIONS_PER_UNIT;
+    cylinder.shapeParams[1] = TPE_FRACTIONS_PER_UNIT;
 
-    b1.position = TPE_vec4(TPE_FRACTIONS_PER_UNIT,TPE_FRACTIONS_PER_UNIT / 2,0,0);
-    b2.position = TPE_vec4(TPE_FRACTIONS_PER_UNIT + TPE_FRACTIONS_PER_UNIT / 2,TPE_FRACTIONS_PER_UNIT / 2,0,0);
+    sphere.position = TPE_vec4(F,F / 2,0,0);
+    cylinder.position = TPE_vec4(F + F / 2,F / 2,0,0);
     
-    ASS(testColl(&b1,&b2,256,640,256,0,512,0,0));
-    ASS(testColl(&b2,&b1,256,640,256,0,-512,0,0));
+    ASS(testColl(&sphere,&cylinder,256,640,256,0,512,0,0));
+    ASS(testColl(&cylinder,&sphere,256,640,256,0,-512,0,0));
 
+    // sphere, cylinder:
 
+    sphere.shape = TPE_SHAPE_SPHERE;
+    cylinder.shape = TPE_SHAPE_CYLINDER;
+    
+    sphere.shapeParams[0] = F;
+    cylinder.shapeParams[0] = F * 2;
+    cylinder.shapeParams[1] = F * 3;
 
+    sphere.position.y = 6 * F;
+    ASS(testColl(&sphere,&cylinder,0,0,0,0,0,0,0)); // no collision
 
-    b1.shape = TPE_SHAPE_SPHERE;
-    b2.shape = TPE_SHAPE_CYLINDER;
-    
-    b1.shapeParams[0] = TPE_FRACTIONS_PER_UNIT;
-    b2.shapeParams[0] = TPE_FRACTIONS_PER_UNIT;
-    b2.shapeParams[1] = TPE_FRACTIONS_PER_UNIT * 2;
+    sphere.position = TPE_vec4(F * 3 + F / 2,F,0,0);
+    cylinder.position = TPE_vec4(F,0,0,0);
+    ASS(testColl(&sphere,&cylinder,F / 2,3 * F,F,0,-1 * F,0,0)); // collision with cyl. body
+    ASS(testColl(&cylinder,&sphere,F / 2,3 * F,F,0,F,0,0));
 
-    b1.position = TPE_vec4(TPE_FRACTIONS_PER_UNIT / 2,TPE_FRACTIONS_PER_UNIT * 10,0,0);
-    b2.position = TPE_vec4(0,0,0,0);
-    
-    ASS(testColl(&b1,&b2,0,0,0,0,0,0,0));
+    sphere.position.x = F + F / 2;
+    sphere.position.y = 2 * F;
+    ASS(testColl(&sphere,&cylinder,F / 2,F + F / 2,F + F / 2,0,0,-1 * F,0)); // collision with cyl. body
+    ASS(testColl(&cylinder,&sphere,F / 2,F + F / 2,F + F / 2,0,0,F,0));
 
+ 
 
 
   }

tinyphysicsengine.h

@@ -104,6 +104,7 @@ void TPE_vec3Substract(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result);
 void TPE_vec3Average(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result);
 void TPE_vec4Substract(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result);
 void TPE_vec3Multiply(TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result);
+void TPE_vec3MultiplyPlain(TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result);
 void TPE_vec4Multiply(TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result);
 void TPE_vec3CrossProduct(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result);
 void TPE_vec3Normalize(TPE_Vec4 *v);
@@ -522,19 +523,11 @@ TPE_Vec4 TPE_vec3Cross(TPE_Vec4 a, TPE_Vec4 b)
 void TPE_bodyApplyVelocity(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 velocity)
 {  
   TPE_Vec4 angularVelocity, rotationAxis;
-
-TPE_PRINTF_VEC4(point);
-TPE_PRINTF_VEC4(velocity);
-printf("\n");
-TPE_PRINTF_VEC4(body->velocity);
   
   TPE_vec3Add(body->velocity,velocity,&(body->velocity));
 
   TPE_Unit pointDistance = TPE_vec3Len(point);
 
-TPE_PRINTF_VEC4(body->velocity);
-printf("\n---\n");
-
   if (pointDistance != 0)  
   {
     /* normalize the point, we don't use the function as we don't want to    
@@ -633,37 +626,80 @@ TPE_Unit TPE_bodyCollides(const TPE_Body *body1, const TPE_Body *body2,
              : :     : :
              : :     : : */
 
-      if (sphereAxisDistance >= tmp + sphere->shapeParams[0]) // case C
-        break;
+      if (sphereAxisDistance >= tmp + sphere->shapeParams[0]) // case C: no col.
+        break; 
+
+      TPE_Vec4 sphereAxisToRelative =
+        TPE_vec3Minus(sphereRelativePos,sphereAxisPos);
 
-      TPE_Unit sphereSurfaceDistance = // sphere perpend. dist. to the cylinder
-        TPE_vec3Dist(sphereRelativePos,sphereAxisPos); 
+      TPE_Unit sphereCylinderDistance = TPE_vec3Len(sphereAxisToRelative);
 
       tmp = sphereAxisDistance - tmp;
 
-      if (tmp < 0) // case A
+      if (tmp < 0) // case A: potential collision with cylinder body
       {
-        TPE_Unit penetration = cylinder->shapeParams[0] - sphereSurfaceDistance;
+        TPE_Unit penetration = cylinder->shapeParams[0] 
+          - (sphereCylinderDistance - sphere->shapeParams[0]);
 
         if (penetration > 0)
         {
-          // TODO: NORMAL AND POS!!!
+          TPE_vec3Normalize(&sphereAxisToRelative);
+
+          *collisionPoint = TPE_vec3Plus(cylinder->position,
+            TPE_vec3Plus(sphereAxisPos,TPE_vec3Times(
+            sphereAxisToRelative,cylinder->shapeParams[0])));
+
+          *collisionNormal = sphereAxisToRelative;
+
+          if (sphere == body1)
+            TPE_vec3MultiplyPlain(*collisionNormal,-1,collisionNormal);
+
           return penetration;
         }
+        else
+          break;
       }
 
-      // case B:
+      /* case B: here we have two subcases, one with the sphere center being
+         within the cylinder radius (collision with the cylinder top/bottom),
+         and the other case (collision with the cylinder top/bottom edge). */
+
+      if (sphereCylinderDistance < cylinder->shapeParams[0]) // top/bottom cap
+      {
+        TPE_Unit penetration = cylinder->shapeParams[1] / 2 - 
+          (sphereAxisDistance - sphere->shapeParams[0]);
+
+        if (penetration <= 0) // shouldn't normally happen, but rounding errors 
+          penetration = 1;
+
+        *collisionNormal = TPE_vec3Normalized(sphereAxisPos);
 
-      tmp = // extra penetration depth needed for collision
-        TPE_sqrt(sphere->shapeParams[0] * sphere->shapeParams[0] - tmp * tmp);
+        *collisionPoint = 
+          TPE_vec3Plus(
+            cylinder->position,
+            TPE_vec3Plus(
+              sphereAxisToRelative,
+              TPE_vec3Times(
+                cylinderAxis,cylinder->shapeParams[1] / 2)));
 
-      TPE_Unit penetration = 
-        cylinder->shapeParams[0] - tmp - sphereSurfaceDistance;
+        if (body1 == sphere)
+          TPE_vec3MultiplyPlain(*collisionNormal,-1,collisionNormal);
 
-      if (penetration > 0)
+        return penetration;
+      }
+      else // potential edge collision
       {
-        // TODO: NORMAL AND POS!!!
-        return penetration; // TODO: what is actually the penetration?
+        tmp = // extra penetration depth needed for collision
+          TPE_sqrt(sphere->shapeParams[0] * sphere->shapeParams[0] - tmp * tmp);
+
+        TPE_Unit penetration = cylinder->shapeParams[0] - tmp -
+            (sphereCylinderDistance - sphere->shapeParams[0]);
+
+        if (penetration > 0)
+        {
+          // TODO: NORMAL AND POS!!!
+          return penetration; // TODO: what is actually the penetration?
+        }
       }
 
       break;
@@ -773,9 +809,6 @@ void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2,
   
   TPE_bodyApplyVelocity(body2,p2,
     TPE_vec3Times(collisionNormal,v2ScalarNew - v2Scalar));
-
-printf("=====\n");
-
 }
 
 TPE_Unit TPE_linearVelocityToAngular(TPE_Unit velocity, TPE_Unit distance)
@@ -1061,6 +1094,13 @@ void TPE_vec3Multiply(const TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result)
   result->z = (v.z * f) / TPE_FRACTIONS_PER_UNIT;
 }
 
+void TPE_vec3MultiplyPlain(TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result)
+{
+  result->x = v.x * f;
+  result->y = v.y * f;
+  result->z = v.z * f;
+}
+
 void TPE_vec4Multiply(const TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result)
 {
   result->x = (v.x * f) / TPE_FRACTIONS_PER_UNIT;