Continue coll

test_sdl.c

@@ -346,10 +346,10 @@ int main()
 //  addBody(TPE_SHAPE_CAPSULE,256,0,0);
 //addBody(TPE_SHAPE_CAPSULE,300,1024,0);
 
-  addBody(TPE_SHAPE_CUBOID,512,1224,1024);
-  addBody(TPE_SHAPE_CUBOID,4000,512,4000);
+  addBody(TPE_SHAPE_CUBOID,2000,2000,2000);
+  addBody(TPE_SHAPE_CUBOID,5000,512,5000);
 
-bodies[0].body.mass = 3 * TPE_FRACTIONS_PER_UNIT;
+bodies[0].body.mass = 20 * TPE_FRACTIONS_PER_UNIT;
 bodies[1].body.mass = TPE_INFINITY;
 
   //-------
@@ -366,7 +366,7 @@ bodies[1].body.mass = TPE_INFINITY;
 
   TPE_Unit frame = 0;
 
-bodies[0].body.position = TPE_vec4(0,1600,0,0);
+bodies[0].body.position = TPE_vec4(0,3000,0,0);
 bodies[1].body.position = TPE_vec4(0,-1000,0,0);
 bodies[0].body.velocity = TPE_vec4(0,0,0,0);
 
@@ -380,9 +380,9 @@ bodies[0].body.velocity = TPE_vec4(0,0,0,0);
 //TPE_bodySetRotation(&(bodies[1].body),TPE_vec4(210,50,1,0),5);
 
 TPE_Vec4 qqq;
-//TPE_rotationToQuaternion(TPE_vec4(80,90,93,0),243,&qqq);
+TPE_rotationToQuaternion(TPE_vec4(80,90,113,0),243,&qqq);
 
-qqq = TPE_vec4(350,270,128,224);
+//qqq = TPE_vec4(350,270,100,224);
 
 TPE_bodySetOrientation(&(bodies[0].body),qqq);
 
@@ -410,7 +410,7 @@ bodies[0].body.velocity.y -= 4;
 
     TPE_Vec4 p, n;
 
-#define BOUND 2000
+#define BOUND 10000
 
 for (int i = 0; i < bodyCount; ++i)
 {

tinyphysicsengine.h

@@ -89,6 +89,7 @@ TPE_Unit TPE_sin(TPE_Unit x);
 TPE_Unit TPE_cos(TPE_Unit x);
 TPE_Unit TPE_asin(TPE_Unit x);
 TPE_Unit TPE_acos(TPE_Unit x);
+uint8_t TPE_sign(TPE_Unit x);
 
 typedef struct
 {
@@ -565,10 +566,6 @@ void TPE_bodyApplyImpulse(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 impulse)
 {
   TPE_Unit pointDistance = TPE_vec3Len(point);
 
-//printf("---\n");
-//TPE_PRINTF_VEC4(impulse)
-//TPE_PRINTF_VEC4(point)
-
   if (pointDistance != 0)  
   {
     impulse.x = (impulse.x * TPE_FRACTIONS_PER_UNIT) / body->mass;
@@ -587,21 +584,22 @@ void TPE_bodyApplyImpulse(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 impulse)
     /* for simplicity we'll suppose angular momentum of a sphere: */
 
     impulse = TPE_vec3Cross(impulse,point);
-//TPE_PRINTF_VEC4(impulse)
+
+   // impulse = TPE_vec3Cross(impulse,point);
 
     TPE_Unit r = TPE_bodyGetMaxExtent(body);
 
-    r = (2 * r * r) / TPE_FRACTIONS_PER_UNIT;
+    r = TPE_nonZero((2 * r * r) / TPE_FRACTIONS_PER_UNIT);
 
-    impulse.x = (5 * impulse.x * TPE_FRACTIONS_PER_UNIT) / r;   
-    impulse.y = (5 * impulse.y * TPE_FRACTIONS_PER_UNIT) / r;   
-    impulse.z = (5 * impulse.z * TPE_FRACTIONS_PER_UNIT) / r;   
+    impulse.x = (impulse.x * 5 * TPE_FRACTIONS_PER_UNIT) / r;   
+    impulse.y = (impulse.y * 5 * TPE_FRACTIONS_PER_UNIT) / r;   
+    impulse.z = (impulse.z * 5 * TPE_FRACTIONS_PER_UNIT) / r;   
 
-    TPE_bodyAddRotation(body,impulse,TPE_vec3Len(impulse));
+TPE_PRINTF_VEC4(impulse)
+printf("\n");
 
-//TPE_PRINTF_VEC4(body->rotation.axisVelocity);
+    TPE_bodyAddRotation(body,impulse,TPE_vec3Len(impulse));
   }
-//printf("\n");
 }
 
 void _TPE_getShapes(const TPE_Body *b1, const TPE_Body *b2, uint8_t shape1,
@@ -1189,12 +1187,22 @@ TPE_vec3MultiplyPlain(normal,-1,&normal); // TODO: think about WHY
 
 void TPE_bodyMultiplyKineticEnergy(TPE_Body *body, TPE_Unit f)
 {
-
   f = TPE_sqrt(f * TPE_FRACTIONS_PER_UNIT);
 
   TPE_vec3Multiply(body->velocity,f,&(body->velocity));
+
+  int8_t sign =
+    TPE_sign(body->rotation.axisVelocity.w);
+
   body->rotation.axisVelocity.w =
     (body->rotation.axisVelocity.w * f) / TPE_FRACTIONS_PER_UNIT;
+
+  /* we try to prevent the angular welocity from falling to 0 as that causes
+    issues with gravity */
+
+  if (f > TPE_FRACTIONS_PER_UNIT / 2 &&
+    sign != 0 && body->rotation.axisVelocity.w == 0)
+    body->rotation.axisVelocity.w = sign;
 }
 
 void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2, 
@@ -1402,7 +1410,8 @@ void TPE_bodyStep(TPE_Body *body)
 
 void TPE_bodySetRotation(TPE_Body *body, TPE_Vec4 axis, TPE_Unit velocity)
 {
-  body->rotation.originalOrientation = TPE_bodyGetOrientation(body);
+  if (body->rotation.currentAngle != 0)
+    body->rotation.originalOrientation = TPE_bodyGetOrientation(body);
 
   if (velocity < 0)
   {
@@ -1993,4 +2002,9 @@ TPE_Unit TPE_timesAntiZero(TPE_Unit a, TPE_Unit b)
     result / TPE_FRACTIONS_PER_UNIT : (result != 0 ? 1 : 0);
 }
 
+uint8_t TPE_sign(TPE_Unit x)
+{
+  return x > 0 ? 1 : (x < 0 ? -1 : 0);
+}
+
 #endif // guard