Start kin energy

test_sdl.c

@@ -273,6 +273,8 @@ void addBody(uint8_t shape, TPE_Unit param1, TPE_Unit param2, TPE_Unit param3)
   b->body.shapeParams[1] = param2;
   b->body.shapeParams[2] = param3;
 
+  TPE_bodyRecomputeBounds(&b->body);
+
   const S3L_Unit *v;
   const S3L_Index *t;
   S3L_Index vc, tc;
@@ -344,8 +346,8 @@ int main()
 //  addBody(TPE_SHAPE_CAPSULE,256,0,0);
 //addBody(TPE_SHAPE_CAPSULE,300,1024,0);
 
-  addBody(TPE_SHAPE_CUBOID,600,600,600);
-  addBody(TPE_SHAPE_CUBOID,700,400,500);
+  addBody(TPE_SHAPE_CUBOID,1200,1200,1200);
+  addBody(TPE_SHAPE_CUBOID,1400,800,1000);
 
   //-------
   S3L_Model3D models[bodyCount];
@@ -356,16 +358,21 @@ int main()
   S3L_Scene scene;
   S3L_initScene(models,bodyCount,&scene);
   
-  scene.camera.transform.translation.z = -4 * S3L_FRACTIONS_PER_UNIT;
+  scene.camera.transform.translation.z = -8 * S3L_FRACTIONS_PER_UNIT;
   //-------
 
   TPE_Unit frame = 0;
 
-bodies[0].body.position.x = -350;
-bodies[1].body.position.x = 400;
+bodies[0].body.position.x = -700;
+bodies[1].body.position.x = 800;
 bodies[1].body.position.z = -100;
 
-bodies[0].body.velocity = TPE_vec4(20,20,0,0);
+bodies[0].body.position = TPE_vec4(2875,-950,0,0);
+bodies[1].body.position = TPE_vec4(-1725,-550,-100,0);
+bodies[0].body.velocity = TPE_vec4(150,0,0,0);
+bodies[1].body.velocity = TPE_vec4(0,100,0,0);
+
+//bodies[0].body.velocity = TPE_vec4(150,100,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);
@@ -395,7 +402,7 @@ int collided = 0;
 
     TPE_Vec4 p, n;
 
-#define BOUND 1000
+#define BOUND 3000
 for (int i = 0; i < bodyCount; ++i)
 {
   if (bodies[i].body.position.x > BOUND ||
@@ -411,14 +418,16 @@ for (int i = 0; i < bodyCount; ++i)
     bodies[i].body.velocity.z *= -1;
 }
 
+printf("\nkin. energy: %d\n",
+  TPE_bodyGetKineticEnergy(&bodies[0].body) +
+  TPE_bodyGetKineticEnergy(&bodies[1].body));
+
 
     TPE_Unit collDepth = TPE_bodyCollides(&(bodies[1].body),&(bodies[0].body),&p,&n);
 
     if (collDepth)
     {
-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), 
@@ -502,6 +511,8 @@ TPE_vec3Add
         scene.camera.transform.rotation.y -= SHIFT_STEP;
     }
 
+#define SHIFT_STEP 50
+
     if (state[SDL_SCANCODE_L])
       bodies[1].body.position.x += SHIFT_STEP;
     else if (state[SDL_SCANCODE_J])

tinyphysicsengine.h

@@ -190,12 +190,17 @@ typedef struct
                                  the rotation axis, angular momentum and data
                                  from which current orientation can be
                                  inferred */
+  TPE_Unit boundingSphereRadius;
 } TPE_Body;
 
 /** Initializes a physical body, this should be called on all TPE_Body objects
   that are created.*/
 void TPE_bodyInit(TPE_Body *body);
 
+/** Recomputes the body bounding sphere, must be called every time the body's
+  shape parameters change. */
+void TPE_bodyRecomputeBounds(TPE_Body *body);
+
 /** Computes a 4x4 transform matrix of given body. The matrix has the same
   format as S3L_Mat4 from small3dlib. */
 void TPE_bodyGetTransformMatrix(const TPE_Body *body, TPE_Unit matrix[4][4]);
@@ -224,13 +229,24 @@ void TPE_bodyAddRotation(TPE_Body *body, TPE_Vec4 axis, TPE_Unit velocity);
   similar to an impulse but doesn't take mass into account, only velocity. */
 void TPE_bodyApplyVelocity(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 velocity);
 
