Reword antiviration

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,2000,1000);
+  addBody(TPE_SHAPE_CUBOID,2000,2000,2000);
   addBody(TPE_SHAPE_CUBOID,15000,512,15000);
 
 bodies[0].body.mass = TPE_FRACTIONS_PER_UNIT * 3;
@@ -383,15 +383,17 @@ 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,100,113,0),80,&qqq);
+TPE_rotationToQuaternion(TPE_vec4(0,0,255,0),64,&qqq);
 
 //qqq = TPE_vec4(350,270,100,224);
 
 TPE_bodySetOrientation(&(bodies[0].body),qqq);
 
+/*
 TPE_Vec4 quat;
-TPE_rotationToQuaternion(TPE_vec4(0,0,255,0),40,&quat);
-TPE_bodySetOrientation(&(bodies[0].body),quat);
+TPE_rotationToQuaternion(TPE_vec4(0,0,255,0),255,&quat);
+TPE_bodySetOrientation(&(bodies[1].body),quat);
+*/
 
 int collided = 0;
 

tinyphysicsengine.h

@@ -71,15 +71,9 @@ typedef int32_t TPE_Unit;
 #define TPE_BODY_FLAG_NONCOLLIDING 0x01 ///< simulated but won't collide
 
                                           // anti-vibration constants:
-#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 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 40   /**< velocity threshold of a
-                                             micro-collision*/
+#define TPE_ANTI_VIBRATION_MAX_FRAMES     100
+#define TPE_ANTI_VIBRATION_INCREMENT      10
+#define TPE_ANTI_VIBRATION_VELOCITY_BREAK 50 
 
 TPE_Unit TPE_wrap(TPE_Unit value, TPE_Unit mod);
 TPE_Unit TPE_clamp(TPE_Unit v, TPE_Unit v1, TPE_Unit v2);
@@ -208,9 +202,7 @@ typedef struct
                                  inferred */
   TPE_Unit boundingSphereRadius;
 
-
-  uint8_t vibrationTime;
-  uint8_t vibrationCountDown;
+  uint8_t antiVibration;
 } TPE_Body;
 
 /** Initializes a physical body, this should be called on all TPE_Body objects
@@ -540,8 +532,7 @@ void TPE_bodyInit(TPE_Body *body)
 
   body->boundingSphereRadius = 0;
 
-body->vibrationTime = 0;
-body->vibrationCountDown = 0;
+  body->antiVibration = 0;
 }
 
 void TPE_bodySetOrientation(TPE_Body *body, TPE_Vec4 orientation)
@@ -1291,6 +1282,22 @@ void TPE_correctEnergies(TPE_Body *body1, TPE_Body *body2,
   }
 }
 
+uint8_t _TPE_bodyUpdateAntivibration(TPE_Body *body)
+{
+  uint8_t tmp = body->antiVibration & 0x7f;
+
+  if (body->antiVibration & 0x80)
+  {
+    tmp = (tmp < 127 - TPE_ANTI_VIBRATION_INCREMENT) ? 
+      tmp + TPE_ANTI_VIBRATION_INCREMENT : 127;
+
+    body->antiVibration = (body->antiVibration & 0x80) |
+      tmp;
+  }
+
+  return tmp <= TPE_ANTI_VIBRATION_MAX_FRAMES;
+}
+
 void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2, 
   TPE_Vec4 collisionPoint, TPE_Vec4 collisionNormal, TPE_Unit collisionDepth,
   TPE_Unit energyMultiplier)
@@ -1339,22 +1346,18 @@ void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2,
     TPE_vec3Add(body2->position,collisionPoint,&body2->position);
   }
 
-  if (collisionDepth >= TPE_VIBRATION_DEPTH_CANCEL)
-  {
-    body1->vibrationCountDown = 0;
-    body2->vibrationCountDown = 0;
-  }
-
-  uint8_t microCollision;
-
   {
     TPE_Vec4 vel1, vel2;
 
     vel1 = TPE_bodyGetPointVelocity(body1,p1);
     vel2 = TPE_bodyGetPointVelocity(body2,p2); 
 
-    microCollision = TPE_vec3Len(TPE_vec3Minus(vel1,vel2)) <= 
-      TPE_VIBRATION_VELOCITY_LIMIT;
+    if (TPE_vec3Len(TPE_vec3Minus(vel1,vel2)) >= 
+      TPE_ANTI_VIBRATION_VELOCITY_BREAK)
+    {
+      body1->antiVibration = 0;
+      body2->antiVibration = 0;
+    }
 
     if (TPE_vec3DotProduct(collisionNormal,vel1) <
       TPE_vec3DotProduct(collisionNormal,vel2))
@@ -1466,36 +1469,20 @@ void TPE_resolveCollision(TPE_Body *body1 ,TPE_Body *body2,
 
   collisionNormal = TPE_vec3Times(collisionNormal,x1);
 
-#if TPE_ANTIVIBRATION
-  if (microCollision)
-  {
-    body1->vibrationCountDown = TPE_VIBRATION_IMPULSE_FRAMES;
-    body2->vibrationCountDown = TPE_VIBRATION_IMPULSE_FRAMES;
-  }
-#endif
-
-  if (body2->vibrationTime <= TPE_VIBRATION_MAX_FRAMES)
-  {
+  if (_TPE_bodyUpdateAntivibration(body2))
     TPE_bodyApplyImpulse(body2,p2,collisionNormal);
-  }
   else
-  {
     TPE_bodyMultiplyKineticEnergy(body2,0);
-    body2->vibrationCountDown = TPE_VIBRATION_IMPULSE_FRAMES;
-  }
 
   if (body1->mass != TPE_INFINITY)
   {
-    if (body1->vibrationTime <= TPE_VIBRATION_MAX_FRAMES)
+    if (_TPE_bodyUpdateAntivibration(body1))
     {
       TPE_vec3MultiplyPlain(collisionNormal,-1,&collisionNormal);
       TPE_bodyApplyImpulse(body1,p1,collisionNormal);
     }
     else
-    {
       TPE_bodyMultiplyKineticEnergy(body1,0);
-      body1->vibrationCountDown = TPE_VIBRATION_IMPULSE_FRAMES;
-    }
   }
 
   // we try to correct possible numerical errors:
@@ -1523,14 +1510,13 @@ void TPE_bodyStep(TPE_Body *body)
     body->rotation.currentAngle += body->rotation.axisVelocity.w;
   }
 
-  if (body->vibrationCountDown == 0)
-    body->vibrationTime = 0;
-  else
+  if ((body->antiVibration & 0x7f) > 0)
   {
-    body->vibrationCountDown--;
+    body->antiVibration = (body->antiVibration & 0x80) |
+      ((body->antiVibration & 0x7f) - 1);
 
-    if (body->vibrationTime < 255)
-      body->vibrationTime++;
+    if (body->antiVibration == 0x80)
+      body->antiVibration = 0;
   }
 }
 
@@ -1549,6 +1535,9 @@ void TPE_bodySetRotation(TPE_Body *body, TPE_Vec4 axis, TPE_Unit velocity)
 
   TPE_vec3Normalize(&axis);
 
+  body->antiVibration = (body->antiVibration & 0x7f) |
+    ((TPE_vec3DotProductPlain(axis,body->rotation.axisVelocity) <= 0) << 7);
+
   body->rotation.axisVelocity = axis;
   body->rotation.axisVelocity.w = velocity;
   body->rotation.currentAngle = 0;