Add fake rotations

TODO.txt

@@ -1,6 +1,5 @@
 TODO:
 - zero weight joints should behave how?
-- collision callbacks, ability to custom handle collision response
 - debug draw inactive bodies with different color?
 - test different tick lengths
 - air friction
@@ -12,8 +11,12 @@ TODO:
   rotate by the axis perpendicular to its linear velocity vec
 - BUG? makeCenterBox doesnt seem to add the center joint: check it out
 - demo: momentum conservation
+- bounding box/sphere test functions/macros for optimization of environment
+  building
+- unify naming: rotation vs orientation
 
 DONE:
+- collision callbacks, ability to custom handle collision response
 - zero sized joints should never collide (can be useful) <- NO because 0 size
   can also mean just small size (due to size being stored divided)
 - when waking a body by collision, maybe set the disable count to lower values,

programs/cubes.c

@@ -17,6 +17,7 @@ TPE_Connection ballConnections[3];
 void updateOrientPos(int i)
 {
   cubeOrientations[i] = TPE_bodyGetOrientation(&tpe_world.bodies[i],0,2,1);
+  cubePositions[i] = TPE_bodyGetCenter(&tpe_world.bodies[i]);
 }
 
 int main(void)

programs/helper.h

@@ -218,7 +218,72 @@ const S3L_Index sphereTriangleIndices[SPHERE_TRIANGLE_COUNT * 3] = {
      13,     1,    14         // 237
 }; // sphereTriangleIndices
 
+#define CYLINDER_VERTEX_COUNT 20
+const S3L_Unit cylinderVertices[CYLINDER_VERTEX_COUNT * 3] = {
+      0,  -256,   512,        // 0
+      0,   256,   512,        // 3
+    300,  -256,   414,        // 6
+    300,   256,   414,        // 9
+    486,  -256,   158,        // 12
+    486,   256,   158,        // 15
+    486,  -256,  -158,        // 18
+    486,   256,  -158,        // 21
+    300,  -256,  -414,        // 24
+    300,   256,  -414,        // 27
+      0,  -256,  -512,        // 30
+      0,   256,  -512,        // 33
+   -300,  -256,  -414,        // 36
+   -300,   256,  -414,        // 39
+   -486,  -256,  -158,        // 42
+   -486,   256,  -158,        // 45
+   -486,  -256,   158,        // 48
+   -486,   256,   158,        // 51
+   -300,  -256,   414,        // 54
+   -300,   256,   414         // 57
+}; // cylinderVertices
+
+#define CYLINDER_TRIANGLE_COUNT 36
+const S3L_Index cylinderTriangleIndices[CYLINDER_TRIANGLE_COUNT * 3] = {
+      1,     2,     0,        // 0
+      3,     4,     2,        // 3
+      5,     6,     4,        // 6
+      7,     8,     6,        // 9
+      9,    10,     8,        // 12
+     11,    12,    10,        // 15
+     13,    14,    12,        // 18
+     15,    16,    14,        // 21
+     17,     7,     5,        // 24
+     17,    18,    16,        // 27
+     19,     0,    18,        // 30
+      6,    14,    18,        // 33
+      1,     3,     2,        // 36
+      3,     5,     4,        // 39
+      5,     7,     6,        // 42
+      7,     9,     8,        // 45
+      9,    11,    10,        // 48
+     11,    13,    12,        // 51
+     13,    15,    14,        // 54
+     15,    17,    16,        // 57
+      5,     3,    17,        // 60
+      1,    19,    17,        // 63
+     17,    15,    13,        // 66
+     13,    11,    17,        // 69
+      9,     7,    17,        // 72
+      3,     1,    17,        // 75
+     17,    11,     9,        // 78
+     17,    19,    18,        // 81
+     19,     1,     0,        // 84
+     18,     0,     2,        // 87
+      2,     4,     6,        // 90
+      6,     8,    10,        // 93
+     10,    12,    14,        // 96
+     14,    16,    18,        // 99
+     18,     2,     6,        // 102
+      6,    10,    14         // 105
+}; // cylinderTriangleIndices
+
 S3L_Model3D sphereModel;
+S3L_Model3D cylinderModel;
 
 #define MAX_BODIES 128
 #define MAX_JOINTS 1024
@@ -553,6 +618,11 @@ void helper_draw3dCube(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
   helper_drawModel(&cubeModel,pos,scale,rot);
 }
 
