Add env test

TODO.txt

@@ -1,19 +1,19 @@
 TODO:
 - demo: angry-birds-like game
-- elasticity doesn't really work in cases when there is an opposing joint to the
-  colliding joint, the speeds always cancel out -- leave as is or somehow fix?
-- function that tests a validity of an environemnt function, i.e. if it behaves
+- function that tests validity of an environemnt function, i.e. if it behaves
   mathematically correct + test built in env functions with it
-- zero weight joints should behave how?
-- test env functions with a single distinct point near camera
 - env function: heightmap
+- BUG: envBox function doesn't pass the env function test!
 - bounding box/sphere test functions/macros for optimization of environment
   building, plus a demo that tests if it actually accelerates it
 - test ray casting (e.g. the hit of an outside ray should always be outside)
+- try to make better car physics
 
 DONE:
 - demo: car
+- zero weight joints should behave how? <-- disallowed 0 mass
 - demo: testing different frictions on skewed plane, also different elasticities
+- test env functions with a single distinct point near camera
 - bug? bodies stuck inside each other resist falling down by gravity (stack.c)
   ^ Seems to be okay now?
 - test different tick lengths (demo) <-- seems OK
@@ -22,6 +22,9 @@ DONE:
 - check if using fastBSphere vs normal BSphere doesn't affect the simulation
   result (it shouldn't) <-- on stacks.c gave the same world hash
 - fine tune the number of reshapes needed for nice behavior
+- elasticity doesn't really work in cases when there is an opposing joint to the
+  colliding joint, the speeds always cancel out -- leave as is or somehow fix?
+  ^ let's just ignore it
 - body being pushed (e.g. by gravity) onto a sharp edge will likely not resist
   (due to only linear forces in connections) and will very easily be split very
   wide -- TRY AND FIX! Fix could be e.g. in a special function that checks

programs/helper.h

@@ -620,7 +620,7 @@ void s3l_drawPixel(S3L_PixelInfo *p)
   sdl_drawPixel(p->x,p->y,s3l_rr,s3l_gg,s3l_bb);
 }
 
-void helper_set3dColor(uint8_t r, uint8_t g, uint8_t b)
+void helper_set3DColor(uint8_t r, uint8_t g, uint8_t b)
 {
   s3l_r = r;
   s3l_g = g;
@@ -679,7 +679,7 @@ void helper_drawModel(S3L_Model3D *model, TPE_Vec3 pos, TPE_Vec3 scale,
 #endif
 }
 
-void helper_draw3dTriangle(TPE_Vec3 v1, TPE_Vec3 v2, TPE_Vec3 v3)
+void helper_draw3DTriangle(TPE_Vec3 v1, TPE_Vec3 v2, TPE_Vec3 v3)
 {
   triangleVertices[0] = v1.x; 
   triangleVertices[1] = v1.y; 
@@ -696,35 +696,35 @@ void helper_draw3dTriangle(TPE_Vec3 v1, TPE_Vec3 v2, TPE_Vec3 v3)
     TPE_vec3(0,0,0)); 
 }
 
