Add conservation example

programs/2d.c

@@ -13,6 +13,7 @@ TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit maxD)
   return TPE_envAABoxInside(p,TPE_vec3(0,0,0),TPE_vec3(ROOM_W,ROOM_H,ROOM_W));
 }
 
+inactiveCount = 0;
 
 int main(void)
 {
@@ -67,6 +68,11 @@ else
     for (int i = 0; i < tpe_world.bodyCount; ++i)
       TPE_bodyApplyGravity(&tpe_world.bodies[i],5);
 
+
+TPE_Unit speed, speedMax = 0;
+
+int anyActive = 0;
+
 for (int i = 0; i < tpe_world.bodyCount; ++i)
 {
   for (int j = 0; j < tpe_world.bodies[i].jointCount; ++j)
@@ -74,9 +80,32 @@ for (int i = 0; i < tpe_world.bodyCount; ++i)
     tpe_world.bodies[i].joints[j].position.z = 0;
     tpe_world.bodies[i].joints[j].velocity[2] = 0;
   }
+
+if (!(tpe_world.bodies[i].flags & TPE_BODY_FLAG_DEACTIVATED))
+  anyActive = 1;
+
+
+speed = TPE_bodyGetAverageSpeed(&tpe_world.bodies[i]);
+
+if (speed > speedMax)
+  speedMax = speed;
+
+
+
 }
 
 
+if (anyActive && speedMax < 50)
+  inactiveCount++;
+else
+  inactiveCount = 0;
+
+if (inactiveCount > 100)
+{
+  TPE_worldDeactivateAll(&tpe_world);
+  inactiveCount = 0;
+}
+
 
 
 

programs/conservation.c

@@ -0,0 +1,152 @@
+/**
+  Example that fakes the momentum/energy conservation. We simply keep the record
+  of total speed in the system (as an approximation of momentum/energy/whatever)
+  and if it changes too much, we multiply them accordingly to get them back :)
+*/
+
+#include "helper.h"
+
+#define SPHERE_R 3500
+
+TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit maxD)
+{
+TPE_ENV_START( TPE_envSphereInside(p,TPE_vec3(0,0,0),SPHERE_R),p )
+TPE_ENV_END
+}
+
+uint8_t debugDrawOn = 1;
+
+unsigned long timeMeasure = 0;
+
+int main(void)
+{
+  helper_init();
+
+  tpe_world.environmentFunction = environmentDistance;
+
+  s3l_scene.camera.transform.translation.z = -1 * SPHERE_R - 1000;
+
+
+  for (int i = 0; i < 4; ++i)
+  {
+    switch (i)
+    {
+      case 0: helper_addBox(800,800,800,400,700); break;
+      case 1: helper_addBall(500,700); break;
+      case 2: helper_addRect(800,800,400,800); break;
+      case 3: helper_add2Line(900,200,600); break;
+      default: break;
+    }
+
+TPE_Body *b = &tpe_world.bodies[tpe_world.bodyCount - 1];
+
+    TPE_bodyMove(b,TPE_vec3((i - 2) * 1200,0,0));
+
+
+b->friction = 0;
+b->elasticity = TPE_FRACTIONS_PER_UNIT;
+
+TPE_bodyAccelerate(b,
+
+TPE_vec3Plus(
+TPE_vec3Times(TPE_bodyGetCenterOfMass(b),50),
+TPE_vec3(0,10,0))
+
+);
+  } 
+
+TPE_Unit sTotal = TPE_worldGetNetSpeed(&tpe_world);
+
+  while (helper_running)
+  {
+    helper_frameStart();
+
+    helper_cameraFreeMovement();
+
+    if (helper_frame % 16 == 0)
+    {
+      //helper_printCPU();
+      //helper_printCamera();
+
+      if (sdl_keyboard[SDL_SCANCODE_L])
+        for (int i = 0; i < tpe_world.bodyCount; ++i)
+        {
+          TPE_bodyActivate(&tpe_world.bodies[i]);
+          TPE_bodyAccelerate(&tpe_world.bodies[i],
+            TPE_vec3(0,(500 * 30) / FPS,0));
+        }
+
+
+      timeMeasure = 0;
+    }
+
+unsigned long t1 = helper_getMicroSecs();
+
+    TPE_worldStep(&tpe_world);
+
+timeMeasure += helper_getMicroSecs() - t1;
+
+    helper_set3dColor(180,10,10); 
+
+TPE_Unit s = TPE_worldGetNetSpeed(&tpe_world);
+
+TPE_Unit ratio = (sTotal * TPE_FRACTIONS_PER_UNIT) / TPE_nonZero(s);
+
+if (ratio < (4 * TPE_FRACTIONS_PER_UNIT) / 5 ||
+  ratio > (6 * TPE_FRACTIONS_PER_UNIT) / 5)
+{
+
+printf("net speed is now %d but needs to be %d, correcting!\n",s,sTotal);
+
+for (int i = 0; i < tpe_world.bodyCount; ++i)
+  TPE_bodyMultiplyNetSpeed(&tpe_world.bodies[i],ratio);
+
+}
+
+    for (int i = 0; i < tpe_world.bodyCount; ++i)
+    {
+      TPE_Joint *joints = tpe_world.bodies[i].joints;
+      TPE_Vec3 pos = TPE_bodyGetCenterOfMass(&tpe_world.bodies[i]);
+      TPE_Vec3 right = TPE_vec3(512,0,0);
+      TPE_Vec3 forw = TPE_vec3(0,0,512);
+
+TPE_bodyActivate(&tpe_world.bodies[i]);
+
+      if (i != 1)
+      { 
+        if (i != 3)
+        {
+          forw = TPE_vec3Minus(joints[2].position,joints[0].position);
+          right = TPE_vec3Minus(joints[1].position,joints[0].position);
+        }
+        else
+          forw = TPE_vec3Minus(joints[1].position,joints[0].position);
+      }
+
+      TPE_Vec3 orient = TPE_rotationFromVecs(forw,right);
+
+      switch (i % 5)
+      {
+        case 0: helper_draw3dBox(pos,TPE_vec3(1200,1200,1200),orient); break;
+        case 1: helper_draw3dSphere(pos,TPE_vec3(500,500,500),orient); break;
+        case 2: helper_draw3dBox(pos,TPE_vec3(1200,400,1200),orient); break; 
+        case 3: helper_draw3dBox(pos,TPE_vec3(200,200,1200),orient); break;
+        default: break;
+      }
+    }
+
+    helper_set3dColor(200,200,200); 
+
+helper_draw3dSphereInside(TPE_vec3(0,0,0),TPE_vec3(SPHERE_R,SPHERE_R,SPHERE_R),
+  TPE_vec3(0,0,0) );
+
+    if (helper_debugDrawOn)
+      helper_debugDraw(1);
+
+    helper_frameEnd();
+  }
+
+  helper_end();
+
+  return 0;
+}