+void helper_draw3dCylinder(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
+{
+  helper_drawModel(&cylinderModel,pos,scale,rot);
+}
+
 void helper_draw3dCubeInside(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
 {
   cubeModel.config.backfaceCulling = 1;
@@ -591,6 +661,9 @@ void helper_init(void)
   S3L_model3DInit(sphereVertices,SPHERE_VERTEX_COUNT,sphereTriangleIndices,
     SPHERE_TRIANGLE_COUNT,&sphereModel);
 
+  S3L_model3DInit(cylinderVertices,CYLINDER_VERTEX_COUNT,
+    cylinderTriangleIndices,CYLINDER_TRIANGLE_COUNT,&cylinderModel);
+
   S3L_model3DInit(triangleVertices,3,triangleTriangles,2,&triangleModel);
 
   S3L_sceneInit(0,1,&s3l_scene);

programs/player.c

@@ -1,4 +1,4 @@
-#define SCALE_3D_RENDERING 8
+#define SCALE_3D_RENDERING 1
 
 #include "helper.h"
 
@@ -12,6 +12,7 @@ TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit maxD)
   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_NEXT( TPE_envInfiniteCylinder(p,TPE_vec3(2000,0,-1100),TPE_vec3(0,255,0),400),p )
   TPE_ENV_END
 }
 
@@ -19,6 +20,8 @@ int jumpCountdown = 0;
 TPE_Unit rotation = 0;
 int onGroundCount = 0;
 
+TPE_Vec3 ballRot, ballPreviousPos;
+
 TPE_Vec3 directionVec;
 
 void updateDirection(void)
@@ -38,12 +41,17 @@ uint8_t collisionCallback(uint16_t b1, uint16_t j1, uint16_t b2, uint16_t j2,
 
 int main(void)
 {
+
+
+
   helper_init();
 
   helper_debugDrawOn = 1;
 
   updateDirection();
 
+ballRot = TPE_vec3(0,0,0);
+
   tpe_world.environmentFunction = environmentDistance;
 
 tpe_world.collisionCallback = collisionCallback;
@@ -65,6 +73,8 @@ tpe_world.bodies[0].friction = 0;
 helper_addBall(1000,100);
 TPE_bodyMove(&tpe_world.bodies[1],TPE_vec3(-1000,1000,0));
 
+ballPreviousPos = tpe_world.bodies[1].joints[0].position;
+
 tpe_world.bodies[1].elasticity = 400;
 tpe_world.bodies[1].friction = 100;
  
@@ -100,6 +110,15 @@ tpe_world.bodies[2].friction = 50;
 
     TPE_worldStep(&tpe_world);
 
+TPE_Vec3 ballRoll = TPE_fakeSphereRotation
+  (ballPreviousPos,tpe_world.bodies[1].joints[0].position,1000);
+
+
+ballRot = TPE_rotationRotateByAxis(ballRot,ballRoll);
+
+ballPreviousPos = tpe_world.bodies[1].joints[0].position;
+
+
     TPE_bodyActivate(&tpe_world.bodies[0]);
 
     if (onGroundCount)
@@ -148,6 +167,9 @@ tpe_world.bodies[2].friction = 50;
     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_draw3dCylinder(
+TPE_vec3(2000,
+5000,-1100),TPE_vec3(400,10000,400),TPE_vec3(0,0,0));
 
 helper_draw3dPlane(
 TPE_vec3(0,1500,-3500),
@@ -167,7 +189,7 @@ TPE_bodyGetOrientation(&tpe_world.bodies[2],0,2,1)
 
 helper_draw3dSphere(
 tpe_world.bodies[1].joints[0].position
-,TPE_vec3(1000,1000,1000),TPE_vec3(0,0,0)  );
+,TPE_vec3(1000,1000,1000),ballRot  );
 
     helper_set3dColor(200,10,10);
 

tinyphysicsengine.h

@@ -11,7 +11,10 @@
   to make them easily integrate with each other.
 
   Orientations/rotations are in extrinsic Euler angles in the ZXY order (by Z,
-  then by X, then by Y).
+  then by X, then by Y). Angles are in TPE_Units, TPE_FRACTIONS_PER_UNIT is
+  full angle (2 PI). Sometimes rotations can also be specified in the
+  "about axis" format: here the object is rotated CW by given axis by an angle
+  that's specified by the magnitude of the vector.
 
   Where it matters (e.g. rotations about axes) we consider a left-handed coord.
   system (x right, y up, z forward).
@@ -237,6 +240,8 @@ TPE_Vec3 TPE_pointRotate(TPE_Vec3 point, TPE_Vec3 rotation);
 
 TPE_Vec3 TPE_rotationInverse(TPE_Vec3 rotation);
 
+TPE_Vec3 TPE_rotationRotateByAxis(TPE_Vec3 rotation, TPE_Vec3 rotationByAxis);
+
 void TPE_getVelocitiesAfterCollision(
   TPE_Unit *v1,
   TPE_Unit *v2,
@@ -305,6 +310,14 @@ uint8_t TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2);
 
 void TPE_jointPin(TPE_Joint *joint, TPE_Vec3 position);
 
+/** "Fakes" a rotation of a moving sphere by rotating it in the direction of
+  its movement; this can create the illusion of the sphere actually rotating
+  due to friction even if the physics sphere object (a body with a single joint)
+  isn't rotating at all. Returns a rotating in the "about axis" format (see
+  library conventions). */
+TPE_Vec3 TPE_fakeSphereRotation(TPE_Vec3 position1, TPE_Vec3 position2,
+  TPE_Unit radius);
+
 // -----------------------------------------------------------------------------
 // body generation functions:
 
@@ -335,7 +348,7 @@ TPE_Vec3 TPE_envBox(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 maxCornerVec,
   TPE_Vec3 rotation);
 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);
-TPE_Vec3 TPE_envInfiniteCyllinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3
+TPE_Vec3 TPE_envInfiniteCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3
   direction, TPE_Unit radius);
 
 #define TPE_ENV_START(test,point) TPE_Vec3 _pBest = test, _pTest; \