-void helper_draw3dBox(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+void helper_draw3DBox(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   cubeModel.config.backfaceCulling = 2;
   helper_drawModel(&cubeModel,pos,scale,rot);
 }
 
-void helper_draw3dCylinder(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+void helper_draw3DCylinder(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   helper_drawModel(&cylinderModel,pos,scale,rot);
 }
 
-void helper_draw3dBoxInside(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+void helper_draw3DBoxInside(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   cubeModel.config.backfaceCulling = 1;
   helper_drawModel(&cubeModel,pos,scale,rot);
 }
 
-void helper_draw3dPlane(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+void helper_draw3DPlane(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   helper_drawModel(&planeModel,pos,scale,rot);
 }
 
-void helper_draw3dSphere(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+void helper_draw3DSphere(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   sphereModel.config.backfaceCulling = 2;
   helper_drawModel(&sphereModel,pos,scale,rot);
 }
 
-void helper_draw3dSphereInside(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+void helper_draw3DSphereInside(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   sphereModel.config.backfaceCulling = 1;
   helper_drawModel(&sphereModel,pos,scale,rot);

programs/make.sh

@@ -1,5 +1,5 @@
 #!/bin/bash
 
-PROGRAM=water # change this to name of a program you want to compile :)
+PROGRAM=test # 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

programs/stack.c

@@ -69,6 +69,11 @@ int main(void)
 
     timeMeasure += helper_getMicroSecs() - t1;
 
+
+
+
+
+
 if (helper_frame == 200)
   printf("hash: %lu\n",TPE_worldHash(&tpe_world));
 
@@ -94,7 +99,7 @@ if (helper_frame == 200)
 
       TPE_Vec3 orient = TPE_rotationFromVecs(forw,right);
     
-      helper_set3dColor(100 + i * 5,16 - i,100 - i * 5); 
+      helper_set3DColor(100 + i * 5,16 - i,100 - i * 5); 
 
 bodyPositions[i] =
 TPE_vec3KeepWithinBox(
@@ -117,21 +122,21 @@ bodyOrientations[i].z =
 
       switch (i % 5)
       {
-        case 0: helper_draw3dBox(bodyPositions[i],TPE_vec3(1200,1200,1200),bodyOrientations[i]); break;
-        case 1: helper_draw3dTriangle(bodyPositions[i],joints[1].position,joints[2].position); break;
-        case 2: helper_draw3dSphere(bodyPositions[i],TPE_vec3(500,500,500),bodyOrientations[i]); break;
-        case 3: helper_draw3dBox(bodyPositions[i],TPE_vec3(1200,400,1200),bodyOrientations[i]); break; 
-        case 4: helper_draw3dBox(bodyPositions[i],TPE_vec3(200,200,1200),bodyOrientations[i]); break;
+        case 0: helper_draw3DBox(bodyPositions[i],TPE_vec3(1200,1200,1200),bodyOrientations[i]); break;
+        case 1: helper_draw3DTriangle(bodyPositions[i],joints[1].position,joints[2].position); break;
+        case 2: helper_draw3DSphere(bodyPositions[i],TPE_vec3(500,500,500),bodyOrientations[i]); break;
+        case 3: helper_draw3DBox(bodyPositions[i],TPE_vec3(1200,400,1200),bodyOrientations[i]); break; 
+        case 4: helper_draw3DBox(bodyPositions[i],TPE_vec3(200,200,1200),bodyOrientations[i]); break;
         default: break;
       }
     }
 
-    helper_set3dColor(100,100,100); 
-    helper_draw3dTriangle(TPE_vec3(0,0,0),TPE_vec3(-5000,5000,-10000),TPE_vec3(-5000,5000,10000));
-    helper_set3dColor(140,140,140);
-    helper_draw3dTriangle(TPE_vec3(0,0,0),TPE_vec3(-5000,5000,10000),TPE_vec3(5000,5000,0));
-    helper_set3dColor(80,80,80);
-    helper_draw3dTriangle(TPE_vec3(0,0,0),TPE_vec3(-5000,5000,-10000),TPE_vec3(5000,5000,0));
+    helper_set3DColor(100,100,100); 
+    helper_draw3DTriangle(TPE_vec3(0,0,0),TPE_vec3(-5000,5000,-10000),TPE_vec3(-5000,5000,10000));
+    helper_set3DColor(140,140,140);
+    helper_draw3DTriangle(TPE_vec3(0,0,0),TPE_vec3(-5000,5000,10000),TPE_vec3(5000,5000,0));
+    helper_set3DColor(80,80,80);
+    helper_draw3DTriangle(TPE_vec3(0,0,0),TPE_vec3(-5000,5000,-10000),TPE_vec3(5000,5000,0));
 
     if (helper_debugDrawOn)
       helper_debugDraw(1);

programs/test.c

@@ -5,6 +5,37 @@
 
 #define ass(cond,text) { printf(text ": "); if (!(cond)) { puts("ERROR"); return 1; } else puts("OK"); }
 
+TPE_Unit rampPoits[6] =
+{
+  0,0,
+  -2400,1400,
+  -2400,0
+};
+
+TPE_Vec3 envFunc(TPE_Vec3 p, TPE_Unit maxD)
+{
+  TPE_ENV_START( TPE_envAABoxInside(p,TPE_vec3(0,1000,0),TPE_vec3(3000,2500,3000)),p )
+  TPE_ENV_NEXT( TPE_envAATriPrism(p,TPE_vec3(100,200,-10),rampPoits,3000,2),p)
+//TPE_ENV_NEXT( TPE_envBox(p,TPE_vec3(-100,300,200),TPE_vec3(500,600,700),TPE_vec3(10,30,50)), p)
+  TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(-100,300,200),TPE_vec3(500,600,700)), p)
+
+  TPE_ENV_END
+}
+
+TPE_Vec3 envFuncBad(TPE_Vec3 p, TPE_Unit maxD)
+{
+  p.x += 200;
+  return p;
+}
+
+TPE_Vec3 envFuncBad2(TPE_Vec3 p, TPE_Unit maxD)
+{
+  if (p.y > p.x)
+    p.x = p.y;
+
+  return p;
+}
+
 int main(void)
 {
   puts("== testing tinyphysicsengine ==");
@@ -88,6 +119,20 @@ int main(void)
       for (int j = i + 1; j < 4; ++j)
         ass(wHashes[i] != wHashes[j],"world hash");
   }
+ 
+  {
+    puts("-- environment functions --");
+
+    ass(TPE_testClosestPointFunction(envFunc,TPE_vec3(-3000,-3000,-3000),
+      TPE_vec3(3000,3000,3000),32,10,0),"env function");
+
+    ass(!TPE_testClosestPointFunction(envFuncBad,TPE_vec3(-1000,-1000,-1000),
+      TPE_vec3(2000,3000,100),32,10,0),"env function bad");
+
+    ass(!TPE_testClosestPointFunction(envFuncBad2,TPE_vec3(-1000,-2000,-200),
+      TPE_vec3(1000,1000,2000),32,10,0),"env function bad");
+
+  }
 
 
   return 0;

tinyphysicsengine.h

@@ -189,6 +189,7 @@ typedef struct
                                           to keep the body's shape. */
 
 static inline TPE_Unit TPE_abs(TPE_Unit x);
+static inline TPE_Unit TPE_max(TPE_Unit a, TPE_Unit b);
 
 /** Function used for defining static environment, working similarly to an SDF
   (signed distance function). The parameters are: 3D point P, max distance D.
@@ -236,6 +237,24 @@ typedef struct
   TPE_CollisionCallback collisionCallback;
 } TPE_World;
 
+/** Tests the mathematical validity of given closest point function (function
+  representing the physics environment), i.e. whether for example approaching
+  some closest point in a straight line keeps approximately the same closest
+  point. Note that this function may take a long time to complete, especially
+  with higher gridResolution values and more complex environment functions. You
+  should use this function to test your environment function, especially if you
+  create functions for your own shapes etc. The cornerFrom and cornerTo points
+  are corners of an axis-aligned box within which testing will take place, 
+  gridResolution defines numbers of points (i.e. step length) along each
+  dimension to test (recommended e.g. 64), allowedError says error within which
+  points will be considered the same (recommended range approx. 10 to 200). If
+  testing is successful, 1 is returned, otherwise 0 is returned and the point
+  around which error was detected is returned in errorPoint (unless the pointer
+  is 0 in which case it is ignored). */
+uint8_t TPE_testClosestPointFunction(TPE_ClosestPointFunction f,
+  TPE_Vec3 cornerFrom, TPE_Vec3 cornerTo, uint8_t gridResolution,
+  TPE_UnitReduced allowedError, TPE_Vec3 *errorPoint);
+
 void TPE_bodyInit(TPE_Body *body, 
   TPE_Joint *joints, uint8_t jointCount, 
   TPE_Connection *connections, uint8_t connectionCount,
@@ -705,10 +724,7 @@ void TPE_bodyInit(TPE_Body *body,
 
   body->flags = 0;
 
-  body->jointMass = mass / jointCount;
- 
-  if (body->jointMass == 0)
-    body->jointMass = 1;
+  body->jointMass = TPE_nonZero(mass / jointCount);
 
   for (uint32_t i = 0; i < connectionCount; ++i)
   {
@@ -2716,6 +2732,11 @@ TPE_Unit TPE_abs(TPE_Unit x)
   return x >= 0 ? x : (-1 * x);
 }
 
+TPE_Unit TPE_max(TPE_Unit a, TPE_Unit b)
+{
+  return (a > b) ? a : b;
+}
+
 TPE_Vec3 TPE_envAATriPrism(TPE_Vec3 point, TPE_Vec3 center,
   const TPE_Unit sides[6], TPE_Unit depth, uint8_t direction)
 {
@@ -2868,4 +2889,93 @@ void TPE_bodyMoveTo(TPE_Body *body, TPE_Vec3 position)
     body->joints[i].position = TPE_vec3Plus(body->joints[i].position,position);
 }
 
+uint8_t TPE_testClosestPointFunction(TPE_ClosestPointFunction f,
+  TPE_Vec3 cornerFrom, TPE_Vec3 cornerTo, uint8_t gridResolution,
+  TPE_UnitReduced allowedError, TPE_Vec3 *errorPoint)
+{
+  TPE_Vec3 p;
+
+  cornerTo = TPE_vec3Minus(cornerTo,cornerFrom);
+
+  for (uint16_t z = 0; z < gridResolution; ++z)
+  {
+    p.z = cornerFrom.z + (z * cornerTo.z) / gridResolution;
+
+    for (uint16_t y = 0; y < gridResolution; ++y)
+    {
+      p.y = cornerFrom.y + (y * cornerTo.y) / gridResolution;
+
+      for (uint16_t x = 0; x < gridResolution; ++x)
+      {
+        p.x = cornerFrom.x + (x * cornerTo.x) / gridResolution;
+
+        TPE_Vec3 p2 = f(p,TPE_INFINITY);
+
+        if (p.x != p2.x || p.y != p2.y || p.z != p2.z) // only test outside
+        {
+          // 1st try to approach the closest point and see if it stays the same:
+
+          TPE_Vec3 p3 = p;
+
+          for (uint8_t i = 0; i < 3; ++i)
+          {
+            p3 =
+              TPE_vec3((p3.x + p2.x) / 2,(p3.y + p2.y) / 2,(p3.z + p2.z) / 2);
+
+            TPE_Vec3 p4 = f(p3,TPE_INFINITY);
+
+            if (TPE_abs(p4.x - p2.x) + TPE_abs(p4.y - p2.y) 
+              + TPE_abs(p4.z - p2.z) > allowedError) // taxicab dist. for speed
+            {
+              if (errorPoint != 0)
+                *errorPoint = p;
+
+              return 0;
+            }
+          }
+ 
+          // now test 8 points inside the sphere of radius:
+
+          TPE_Unit d = TPE_DISTANCE(p,p2);
+
+          p3.z = p.z - d / 2;
+         
+          for (uint8_t zz = 0; zz < 2; ++zz)
+          {
+            p3.y = p.y - d / 2;
+
+            for (uint8_t yy = 0; yy < 2; ++yy)
+            {
+              p3.x = p.x - d / 2;
+
+              for (uint8_t zz = 0; zz < 2; ++zz)
+              {
+                if (TPE_DISTANCE(p,f(p3,TPE_INFINITY)) + allowedError < d)
+                {
+                  /* In the sphere of distance radius to the original point's
+                     closest point we've gotten a closer point which should
+                     never happen. */
+
+                  if (errorPoint != 0)
+                    *errorPoint = p;
+
+                  return 0;
+                }
+
+                p3.x += d;
+              }
+
+              p3.y += d;
+            }
+ 
+            p3.z += d;
+          }
+        }
+      }
+    }
+  }
+
+  return 1;
+}
+
 #endif // guard