Continue coll resolving

test_sdl.c

@@ -365,10 +365,10 @@ bodies[0].body.position.x = -350;
 bodies[1].body.position.x = 400;
 bodies[1].body.position.z = -100;
 
-//bodies[0].body.velocity = TPE_vec4(20,20,0,0);
+bodies[0].body.velocity = TPE_vec4(20,20,0,0);
 
-TPE_bodySetRotation(&(bodies[0].body),TPE_vec4(0,128,0,0),8);
-TPE_bodySetRotation( &(bodies[1].body),TPE_vec4(210,50,1,0),5);
+//TPE_bodySetRotation(&(bodies[0].body),TPE_vec4(0,128,0,0),8);
+//TPE_bodySetRotation( &(bodies[1].body),TPE_vec4(210,50,1,0),5);
 /*
 TPE_Vec4 quat;
 TPE_rotationToQuaternion(TPE_vec4(0,0,255,0),40,&quat);
@@ -395,7 +395,7 @@ int collided = 0;
 
     TPE_Vec4 p, n;
 
-#define BOUND 2000
+#define BOUND 1000
 for (int i = 0; i < bodyCount; ++i)
 {
   if (bodies[i].body.position.x > BOUND ||
@@ -416,11 +416,15 @@ for (int i = 0; i < bodyCount; ++i)
 
     if (collDepth)
     {
-//if (!collided)
+printf("momentum: %d\n",
+TPE_vec3Len(bodies[0].body.velocity) + 
+TPE_vec3Len(bodies[1].body.velocity));
+
+//if (collided < 2)
 TPE_resolveCollision(&(bodies[1].body),&(bodies[0].body), 
   p,n,collDepth);
 
-collided = 1;
+collided++;
 
       S3L_Vec4 p2, scr;
      
@@ -522,7 +526,7 @@ TPE_vec3Add
     SDL_RenderCopy(renderer,textureSDL,NULL,NULL);
     SDL_RenderPresent(renderer);
 
-    usleep(20000);
+    usleep(10000);
 
     frame++;
   }

tinyphysicsengine.h

@@ -1142,12 +1142,24 @@ printf("---\n");
   p1 = TPE_vec3Minus(collisionPoint,body1->position);
   p2 = TPE_vec3Minus(collisionPoint,body2->position);
 
+
+// separate the bodies:
+
+collisionPoint = collisionNormal; // reuse collisionPoint
+TPE_vec3Multiply(collisionPoint,collisionDepth,&collisionPoint);
+TPE_vec3Add(body2->position,collisionPoint,&body2->position);
+TPE_vec3Substract(body1->position,collisionPoint,&body1->position);
+
+
+
   v1 = TPE_bodyGetPointVelocity(body1,p1); 
   v2 = TPE_bodyGetPointVelocity(body2,p2);
+ 
+  TPE_Unit tmp = TPE_vec3DotProduct(v1,collisionNormal);
+  int8_t v1Sign = tmp > 0 ? 1 : (tmp < 0 ? -1 : 0);
 
-  int8_t 
-    v1Sign = TPE_vec3DotProduct(v1,collisionNormal) >= 0,
-    v2Sign = TPE_vec3DotProduct(v2,collisionNormal) >= 0;
+  tmp = TPE_vec3DotProduct(v2,collisionNormal);
+  int8_t v2Sign = tmp > 0 ? 1 : (tmp < 0 ? -1 : 0);
 
 TPE_PRINTF_VEC4(collisionNormal)
 printf("\n");
@@ -1160,8 +1172,8 @@ TPE_PRINTF_VEC4(v1)
 TPE_PRINTF_VEC4(v2)
 printf("\n");
 
-//  if (!v1Sign && v2Sign)
-//    return; // opposite going velocities => not a real collision
+  if (v1Sign == -1 && v2Sign != -1)
+    return; // opposite going velocities => not a real collision
 
   /* if the velocities are too small, weird behavior occurs, so we define a min
     velocity for collisions and potentially modify the velocities: */
@@ -1192,12 +1204,12 @@ printf("\n");
   TPE_vec3Project(v2,collisionNormal,&v2);
 
   TPE_Unit 
-    v1Scalar = TPE_vec3Len(v1) * (v1Sign ? 1 : -1),
-    v2Scalar = TPE_vec3Len(v2) * (v2Sign ? 1 : -1);
+    v1Scalar = TPE_vec3Len(v1) * v1Sign,
+    v2Scalar = TPE_vec3Len(v2) * v2Sign;
 
-//  if ((v1Sign && v2Sign && (v2Scalar > v1Scalar)) ||
-//    (!v1Sign && !v2Sign && (v1Scalar > v2Scalar)))
-//    return; // not a valid collision
+  if ((v1Sign == 1 && v2Sign == 1 && (v2Scalar > v1Scalar)) ||
+    (v1Sign == -1 && v2Sign == -1 && (v1Scalar > v2Scalar)))
+    return; // not a valid collision
 
   TPE_Unit
     v1ScalarNew = v1Scalar,
@@ -1210,7 +1222,7 @@ printf("\n");
     body2->mass,
     512); // TODO: elasticity
 
-// TODO: ACTUALLY MAKE SURE ENERGY IS CONSERVED (rounding errors may add energy!)
+// TODO: ACTUALLY MAKE SURE MOMENTUM IS CONSERVED (rounding errors may add energy!)
 
 //TPE_vec3MultiplyPlain(collisionNormal,-1,&collisionNormal);
 
@@ -1486,6 +1498,25 @@ TPE_Vec4 TPE_vec3Times(TPE_Vec4 a, TPE_Unit f)
 
 TPE_Vec4 TPE_vec3TimesAntiZero(TPE_Vec4 a, TPE_Unit f)
 {
+  a.x *= f;
+
+  if (a.x != 0)
+    a.x = a.x >= TPE_FRACTIONS_PER_UNIT ? a.x / TPE_FRACTIONS_PER_UNIT : 
+      (a.x > 0 ? 1 : -1);
+
+  a.y *= f;
+
+  if (a.y != 0)
+    a.y = a.y >= TPE_FRACTIONS_PER_UNIT ? a.y / TPE_FRACTIONS_PER_UNIT :
+      (a.y > 0 ? 1 : -1);
+
+  a.z *= f;
+
+  if (a.z != 0)
+    a.z = a.z >= TPE_FRACTIONS_PER_UNIT ? a.z / TPE_FRACTIONS_PER_UNIT :
+      (a.z > 0 ? 1 : -1);
+
+/*
   if (a.x != 0)
     a.x = a.x >= TPE_FRACTIONS_PER_UNIT ? a.x / TPE_FRACTIONS_PER_UNIT : 
       (a.x > 0 ? 1 : -1);
@@ -1497,6 +1528,7 @@ TPE_Vec4 TPE_vec3TimesAntiZero(TPE_Vec4 a, TPE_Unit f)
   if (a.z != 0)
     a.z = a.z >= TPE_FRACTIONS_PER_UNIT ? a.z / TPE_FRACTIONS_PER_UNIT :
       (a.z > 0 ? 1 : -1);
+*/
 
   return a;
 }