Add joint friction

main.c

@@ -29,6 +29,8 @@ uint8_t red = 100;
 
 TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit distLim)
 {
+
+
   TPE_Vec3 r, rMin = p;
   TPE_Unit d, dMin = 10000000;
 
@@ -36,6 +38,10 @@ TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit distLim)
   r = c; if (p.x == r.x && p.y == r.y && p.z == r.z) return p; \
   d = TPE_DISTANCE(p,r); if (d < dMin) { dMin = d; rMin = r; }
 
+/*
+testShape( TPE_envHalfPlane(p,TPE_vec3(-1000,0,0),TPE_vec3(20,20,0)) )
+testShape( TPE_envHalfPlane(p,TPE_vec3(+1000,0,0),TPE_vec3(-20,20,0)) )
+*/
   testShape( TPE_envAABoxInside(p,TPE_vec3(0,0,0),TPE_vec3(16000,6000,16000)) )
   testShape( TPE_envSphere(p,TPE_vec3(2000,-3500,2000),2000) )
   testShape( TPE_envSphere(p,TPE_vec3(-2000,-3500,0),2000) )
@@ -321,7 +327,7 @@ switch (1)
     break;
 
   case 1:
-    TPE_makeBox(joints,connections,1000,1000,1000,100);
+    TPE_makeBox(joints,connections,1000,1000,1000,500);
     TPE_bodyInit(bodies,joints,8,connections,16,MASS);
     break;
 
@@ -342,26 +348,43 @@ switch (1)
     TPE_bodyInit(bodies,joints,3,connections,3,MASS);
     break;
 
+  case 5:
+    TPE_make2Line(joints,connections,1500,200);
+    TPE_bodyInit(bodies,joints,1,connections,0,MASS);
+    break;
+
   default: break;
 }
 
-//TPE_makeBox(joints + 20,connections + 20,300,128);
+//TPE_make2Line(joints + 20,connections + 20,1000,200);
+//TPE_bodyInit(bodies + 1,joints + 20,1,connections + 20,0,MASS);
 
-//TPE_bodyInit(bodies,joints,1,connections,0,100);
+TPE_makeBox(joints + 20,connections + 20,1000,1000,1000,500);
+TPE_bodyInit(bodies + 1,joints + 20,8,connections + 20,16,100);
 
 //bodies[0].flags |= TPE_BODY_FLAG_SOFT;
 
-bodies[0].elasticity = 256;
+//bodies[0].elasticity = 256;
 
 
-TPE_worldInit(&world,bodies,1,environmentDistance);
 
-TPE_bodyMove(world.bodies,TPE_vec3(-800,1000,0));
-//TPE_bodyMove(&world.bodies[1],TPE_vec3(400,100,1));
+
+TPE_worldInit(&world,bodies,2,environmentDistance);
+
+#define FR 256
+world.bodies[0].friction = FR;
+world.bodies[1].friction = FR;
+
+TPE_bodyMove(world.bodies,TPE_vec3(500,-200,400));
+TPE_bodyMove(&world.bodies[1],TPE_vec3(-500,800,400));
+
 
 TPE_bodyStop(world.bodies);
 TPE_bodyStop(world.bodies + 1);
 
+TPE_bodyAccelerate(world.bodies,TPE_vec3(-50,0,0));
+TPE_bodyAccelerate(world.bodies + 1,TPE_vec3(50,0,0));
+
 //TPE_bodyRotate(world.bodies,TPE_vec3(0,0,200));
 
 
@@ -389,9 +412,8 @@ for (int i = 0; i < world.bodyCount; ++i)
 {
 
 
-
-TPE_bodyAccelerate(world.bodies + i,
-TPE_vec3(0,-6,0));
+if (!(world.bodies[i].flags & TPE_BODY_FLAG_DEACTIVATED))
+  TPE_bodyAccelerate(world.bodies + i,TPE_vec3(0,-6,0));
 
 }
 
@@ -471,7 +493,7 @@ TPE_Vec3 camRot = TPE_vec3(
 
 sphereScene.models = &cube;
 S3L_newFrame();
-S3L_drawScene(sphereScene);
+//S3L_drawScene(sphereScene);
 sphereScene.models = &sphereModel;
 
 TPE_worldDebugDraw(&world,debugDrawPixel,
@@ -526,15 +548,17 @@ if (state[SDL_SCANCODE_M])
    30));
 }
 
+#define S 10
 if (state[SDL_SCANCODE_J])
-  TPE_bodyAccelerate(bodies,TPE_vec3(10,0,0));
+  TPE_bodyAccelerate(bodies,TPE_vec3(S,0,0));
 else if (state[SDL_SCANCODE_G])
