Update player demo

TODO.txt

@@ -8,6 +8,9 @@ TODO:
 - test/handle: collision where the velocity goes "away" from the collision (can
   happen e.g. if one body is hit by a faster body OR by manual manipulation with
   velocities)
+- function for "faking" rotation of a single-joint body (ball): it will simply
+  rotate by the axis perpendicular to its linear velocity vec
+- BUG? makeCenterBox doesnt seem to add the center joint: check it out
 
 DONE:
 - zero sized joints should never collide (can be useful) <- NO because 0 size

programs/cubes.c

@@ -16,13 +16,7 @@ TPE_Connection ballConnections[3];
 
 void updateOrientPos(int i)
 {
-  TPE_Joint *joints = tpe_world.bodies[i].joints;
-
-  cubeOrientations[i] = TPE_orientationFromVecs(
-    TPE_vec3Minus(joints[2].position,joints[0].position),
-    TPE_vec3Minus(joints[1].position,joints[0].position));
-
-  cubePositions[i] = TPE_bodyGetCenter(&tpe_world.bodies[i]);
+  cubeOrientations[i] = TPE_bodyGetOrientation(&tpe_world.bodies[i],0,2,1);
 }
 
 int main(void)

programs/helper.h

@@ -36,6 +36,12 @@
 
 #define S3L_PIXEL_FUNCTION s3l_drawPixel
 
+#ifndef SCALE_3D_RENDERING
+  #define SCALE_3D_RENDERING 1 // helper divider for preveneting overflows
+#endif
+
+#define S3L_NEAR (S3L_FRACTIONS_PER_UNIT / (4 * SCALE_3D_RENDERING))
+
 #ifndef S3L_SORT
   #define S3L_SORT 0
 #endif
@@ -44,8 +50,16 @@
   #define S3L_Z_BUFFER 1
 #endif
 
-#define S3L_NEAR_CROSS_STRATEGY 2
+#ifndef S3L_PERSPECTIVE_CORRECTION
+  #define S3L_PERSPECTIVE_CORRECTION 2
+#endif
+
+#ifndef S3L_NEAR_CROSS_STRATEGY
+  #define S3L_NEAR_CROSS_STRATEGY 2
+#endif
+
 #define S3L_USE_WIDER_TYPES 1
+
 #include "small3dlib.h"
 
 #define PIXELS_SIZE (S3L_RESOLUTION_X * S3L_RESOLUTION_Y * 4)
@@ -286,6 +300,15 @@ void helper_addTriangle(TPE_Unit s, TPE_Unit d, TPE_Unit mass)
   _helper_bodyAdded(3,3,mass);
 }
 
