Fix adding velocity

test_sdl.c

@@ -368,7 +368,7 @@ bodies[1].body.mass = TPE_INFINITY;
 
 bodies[0].body.position = TPE_vec4(0,1600,0,0);
 bodies[1].body.position = TPE_vec4(0,-1000,0,0);
-bodies[0].body.velocity = TPE_vec4(0,-30,0,0);
+bodies[0].body.velocity = TPE_vec4(0,0,0,0);
 
 //TPE_bodyApplyImpulse(&(bodies[0].body),TPE_vec4(256,0,0,0),TPE_vec4(-1,-1,-1,0));
 
@@ -376,7 +376,7 @@ bodies[0].body.velocity = TPE_vec4(0,-30,0,0);
 
 //bodies[0].body.velocity = TPE_vec4(150,100,0,0);
 
-//TPE_bodySetRotation(&(bodies[0].body),TPE_vec4(100,128,10,0),10);
+//TPE_bodySetRotation(&(bodies[0].body),TPE_vec4(0,0,-512,0),1);
 //TPE_bodySetRotation(&(bodies[1].body),TPE_vec4(210,50,1,0),5);
 
 TPE_Vec4 qqq;
@@ -397,8 +397,7 @@ int collided = 0;
   while (running)
   {
 
-
-bodies[0].body.velocity.y -= 1;
+bodies[0].body.velocity.y -= 4;
 
     for (uint32_t i = 0; i < PIXELS_SIZE; ++i)
       pixels[i] = 0;
@@ -464,7 +463,7 @@ collided++;
       project3DPointToScreen(p2,scene.camera,&scr);
       draw2DPoint(scr.x,scr.y,255,0,0);
 
-      TPE_vec3Multiply(n,collDepth,&n);
+//      TPE_vec3Multiply(n,collDepth,&n);
 
 
 /*

tinyphysicsengine.h

@@ -565,6 +565,10 @@ void TPE_bodyApplyImpulse(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 impulse)
 {
   TPE_Unit pointDistance = TPE_vec3Len(point);
 
+//printf("---\n");
+//TPE_PRINTF_VEC4(impulse)
+//TPE_PRINTF_VEC4(point)
+
   if (pointDistance != 0)  
   {
     impulse.x = (impulse.x * TPE_FRACTIONS_PER_UNIT) / body->mass;
@@ -583,6 +587,7 @@ void TPE_bodyApplyImpulse(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 impulse)
     /* for simplicity we'll suppose angular momentum of a sphere: */
 
     impulse = TPE_vec3Cross(impulse,point);
+//TPE_PRINTF_VEC4(impulse)
 
     TPE_Unit r = TPE_bodyGetMaxExtent(body);
 
@@ -593,7 +598,10 @@ void TPE_bodyApplyImpulse(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 impulse)
     impulse.z = (5 * impulse.z * TPE_FRACTIONS_PER_UNIT) / r;   
 
     TPE_bodyAddRotation(body,impulse,TPE_vec3Len(impulse));
+
+//TPE_PRINTF_VEC4(body->rotation.axisVelocity);
   }
+//printf("\n");
 }
 
 void _TPE_getShapes(const TPE_Body *b1, const TPE_Body *b2, uint8_t shape1,
@@ -1385,8 +1393,11 @@ TPE_Unit TPE_angularVelocityToLinear(TPE_Unit velocity, TPE_Unit distance)
 
 void TPE_bodyStep(TPE_Body *body)
 {
-  TPE_vec3Add(body->position,body->velocity,&(body->position));
-  body->rotation.currentAngle += body->rotation.axisVelocity.w;
+  if (body->mass != TPE_INFINITY)
+  {
+    TPE_vec3Add(body->position,body->velocity,&(body->position));
+    body->rotation.currentAngle += body->rotation.axisVelocity.w;
+  }
 }
 
 void TPE_bodySetRotation(TPE_Body *body, TPE_Vec4 axis, TPE_Unit velocity)
@@ -1415,6 +1426,8 @@ void TPE_bodyAddRotation(TPE_Body *body, TPE_Vec4 axis, TPE_Unit velocity)
      magnitude is the rotation speed, then we add these vectors and convert
      the final vector back to normalized rotation axis + scalar rotation 
      speed.*/
+  if (velocity == 0)
+    return;
 
   body->rotation.axisVelocity.x = 
     (body->rotation.axisVelocity.x * body->rotation.axisVelocity.w)