-  TPE_bodyAccelerate(bodies,TPE_vec3(-10,0,0));
+  TPE_bodyAccelerate(bodies,TPE_vec3(-S,0,0));
 
 if (state[SDL_SCANCODE_Y])
-  TPE_bodyAccelerate(bodies,TPE_vec3(0,0,10));
+  TPE_bodyAccelerate(bodies,TPE_vec3(0,0,S));
 else if (state[SDL_SCANCODE_H])
-  TPE_bodyAccelerate(bodies,TPE_vec3(0,0,-10));
+  TPE_bodyAccelerate(bodies,TPE_vec3(0,0,-S));
+#undef S
 
 #define SHIFT_STEP 50
 

tinyphysicsengine.h

@@ -115,7 +115,6 @@ of 2. */
   #define TPE_COLLISION_RESOLUTION_MARGIN (TPE_FRACTIONS_PER_UNIT / 64)
 #endif
 
-
 #define TPE_PRINTF_VEC3(v) printf("[%d %d %d]",(v).x,(v).y,(v).z);
 
 typedef struct
@@ -128,7 +127,7 @@ typedef struct
 typedef struct
 {
   TPE_Vec3 position;
-  int16_t velocity[3]; ///< for saving space only uses int16
+  TPE_UnitReduced velocity[3];
   uint8_t sizeDivided; /**< size (radius, ...), for saving space divided by 
                             TPE_JOINT_SIZE_MULTIPLIER */
 } TPE_Joint;
@@ -231,7 +230,7 @@ static inline TPE_Unit TPE_distApprox(TPE_Vec3 p1, TPE_Vec3 p2);
 TPE_Joint TPE_joint(TPE_Vec3 position, TPE_Unit size);
 
 uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2,
-  TPE_Unit mass1, TPE_Unit mass2, TPE_Unit elasticity);
+  TPE_Unit mass1, TPE_Unit mass2, TPE_Unit elasticity, TPE_Unit friction);
 
 uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, TPE_Unit elasticity,
   TPE_Unit friction, TPE_ClosestPointFunction env);
@@ -239,7 +238,7 @@ uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, TPE_Unit elastici
 uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body, 
   TPE_ClosestPointFunction env);
 
-void TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2);
+uint8_t TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2);
 
 // -----------------------------------------------------------------------------
 // body generation functions:
@@ -267,6 +266,7 @@ void TPE_make2Line(TPE_Joint joints[2], TPE_Connection connections[1],
 
 TPE_Vec3 TPE_envAABoxInside(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 size);
 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);
 
 //---------------------------
 
@@ -585,7 +585,17 @@ void TPE_worldStep(TPE_World *world)
     }
 
     TPE_Connection *connection = body->connections;
-  
+ 
+for (uint16_t j = i + 1; j < world->bodyCount; ++j)
+{
+if (TPE_bodiesResolveCollision(body,world->bodies + j))
+{
+  TPE_bodyWake(body);
+  TPE_bodyWake(world->bodies + j);
+}
+
+}
+ 
     TPE_bodyEnvironmentResolveCollision(body,
       world->environmentFunction);
 
@@ -635,7 +645,7 @@ void TPE_worldStep(TPE_World *world)
       connection++;
     }
 