programs/make.sh

@@ -1,5 +1,5 @@
 #!/bin/bash
 
-PROGRAM=2d # change this to name of a program you want to compile :)
+PROGRAM=conservation # change this to name of a program you want to compile :)
 
 clear; clear; g++ -x c -g -fmax-errors=5 -pedantic -O3 -Wall -Wextra -Wstrict-prototypes -Wold-style-definition -Wno-unused-parameter -Wno-missing-field-initializers -o $PROGRAM $PROGRAM.c -lm -lSDL2 && ./$PROGRAM

tinyphysicsengine.h

@@ -415,8 +415,12 @@ TPE_Vec3 TPE_envInfiniteCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3
 
 void TPE_worldStep(TPE_World *world);
 
-TPE_Unit TPE_bodyNetSpeed(const TPE_Body *body);
-TPE_Unit TPE_bodyAverageSpeed(const TPE_Body *body);
+void TPE_worldDeactivateAll(TPE_World *world);
+
+TPE_Unit TPE_worldGetNetSpeed(const TPE_World *world);
+
+TPE_Unit TPE_bodyGetNetSpeed(const TPE_Body *body);
+TPE_Unit TPE_bodyGetAverageSpeed(const TPE_Body *body);
 
 void TPE_bodyDeactivate(TPE_Body *body);
 
@@ -897,7 +901,7 @@ void TPE_worldStep(TPE_World *world)
       body->deactivateCount = 0;
       body->flags |= TPE_BODY_FLAG_DEACTIVATED;
     }
-    else if (TPE_bodyAverageSpeed(body) <= TPE_LOW_SPEED) // TODO: optimize
+    else if (TPE_bodyGetAverageSpeed(body) <= TPE_LOW_SPEED) // TODO: optimize
       body->deactivateCount++;
     else
       body->deactivateCount = 0;
@@ -916,7 +920,7 @@ void TPE_bodyActivate(TPE_Body *body)
   }
 }
 
-TPE_Unit TPE_bodyNetSpeed(const TPE_Body *body)
+TPE_Unit TPE_bodyGetNetSpeed(const TPE_Body *body)
 {
   TPE_Unit velocity = 0;
 
@@ -933,9 +937,9 @@ TPE_Unit TPE_bodyNetSpeed(const TPE_Body *body)
   return velocity;
 }
 
-TPE_Unit TPE_bodyAverageSpeed(const TPE_Body *body)
+TPE_Unit TPE_bodyGetAverageSpeed(const TPE_Body *body)
 {
-  return TPE_bodyNetSpeed(body) / body->jointCount;
+  return TPE_bodyGetNetSpeed(body) / body->jointCount;
 }
 
 void TPE_bodyMultiplyNetSpeed(TPE_Body *body, TPE_Unit factor)
@@ -958,7 +962,7 @@ void TPE_bodyLimitAverageSpeed(TPE_Body *body, TPE_Unit speedMin,
 {
   for (uint8_t i = 0; i < 16; ++i)
   {
-    TPE_Unit speed = TPE_bodyAverageSpeed(body);
+    TPE_Unit speed = TPE_bodyGetAverageSpeed(body);
 
     if (speed >= speedMin && speed <= speedMax)
       return;
@@ -2476,4 +2480,20 @@ TPE_Vec3 TPE_castBodyRay(TPE_Vec3 rayPos, TPE_Vec3 rayDir, int16_t excludeBody,
   return bestP;
 }
 
+void TPE_worldDeactivateAll(TPE_World *world)
+{
+  for (uint16_t i = 0; i < world->bodyCount; ++i)
+    TPE_bodyDeactivate(&world->bodies[i]);
+}
+
+TPE_Unit TPE_worldGetNetSpeed(const TPE_World *world)
+{
+  TPE_Unit result = 0;
+
+  for (uint16_t i = 0; i < world->bodyCount; ++i)
+    result += TPE_bodyGetNetSpeed(world->bodies + i);
+
+  return result;
+}
+
 #endif // guard