Fix bug

test_sdl.c

@@ -349,7 +349,7 @@ int main()
 //  addBody(TPE_SHAPE_CAPSULE,256,0,0);
 //addBody(TPE_SHAPE_CAPSULE,300,1024,0);
 
-  addBody(TPE_SHAPE_CUBOID,3000,3000,3000);
+  addBody(TPE_SHAPE_CUBOID,3000,2000,1000);
   addBody(TPE_SHAPE_CUBOID,15000,512,15000);
 
 bodies[0].body.mass = TPE_FRACTIONS_PER_UNIT * 3;
@@ -415,7 +415,7 @@ bodies[0].body.velocity.y -= 4;
 
     TPE_Vec4 p, n;
 
-#define BOUND 10000
+#define BOUND 100000
 
 for (int i = 0; i < bodyCount; ++i)
 {
@@ -437,6 +437,7 @@ 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)
@@ -510,9 +511,10 @@ TPE_vec3Add
 
     S3L_Vec4 camF, camR;
  
-    S3L_rotationToDirections(scene.camera.transform.rotation,20,&camF,&camR,0);
+#define SHIFT_STEP 50
+#define ROT_STEP 5
 
-#define SHIFT_STEP 5
+    S3L_rotationToDirections(scene.camera.transform.rotation,SHIFT_STEP,&camF,&camR,0);
 
     if (state[SDL_SCANCODE_LSHIFT])
     {
@@ -528,13 +530,13 @@ TPE_vec3Add
     else
     {
       if (state[SDL_SCANCODE_UP])
-        scene.camera.transform.rotation.x += SHIFT_STEP;
+        scene.camera.transform.rotation.x += ROT_STEP;
       else if (state[SDL_SCANCODE_DOWN])
-        scene.camera.transform.rotation.x -= SHIFT_STEP;
+        scene.camera.transform.rotation.x -= ROT_STEP;
       else if (state[SDL_SCANCODE_LEFT])
-        scene.camera.transform.rotation.y += SHIFT_STEP;
+        scene.camera.transform.rotation.y += ROT_STEP;
       else if (state[SDL_SCANCODE_RIGHT])
-        scene.camera.transform.rotation.y -= SHIFT_STEP;
+        scene.camera.transform.rotation.y -= ROT_STEP;
     }
 
 #define SHIFT_STEP 50

tinyphysicsengine.h

@@ -71,14 +71,14 @@ typedef int32_t TPE_Unit;
 #define TPE_BODY_FLAG_NONCOLLIDING 0x01 ///< simulated but won't collide
 
                                           // anti-vibration constants:
-#define TPE_ANTIVIBRATION 0               ///< whether to allow anti vibration
-#define TPE_VIBRATION_MAX_FRAMES     50   /**< after how many frames vibration
+#define TPE_ANTIVIBRATION 1               ///< whether to allow anti vibration
+#define TPE_VIBRATION_MAX_FRAMES     60   /**< after how many frames vibration
                                              will be stopped */
-#define TPE_VIBRATION_IMPULSE_FRAMES 10   /**< for how long a micro-impulse will
+#define TPE_VIBRATION_IMPULSE_FRAMES 5    /**< for how long a micro-impulse will
                                              last for detecting vibration */
 #define TPE_VIBRATION_DEPTH_CANCEL   100  /**< what penetration depth will
                                              cancel anti-vibration */
-#define TPE_VIBRATION_VELOCITY_LIMIT 30   /**< velocity threshold of a
+#define TPE_VIBRATION_VELOCITY_LIMIT 40   /**< velocity threshold of a
                                              micro-collision*/
 
 TPE_Unit TPE_wrap(TPE_Unit value, TPE_Unit mod);
@@ -263,6 +263,12 @@ TPE_Unit TPE_bodyGetMaxExtent(const TPE_Body *body);
   for all bodies (when in reality moment of inertia depends on shape). */
 TPE_Unit TPE_bodyGetKineticEnergy(const TPE_Body *body);
 
+/** Attempts to correct rounding errors in the total energy of a system of two
+  bodies after collision, given the initial energy and desired restitution
+  (fraction of energy that should remain after the collision). */
+void TPE_correctEnergies(TPE_Body *body1, TPE_Body *body2,
+  TPE_Unit previousEnergy, TPE_Unit restitution);
+
 /** Collision detection: checks if two bodies are colliding. The return value is
   the collision depth along the collision normal (0 if the bodies are not
   colliding). World-space collision point is returned via a pointer. Collision
@@ -1232,6 +1238,7 @@ void TPE_bodyMultiplyKineticEnergy(TPE_Body *body, TPE_Unit f)
   if (body->mass == TPE_INFINITY)
     return;
 
+  TPE_vec3Multiply(body->velocity,f,&(body->velocity));
   f = TPE_sqrt(f * TPE_FRACTIONS_PER_UNIT);
 
   TPE_vec3Multiply(body->velocity,f,&(body->velocity));
@@ -1250,6 +1257,40 @@ void TPE_bodyMultiplyKineticEnergy(TPE_Body *body, TPE_Unit f)
     body->rotation.axisVelocity.w = sign;
 }
 
+void TPE_correctEnergies(TPE_Body *body1, TPE_Body *body2,
+  TPE_Unit previousEnergy, TPE_Unit restitution)
+{
+  if (previousEnergy == 0)
+    return;
+
+  int8_t r = restitution > TPE_FRACTIONS_PER_UNIT ?
+    1 : (restitution < TPE_FRACTIONS_PER_UNIT ? -1 : 0);
+
+  TPE_Unit newEnergy = TPE_bodyGetKineticEnergy(body1) + 
+      TPE_bodyGetKineticEnergy(body2);
+
+  TPE_Unit f = (newEnergy * TPE_FRACTIONS_PER_UNIT) /
+    previousEnergy;
+
+  restitution = (f != 0) ?
+    (restitution * TPE_FRACTIONS_PER_UNIT) / f : 
+    TPE_FRACTIONS_PER_UNIT;
+
+  if (restitution > TPE_FRACTIONS_PER_UNIT + 2 || // TODO: magic const.
+      restitution < TPE_FRACTIONS_PER_UNIT -2)
+  {
+    f = (previousEnergy * restitution) / TPE_FRACTIONS_PER_UNIT;
+
+    if ((r < 0 && f < previousEnergy) ||
+        (r == 0 && f == previousEnergy) ||
+        (r > 0 && f > previousEnergy))
+    {
+      TPE_bodyMultiplyKineticEnergy(body1,restitution);
+      TPE_bodyMultiplyKineticEnergy(body2,restitution);
+    } 
+  }
+}
+
 void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2, 
   TPE_Vec4 collisionPoint, TPE_Vec4 collisionNormal, TPE_Unit collisionDepth,
   TPE_Unit energyMultiplier)
@@ -1459,20 +1500,7 @@ void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2,
 
   // we try to correct possible numerical errors:
 
-  e1 = ((TPE_bodyGetKineticEnergy(body1) + 
-    TPE_bodyGetKineticEnergy(body2)) * TPE_FRACTIONS_PER_UNIT) /
-    TPE_nonZero(e1 + e2);
-
-  energyMultiplier = 
-    e1 != 0 ?
-      (energyMultiplier * TPE_FRACTIONS_PER_UNIT) / e1 : TPE_FRACTIONS_PER_UNIT;
-
-  if (energyMultiplier > TPE_FRACTIONS_PER_UNIT + 2 || // TODO: magic const.
-    energyMultiplier < TPE_FRACTIONS_PER_UNIT - 2)
-  {
-    TPE_bodyMultiplyKineticEnergy(body1,energyMultiplier);
-    TPE_bodyMultiplyKineticEnergy(body2,energyMultiplier);
-  }
+  TPE_correctEnergies(body1,body2,e1 + e2,energyMultiplier);
 }
 
 TPE_Unit TPE_linearVelocityToAngular(TPE_Unit velocity, TPE_Unit distance)