+void helper_addCenterRect(TPE_Unit w, TPE_Unit d, TPE_Unit jointSize, TPE_Unit mass)
+{
+  TPE_makeCenterRect(
+    tpe_joints + helper_jointsUsed,
+    tpe_connections + helper_connectionsUsed,w,d,jointSize);
+
+  _helper_bodyAdded(5,8,mass);
+}
+
 void helper_addRect(TPE_Unit w, TPE_Unit d, TPE_Unit jointSize, TPE_Unit mass)
 {
   TPE_makeRect(
@@ -476,7 +499,35 @@ void helper_drawModel(S3L_Model3D *model, TPE_Vec3 pos, TPE_Vec3 scale,
 
   s3l_scene.models = model;
 
+#if SCALE_3D_RENDERING != 1
+
+  S3L_Vec4 cp = s3l_scene.camera.transform.translation;
+  S3L_Vec4 ms = s3l_scene.models[0].transform.scale;
+  S3L_Vec4 mp = s3l_scene.models[0].transform.translation;
+
+  s3l_scene.camera.transform.translation.x /= SCALE_3D_RENDERING;
+  s3l_scene.camera.transform.translation.y /= SCALE_3D_RENDERING;
+  s3l_scene.camera.transform.translation.z /= SCALE_3D_RENDERING;
+
+  s3l_scene.models[0].transform.scale.x /= SCALE_3D_RENDERING;
+  s3l_scene.models[0].transform.scale.y /= SCALE_3D_RENDERING;
+  s3l_scene.models[0].transform.scale.z /= SCALE_3D_RENDERING;
+
+  s3l_scene.models[0].transform.translation.x /= SCALE_3D_RENDERING;
+  s3l_scene.models[0].transform.translation.y /= SCALE_3D_RENDERING;
+  s3l_scene.models[0].transform.translation.z /= SCALE_3D_RENDERING;
+
+#endif
+
   S3L_drawScene(s3l_scene);
+
+#if SCALE_3D_RENDERING != 1
+
+  s3l_scene.camera.transform.translation = cp;
+  s3l_scene.models[0].transform.scale = ms;
+  s3l_scene.models[0].transform.translation = mp;
+
+#endif
 }
 
 void helper_draw3dTriangle(TPE_Vec3 v1, TPE_Vec3 v2, TPE_Vec3 v3)

programs/player.c

@@ -1,3 +1,5 @@
+#define SCALE_3D_RENDERING 8
+
 #include "helper.h"
 
 #define ROOM_SIZE 10000
@@ -9,14 +11,10 @@ TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit maxD)
   TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(4000,160,4000),TPE_vec3(1000,160,1000)),p )
   TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(4000,80,2500),TPE_vec3(1000,80,500)),p )
   TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(-1000,270,4500),TPE_vec3(4000,270,250)),p )
+  TPE_ENV_NEXT( TPE_envHalfPlane(p,TPE_vec3(0,0,-2000),TPE_vec3(0,255,255)),p )
   TPE_ENV_END
 }
 
-/*
-320
-320 + 160
-*/
-
 int jumpCountdown = 0;
 TPE_Unit rotation = 0;
 int onGroundCount = 0;
@@ -67,7 +65,16 @@ tpe_world.bodies[0].friction = 0;
 helper_addBall(1000,100);
 TPE_bodyMove(&tpe_world.bodies[1],TPE_vec3(-1000,1000,0));
 