@@ -391,9 +404,8 @@ tick to a point in the distance of TPE_FRACTIONS_PER_UNIT from the rotation
 axis). */
 void TPE_bodySpin(TPE_Body *body, TPE_Vec3 rotation);
 
-/** Instantly rotate soft body, the rotation vector specifies the rotation axis
-by its direction and the rotation angle by its magnitude (TPE_FRACTIONS_PER_UNIT
-stands for full angle, i.e. 2 * PI x). */
+/** Instantly rotate soft body about an axis (see library conventions for
+  the rotation format). */
 void TPE_bodyRotateByAxis(TPE_Body *body, TPE_Vec3 rotation);
 
 /** Compute the center of mass of a soft body. */
@@ -963,12 +975,23 @@ void TPE_vec3Normalize(TPE_Vec3 *v)
 {
   TPE_Unit l = TPE_LENGTH(*v);
 
-  if (l != 0)
+if (l == 0)
+  *v = TPE_vec3(TPE_FRACTIONS_PER_UNIT,0,0);
+else
+{
+  if (l < 16) // TODO: const, for too short
   {
-    v->x = (v->x * TPE_FRACTIONS_PER_UNIT) / l;
-    v->y = (v->y * TPE_FRACTIONS_PER_UNIT) / l;
-    v->z = (v->z * TPE_FRACTIONS_PER_UNIT) / l;
+v->x *= 8;
+v->y *= 8;
+v->z *= 8;
+l = TPE_LENGTH(*v);
   }
+
+  v->x = (v->x * TPE_FRACTIONS_PER_UNIT) / l;
+  v->y = (v->y * TPE_FRACTIONS_PER_UNIT) / l;
+  v->z = (v->z * TPE_FRACTIONS_PER_UNIT) / l;
+}
+
 }
 
 TPE_Vec3 TPE_bodyGetOrientation(const TPE_Body *body, uint16_t joint1, 
@@ -1025,6 +1048,22 @@ void TPE_bodySpin(TPE_Body *body, TPE_Vec3 rotation)
   }
 }
 