+/** Computes and returns a body's bounding sphere radius, i.e. the maximum
+  extent from its center point. */
+TPE_Unit TPE_bodyGetMaxExtent(const TPE_Body *body);
+
+/** Computes and returns a body's total kinetic energy (sum of linear and
+  rotational kin. energy). In rotating bodies this may not be physically
+  accurate as, for simplicity, we operate with the moment of inertia of sphere
+  for all bodies (when in reality moment of inertia depends on shape). */
+TPE_Unit TPE_bodyGetKineticEnergy(const TPE_Body *body);
+
 /** 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
   normal (normalized) is also returned via a pointer and its direction is
   "away from body1", i.e. if you move body1 in the opposite direction of this
   normal by the collision depth (return value), the bodies should no longer
-  exhibit this particular collision. */
+  exhibit this particular collision. This function checks the bounding spheres
+  to quickly opt out of impossible collisions. */
 TPE_Unit TPE_bodyCollides(const TPE_Body *body1, const TPE_Body *body2, 
   TPE_Vec4 *collisionPoint, TPE_Vec4 *collisionNormal);
 
@@ -490,6 +506,8 @@ void TPE_bodyInit(TPE_Body *body)
   body->rotation.currentAngle = 0;
 
   body->mass = TPE_FRACTIONS_PER_UNIT;
+
+  body->boundingSphereRadius = 0;
 }
 
 void TPE_bodySetOrientation(TPE_Body *body, TPE_Vec4 orientation)
@@ -562,9 +580,9 @@ void TPE_bodyApplyVelocity(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 velocity)
     TPE_vec3Substract(velocity,tmp,&angularVelocity);
     TPE_vec3CrossProduct(point,angularVelocity,&rotationAxis);
 
-    TPE_bodyAddRotation(body,rotationAxis,
-      TPE_linearVelocityToAngular(
-        TPE_vec3Len(angularVelocity),-1 * pointDistance));
+//    TPE_bodyAddRotation(body,rotationAxis,
+//      TPE_linearVelocityToAngular(
+//        TPE_vec3Len(angularVelocity),-1 * pointDistance));
   }
 }
 
@@ -649,6 +667,19 @@ TPE_Unit TPE_bodyCollides(const TPE_Body *body1, const TPE_Body *body2,
 {
   // handle collision of different shapes each in a specific case:
 
+  uint16_t collType = TPE_COLLISION_TYPE(body1->shape,body2->shape);
+
+  if (collType != TPE_COLLISION_TYPE(TPE_SHAPE_SPHERE,TPE_SHAPE_SPHERE))
+  {
+    // initial bounding sphere check to quickly discard impossible collisions
+
+    // TODO: taxicab could be also considered here
+
+    if (TPE_vec3Len(TPE_vec3Minus(body1->position,body2->position)) >
+        body1->boundingSphereRadius + body2->boundingSphereRadius)
+      return 0;
+  } 
+
   switch (TPE_COLLISION_TYPE(body1->shape,body2->shape))
   {
     case TPE_COLLISION_TYPE(TPE_SHAPE_SPHERE,TPE_SHAPE_SPHERE):
@@ -1171,17 +1202,6 @@ TPE_vec3Substract(body1->position,collisionPoint,&body1->position);
   tmp = TPE_vec3DotProduct(v2,collisionNormal);
   int8_t v2Sign = tmp > 0 ? 1 : (tmp < 0 ? -1 : 0);
 
-TPE_PRINTF_VEC4(collisionNormal)
-printf("\n");
-
-TPE_PRINTF_VEC4(body1->position)
-TPE_PRINTF_VEC4(body2->position)
-printf("%d %d\n",v1Sign,v2Sign);
-
-TPE_PRINTF_VEC4(v1)
-TPE_PRINTF_VEC4(v2)
-printf("\n");
-
   if (v1Sign == -1 && v2Sign != -1)
     return; // opposite going velocities => not a real collision
 
@@ -1787,4 +1807,46 @@ TPE_Vec4 TPE_lineSegmentClosestPoint(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 p)
   return TPE_vec3Plus(a,ab);
 }
 
+TPE_Unit TPE_bodyGetKineticEnergy(const TPE_Body *body)
+{
+  TPE_Unit v = TPE_vec3Len(body->velocity);
+
+  v *= v;
+
+  v = (v == 0 || v >= TPE_FRACTIONS_PER_UNIT) ?
+    v / TPE_FRACTIONS_PER_UNIT : 1;
+
+// TODO: handle small values
+
+  return (body->mass * v) /
+    (2 * TPE_FRACTIONS_PER_UNIT);
+
+  // TODO: rot
+}
+
+TPE_Unit TPE_bodyGetMaxExtent(const TPE_Body *body)
+{
+  switch (body->shape)
+  {
+    case TPE_SHAPE_SPHERE:
+      return body->shapeParams[0]; 
+      break;
+
+    case TPE_SHAPE_CUBOID:
+      return TPE_vec3Len(TPE_vec4(
+        body->shapeParams[0] / 2,
+        body->shapeParams[1] / 2,
+        body->shapeParams[2] / 2,0));
+      break;
+
+    // TODO: other shapes
+    default: return 0; break;
+  }
+}
+
+void TPE_bodyRecomputeBounds(TPE_Body *body)
+{
+  body->boundingSphereRadius = TPE_bodyGetMaxExtent(body);
+}
+
 #endif // guard