-tpe_world.bodies[1].elasticity = 512;
+tpe_world.bodies[1].elasticity = 400;
+tpe_world.bodies[1].friction = 100;
+ 
+//helper_addBox(400,400,400,200,300);
+helper_addCenterRect(600,600,400,50);
+//helper_addBox(500,500,500,250,300);
+TPE_bodyMove(&tpe_world.bodies[2],TPE_vec3(-3000,1000,2000));
+
+tpe_world.bodies[2].elasticity = 400;
+tpe_world.bodies[2].friction = 50;
  
   while (helper_running)
   {
@@ -140,8 +147,22 @@ tpe_world.bodies[1].elasticity = 512;
     helper_draw3dCubeInside(TPE_vec3(0,ROOM_SIZE / 4,0),TPE_vec3(ROOM_SIZE,ROOM_SIZE / 2,ROOM_SIZE),TPE_vec3(0,0,0));
     helper_draw3dCube(TPE_vec3(4000,160,4000),TPE_vec3(2000,320,2000),TPE_vec3(0,0,0));
     helper_draw3dCube(TPE_vec3(4000,80,2500),TPE_vec3(2000,160,1000),TPE_vec3(0,0,0));
-
     helper_draw3dCube(TPE_vec3(-1000,270,4500),TPE_vec3(8000,540,500),TPE_vec3(0,0,0));
+
+helper_draw3dPlane(
+TPE_vec3(0,1500,-3500),
+TPE_vec3(10000,512,4000),
+TPE_vec3(-64,0,0));
+    
+helper_set3dColor(200,50,0);
+
+helper_draw3dCube(
+TPE_bodyGetCenter(&tpe_world.bodies[2]),
+TPE_vec3(1200,800,1200),
+TPE_bodyGetOrientation(&tpe_world.bodies[2],0,2,1)
+
+);
+
     
 
 helper_draw3dSphere(

programs/small3dlib.h

@@ -10,7 +10,7 @@
   license: CC0 1.0 (public domain)
            found at https://creativecommons.org/publicdomain/zero/1.0/
            + additional waiver of all IP
-  version: 0.901d
+  version: 0.902d
 
   Before including the library, define S3L_PIXEL_FUNCTION to the name of the
   function you'll be using to draw single pixels (this function will be called
@@ -83,16 +83,6 @@
   vertical size (y) depends on the aspect ratio (S3L_RESOLUTION_X and
   S3L_RESOLUTION_Y). Camera FOV is defined by focal length in S3L_Units.
 
-           y ^
-             |  _
-             |  /| z
-         ____|_/__
-        |    |/   |
-     -----[0,0,0]-|-----> x
-        |____|____|
-             |    
-             |
-
   Rotations use Euler angles and are generally in the extrinsic Euler angles in
   ZXY order (by Z, then by X, then by Y). Positive rotation about an axis
   rotates CW (clock-wise) when looking in the direction of the axis.
@@ -180,6 +170,14 @@
   #define S3L_USE_WIDER_TYPES 0
 #endif
 
+#ifndef S3L_SIN_METHOD
+  /** Says which method should be used for computing sin/cos functions, possible
+  values: 0 (lookup table, takes more program memory), 1 (Bhaskara's
+  approximation, slower). This may cause the trigonometric functions give
+  slightly different results. */
+  #define S3L_SIN_METHOD 0
+#endif
+
 /** Units of measurement in 3D space. There is S3L_FRACTIONS_PER_UNIT in one
 spatial unit. By dividing the unit into fractions we effectively achieve a
 fixed point arithmetic. The number of fractions is a constant that serves as
@@ -907,6 +905,7 @@ static inline int8_t S3L_stencilTest(
 
 #define S3L_SIN_TABLE_LENGTH 128
 
+#if S3L_SIN_METHOD == 0
 static const S3L_Unit S3L_sinTable[S3L_SIN_TABLE_LENGTH] =
 {
   /* 511 was chosen here as a highest number that doesn't overflow during
@@ -977,6 +976,7 @@ static const S3L_Unit S3L_sinTable[S3L_SIN_TABLE_LENGTH] =
   (510*S3L_FRACTIONS_PER_UNIT)/511, (510*S3L_FRACTIONS_PER_UNIT)/511, 
   (510*S3L_FRACTIONS_PER_UNIT)/511, (510*S3L_FRACTIONS_PER_UNIT)/511
 };
+#endif
 
 #define S3L_SIN_TABLE_UNIT_STEP\
   (S3L_FRACTIONS_PER_UNIT / (S3L_SIN_TABLE_LENGTH * 4))
@@ -1349,6 +1349,7 @@ void S3L_mat4Xmat4(S3L_Mat4 m1, S3L_Mat4 m2)
 
 S3L_Unit S3L_sin(S3L_Unit x)
 {
+#if S3L_SIN_METHOD == 0
   x = S3L_wrap(x / S3L_SIN_TABLE_UNIT_STEP,S3L_SIN_TABLE_LENGTH * 4);
   int8_t positive = 1;
 
@@ -1371,10 +1372,37 @@ S3L_Unit S3L_sin(S3L_Unit x)
   }
 
   return positive ? S3L_sinTable[x] : -1 * S3L_sinTable[x];
+#else
+  int8_t sign = 1;
+    
+  if (x < 0) // odd function
+  {
+    x *= -1;
+    sign = -1;
+  }
+    
+  x %= S3L_FRACTIONS_PER_UNIT;
+  
+  if (x > S3L_FRACTIONS_PER_UNIT / 2)
+  {
+    x -= S3L_FRACTIONS_PER_UNIT / 2;
+    sign *= -1;
+  }
+
+  S3L_Unit tmp = S3L_FRACTIONS_PER_UNIT - 2 * x;
+ 
+  #define _PI2 ((S3L_Unit) (9.8696044 * S3L_FRACTIONS_PER_UNIT))
+  return sign * // Bhaskara's approximation
+    (((32 * x * _PI2) / S3L_FRACTIONS_PER_UNIT) * tmp) / 
+    ((_PI2 * (5 * S3L_FRACTIONS_PER_UNIT - (8 * x * tmp) / 
+      S3L_FRACTIONS_PER_UNIT)) / S3L_FRACTIONS_PER_UNIT);
+  #undef _PI2
+#endif
 }
 
 S3L_Unit S3L_asin(S3L_Unit x)
 {
+#if S3L_SIN_METHOD == 0
   x = S3L_clamp(x,-S3L_FRACTIONS_PER_UNIT,S3L_FRACTIONS_PER_UNIT);
 
   int8_t sign = 1;
@@ -1385,9 +1413,7 @@ S3L_Unit S3L_asin(S3L_Unit x)
     x *= -1;
   }
 
-  int16_t low = 0;
-  int16_t high = S3L_SIN_TABLE_LENGTH -1;
-  int16_t middle;
+  int16_t low = 0, high = S3L_SIN_TABLE_LENGTH -1, middle;
 
   while (low <= high) // binary search
   {
@@ -1406,6 +1432,27 @@ S3L_Unit S3L_asin(S3L_Unit x)
   middle *= S3L_SIN_TABLE_UNIT_STEP;
 
   return sign * middle;
+#else
+  S3L_Unit low = -1 * S3L_FRACTIONS_PER_UNIT / 4,
+           high = S3L_FRACTIONS_PER_UNIT / 4,
+           middle;
+    
+  while (low <= high) // binary search
+  {
+    middle = (low + high) / 2;
+
+    S3L_Unit v = S3L_sin(middle);
+
+    if (v > x)
+      high = middle - 1;
+    else if (v < x)
+      low = middle + 1;
+    else
+      break;
+  }
+
+  return middle;
+#endif
 }
 
 S3L_Unit S3L_cos(S3L_Unit x)
@@ -2244,7 +2291,6 @@ void S3L_drawTriangle(
 #endif
 
       // draw the row -- inner loop:
-
       for (S3L_ScreenCoord x = lXClipped; x < rXClipped; ++x)
       {
         int8_t testsPassed = 1;
@@ -2376,35 +2422,33 @@ void S3L_drawTriangle(
 #endif
 
 #if S3L_NEAR_CROSS_STRATEGY == 3
-
-if (_S3L_projectedTriangleState != 0)
-{
-  S3L_Unit newBarycentric[3];
-
-  newBarycentric[0] = S3L_interpolateBarycentric(
-    _S3L_triangleRemapBarycentrics[0].x,
-    _S3L_triangleRemapBarycentrics[1].x,
-    _S3L_triangleRemapBarycentrics[2].x,
-    p.barycentric); 
-
-  newBarycentric[1] = S3L_interpolateBarycentric(
-    _S3L_triangleRemapBarycentrics[0].y,
-    _S3L_triangleRemapBarycentrics[1].y,
-    _S3L_triangleRemapBarycentrics[2].y,
-    p.barycentric); 
-
-  newBarycentric[2] = S3L_interpolateBarycentric(
-    _S3L_triangleRemapBarycentrics[0].z,
-    _S3L_triangleRemapBarycentrics[1].z,
-    _S3L_triangleRemapBarycentrics[2].z,
-    p.barycentric); 
-
-  p.barycentric[0] = newBarycentric[0];
-  p.barycentric[1] = newBarycentric[1];
-  p.barycentric[2] = newBarycentric[2];
-}
+          if (_S3L_projectedTriangleState != 0)
+          {
+            S3L_Unit newBarycentric[3];
+
+            newBarycentric[0] = S3L_interpolateBarycentric(
+              _S3L_triangleRemapBarycentrics[0].x,
+              _S3L_triangleRemapBarycentrics[1].x,
+              _S3L_triangleRemapBarycentrics[2].x,
+              p.barycentric); 
+
+            newBarycentric[1] = S3L_interpolateBarycentric(
+              _S3L_triangleRemapBarycentrics[0].y,
+              _S3L_triangleRemapBarycentrics[1].y,
+              _S3L_triangleRemapBarycentrics[2].y,
+              p.barycentric); 
+
+            newBarycentric[2] = S3L_interpolateBarycentric(
+              _S3L_triangleRemapBarycentrics[0].z,
+              _S3L_triangleRemapBarycentrics[1].z,
+              _S3L_triangleRemapBarycentrics[2].z,
+              p.barycentric); 
+
+            p.barycentric[0] = newBarycentric[0];
+            p.barycentric[1] = newBarycentric[1];
+            p.barycentric[2] = newBarycentric[2];
+          }
 #endif
-
           S3L_PIXEL_FUNCTION(&p);
         } // tests passed
 
@@ -2727,12 +2771,12 @@ void _S3L_projectTriangle(
     transformed[5] = transformed[infrontI[0]];
 
 #if S3L_NEAR_CROSS_STRATEGY == 3
-  _S3L_triangleRemapBarycentrics[3] =
-    _S3L_triangleRemapBarycentrics[behindI[1]];
-  _S3L_triangleRemapBarycentrics[4] =
-    _S3L_triangleRemapBarycentrics[infrontI[0]];
-  _S3L_triangleRemapBarycentrics[5] =
-    _S3L_triangleRemapBarycentrics[infrontI[0]];
+    _S3L_triangleRemapBarycentrics[3] =
+      _S3L_triangleRemapBarycentrics[behindI[1]];
+    _S3L_triangleRemapBarycentrics[4] =
+      _S3L_triangleRemapBarycentrics[infrontI[0]];
+    _S3L_triangleRemapBarycentrics[5] =
+      _S3L_triangleRemapBarycentrics[infrontI[0]];
 #endif
 
     for (uint8_t i = 0; i < 2; ++i)

tinyphysicsengine.h

@@ -225,6 +225,12 @@ void TPE_worldInit(TPE_World *world,
   TPE_Body *bodies, uint16_t bodyCount,
   TPE_ClosestPointFunction environmentFunction);
 
+/** Gets orientation (rotation) of a body from a position of three of its
+  joints. The vector from joint1 to joint2 is considered the body's forward
+  direction, the vector from joint1 to joint3 its right direction. */
+TPE_Vec3 TPE_bodyGetOrientation(const TPE_Body *body, uint16_t joint1, 
+  uint16_t joint2, uint16_t joint3);
+
 void TPE_vec3Normalize(TPE_Vec3 *v);
 
 TPE_Vec3 TPE_pointRotate(TPE_Vec3 point, TPE_Vec3 rotation);
@@ -328,7 +334,6 @@ TPE_Vec3 TPE_envBox(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 maxCornerVec,
 TPE_Vec3 TPE_envSphere(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit radius);
 TPE_Vec3 TPE_envHalfPlane(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 normal);
 
-
 #define TPE_ENV_START(test,point) TPE_Vec3 _pBest = test, _pTest; \
   TPE_Unit _dBest = TPE_DISTANCE(_pBest,point), _dTest;
 
@@ -962,6 +967,18 @@ void TPE_vec3Normalize(TPE_Vec3 *v)
   }
 }
 
+TPE_Vec3 TPE_bodyGetOrientation(const TPE_Body *body, uint16_t joint1, 
+  uint16_t joint2, uint16_t joint3)
+{
+  return TPE_orientationFromVecs(
+    TPE_vec3Minus(
+      body->joints[joint2].position,
+      body->joints[joint1].position),
+    TPE_vec3Minus(
+      body->joints[joint3].position,
+      body->joints[joint1].position));
+}
+
 TPE_Vec3 TPE_bodyGetCenter(const TPE_Body *body)
 {
 // TODO: take into account possibly different joint sizes? does it even matter?