-    if (!(body->flags & TPE_BODY_FLAG_SOFT))
+    if (body->connectionCount > 0 && !(body->flags & TPE_BODY_FLAG_SOFT))
     {
       TPE_bodyReshape(body,world->environmentFunction);
 
@@ -661,8 +671,13 @@ void TPE_worldStep(TPE_World *world)
 
 void TPE_bodyWake(TPE_Body *body)
 {
-  TPE_bodyStop(body);
-  body->flags &= ~TPE_BODY_FLAG_DEACTIVATED;
+  // the if check has to be here, don't remove it
+
+  if (body->flags & TPE_BODY_FLAG_DISABLED)
+  {
+    TPE_bodyStop(body);
+    body->flags &= ~TPE_BODY_FLAG_DEACTIVATED;
+  }
 }
 
 TPE_Unit TPE_bodyNetSpeed(const TPE_Body *body)
@@ -912,6 +927,8 @@ void TPE_bodyMove(TPE_Body *body, TPE_Vec3 offset)
 
 void TPE_bodyAccelerate(TPE_Body *body, TPE_Vec3 velocity)
 {
+  TPE_bodyWake(body);
+
   for (uint16_t i = 0; i < body->jointCount; ++i)
   {
     body->joints[i].velocity[0] += velocity.x;
@@ -968,25 +985,29 @@ TPE_Unit TPE_sin(TPE_Unit x)
   #undef _PI2
 }
 
-void TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2)
+uint8_t TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2)
 {
 // TODO: bounding sphere (or AABB? maybe ifdef)
+  uint8_t r = 0;
 
   for (uint16_t i = 0; i < b1->jointCount; ++i)
     for (uint16_t j = 0; j < b2->jointCount; ++j)
     {
-      TPE_jointsResolveCollision( 
+      r |= TPE_jointsResolveCollision( 
 &(b1->joints[i]),
 &(b2->joints[j]),
 b1->jointMass,
 b2->jointMass,
-512
+512,
+(b1->friction + b2->friction) / 2
  );
     }
+
+  return r;
 }
 
 uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2,
-  TPE_Unit mass1, TPE_Unit mass2, TPE_Unit elasticity)
+  TPE_Unit mass1, TPE_Unit mass2, TPE_Unit elasticity, TPE_Unit friction)
 {
   TPE_Vec3 dir = TPE_vec3Minus(j2->position,j1->position);
 
@@ -996,7 +1017,7 @@ uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2,
   {
     // separate bodies, the shift distance will depend on the weight ratio:
 
-    d *= -1;
+    d = -1 * d + TPE_COLLISION_RESOLUTION_MARGIN;
 
     TPE_vec3Normalize(&dir);
 
@@ -1027,6 +1048,15 @@ uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2,
     j1->velocity[1] = j1->velocity[1] - vel.y;
     j1->velocity[2] = j1->velocity[2] - vel.z;
 
+    /* friction explanation: Not physically correct (doesn't depend on load), 
+    friction basically means we weighted average the velocities of the bodies
+    in the direction perpendicular to the hit normal, in the ratio of their
+    masses, friction coefficient just says how much of this effect we apply
+    (it multiplies the friction vectors we are subtracting) */
+
+    TPE_Vec3 frictionVec =
+      TPE_vec3(j1->velocity[0],j1->velocity[1],j1->velocity[2]);
+
     v1 = TPE_vec3Dot(vel,dir);
 
     vel = TPE_vec3(j2->velocity[0],j2->velocity[1],j2->velocity[2]);
@@ -1037,21 +1067,33 @@ uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2,
     j2->velocity[1] = j2->velocity[1] - vel.y;
     j2->velocity[2] = j2->velocity[2] - vel.z;
 
+    frictionVec = TPE_vec3Minus(
+      TPE_vec3(j2->velocity[0],j2->velocity[1],j2->velocity[2]),
+      frictionVec);
+
     v2 = TPE_vec3Dot(vel,dir);
 
     TPE_getVelocitiesAfterCollision(&v1,&v2,mass1,mass2,elasticity);
 
     vel = TPE_vec3Times(dir,v1);
 
-    j1->velocity[0] = j1->velocity[0] + vel.x;
-    j1->velocity[1] = j1->velocity[1] + vel.y;
-    j1->velocity[2] = j1->velocity[2] + vel.z;
+#define assignVec(j,i,d,o) \
+  j->velocity[i] = j->velocity[i] + vel.d o (((frictionVec.d * ratio) / \
+    TPE_FRACTIONS_PER_UNIT) * friction) / TPE_FRACTIONS_PER_UNIT;
+
+    assignVec(j1,0,x,+)
+    assignVec(j1,1,y,+)
+    assignVec(j1,2,z,+)
 
     vel = TPE_vec3Times(dir,v2);
 
-    j2->velocity[0] = j2->velocity[0] + vel.x;
-    j2->velocity[1] = j2->velocity[1] + vel.y;
-    j2->velocity[2] = j2->velocity[2] + vel.z;
+    ratio = TPE_FRACTIONS_PER_UNIT - ratio;
+
+    assignVec(j2,0,x,-)
+    assignVec(j2,1,y,-)
+    assignVec(j2,2,z,-)
+
+#undef assignVec
 
     return 1;
   }
@@ -1535,4 +1577,25 @@ TPE_Vec3 TPE_envSphere(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit radius)
   return TPE_vec3Plus(center,dir);
 }
 
+TPE_Vec3 TPE_envHalfPlane(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 normal)
+{
+  TPE_Vec3 point2 = TPE_vec3Minus(point,center);
+
+  TPE_Unit tmp = point2.x * normal.x + point2.y * normal.y + point2.z * normal.z;
+
+  if (tmp < 0)
+    return point;
+
+  TPE_Unit l = TPE_LENGTH(normal);
+
+  tmp /= l;
+
+  normal.x = (normal.x * TPE_FRACTIONS_PER_UNIT) / l;
+  normal.y = (normal.y * TPE_FRACTIONS_PER_UNIT) / l;
+  normal.z = (normal.z * TPE_FRACTIONS_PER_UNIT) / l;
+
+  return TPE_vec3Minus(point,
+    TPE_vec3Times(normal,tmp));
+}
+
 #endif // guard