+TPE_Vec3 _TPE_rotateByAxis(TPE_Vec3 p, TPE_Vec3 axisNormalized, TPE_Unit angle)
+{
+  TPE_Vec3 projected = TPE_vec3ProjectNormalized(p,axisNormalized);
+
+  TPE_Vec3 a = TPE_vec3Minus(p,projected);
+
+  if (a.x == 0 && a.y == 0 && a.z == 0)
+    return p;
+
+  TPE_Vec3 b = TPE_vec3Cross(a,axisNormalized);
+
+  return TPE_vec3Plus(projected,TPE_vec3Plus(
+    TPE_vec3Times(a,TPE_cos(angle)),
+    TPE_vec3Times(b,TPE_sin(angle))));
+}
+
 void TPE_bodyRotateByAxis(TPE_Body *body, TPE_Vec3 rotation)
 {
   TPE_Vec3 bodyCenter = TPE_bodyGetCenter(body);
@@ -1034,22 +1073,9 @@ void TPE_bodyRotateByAxis(TPE_Body *body, TPE_Vec3 rotation)
 
   for (uint16_t i = 0; i < body->jointCount; ++i)
   {
-    TPE_Joint *j = body->joints + i;
-
-    TPE_Vec3 toPoint = TPE_vec3Minus(j->position,bodyCenter);
-
-    toPoint = TPE_vec3Project(toPoint,rotation);
-
-    TPE_Vec3 rotationCenter = TPE_vec3Plus(bodyCenter,toPoint);
-
-    toPoint = TPE_vec3Minus(j->position,rotationCenter);
-
-    TPE_Vec3 toPoint2 = TPE_vec3Cross(toPoint,rotation);
-
-    j->position = TPE_vec3Plus(rotationCenter,
-        TPE_vec3Plus(
-          TPE_vec3Times(toPoint,TPE_cos(angle)),
-          TPE_vec3Times(toPoint2,TPE_sin(angle))));
+    TPE_Vec3 toPoint = TPE_vec3Minus(body->joints[i].position,bodyCenter);
+    body->joints[i].position = TPE_vec3Plus(bodyCenter,
+    _TPE_rotateByAxis(toPoint,rotation,angle));
   }
 }
 
@@ -2014,6 +2040,20 @@ TPE_Vec3 TPE_rotationInverse(TPE_Vec3 rotation)
   return TPE_orientationFromVecs(f,r);
 }
 
+TPE_Vec3 TPE_rotationRotateByAxis(TPE_Vec3 rotation, TPE_Vec3 rotationByAxis)
+{
+  TPE_Vec3 f = TPE_pointRotate(TPE_vec3(0,0,TPE_FRACTIONS_PER_UNIT),rotation);
+  TPE_Vec3 r = TPE_pointRotate(TPE_vec3(TPE_FRACTIONS_PER_UNIT,0,0),rotation);
+
+  TPE_Unit a = TPE_LENGTH(rotationByAxis);
+  TPE_vec3Normalize(&rotationByAxis);
+
+  f = _TPE_rotateByAxis(f,rotationByAxis,a);
+  r = _TPE_rotateByAxis(r,rotationByAxis,a);
+
+  return TPE_orientationFromVecs(f,r);
+}
+
 TPE_Unit TPE_keepInRange(TPE_Unit x, TPE_Unit xMin, TPE_Unit xMax)
 {
   return x > xMin ? (x < xMax ? x : xMax) : xMin;
@@ -2034,7 +2074,7 @@ TPE_Vec3 TPE_vec3KeepWithinBox(TPE_Vec3 point, TPE_Vec3 boxCenter,
   return point;
 }
 
-TPE_Vec3 TPE_envInfiniteCyllinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3
+TPE_Vec3 TPE_envInfiniteCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3
   direction, TPE_Unit radius)
 {
   TPE_Vec3 d = TPE_vec3Minus(point,center);
@@ -2054,4 +2094,27 @@ TPE_Vec3 TPE_envInfiniteCyllinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3
   return TPE_vec3Minus(point,d);
 }
 
+TPE_Vec3 TPE_fakeSphereRotation(TPE_Vec3 position1, TPE_Vec3 position2,
+  TPE_Unit radius)
+{
+  TPE_Vec3 m;
+
+  m.x = position1.z - position2.z;
+  m.y = 0;
+  m.z = position2.x - position1.x;
+    
+  TPE_Unit l = TPE_sqrt(m.x * m.x + m.z * m.z);
+
+  if (l == 0)
+    return TPE_vec3(0,0,0);
+
+  TPE_Unit d = (TPE_DISTANCE(position1,position2) * 
+    TPE_FRACTIONS_PER_UNIT) / (radius * 4);
+
+  m.x = (m.x * d) / l;
+  m.z = (m.z * d) / l;
+  
+  return m;
+}
+
 #endif // guard