Start apply velocity

test_sdl.c

@@ -53,33 +53,22 @@ TPE_bodyInit(&body);
 S3L_Mat4 m;
 cubeModel.customTransformMatrix = &m;
 
-TPE_Vec4 axis;
 
-TPE_vec4Set(&axis,512,0,0,0);
 
-TPE_bodySetRotation(&body,axis,5);
+TPE_Vec4 p, v;
+
+TPE_vec4Set(&p,512,512,0,0);
+TPE_vec4Set(&v,10,0,0,0);
+
+TPE_bodyApplyVelocity(&body,p,v);
+
+//TPE_bodySetRotation(&body,axis,5);
  
   TPE_Unit frame = 0;
 
   while (running)
   {
 
-TPE_Vec4 q;
-TPE_bodyGetOrientation(&body,&q);
-
-if (frame == 60)
-{
-
-TPE_vec4Set(&axis,0,512,0,0);
-TPE_bodySetRotation(&body,axis,3);
-  
-}
-else if (frame == 150)
-{
-TPE_vec4Set(&axis,512,512,512,0);
-TPE_bodySetRotation(&body,axis,6);
-}
-
 /*
 TPE_PRINTF_VEC4(body.rotation.originalOrientation);
 TPE_PRINTF_VEC4(body.rotation.axisVelocity);

tinyphysicsengine.h

@@ -93,6 +93,7 @@ void TPE_vec4Multiply(TPE_Vec4 v, TPE_Unit f, TPE_Vec4 *result);
 TPE_Unit TPE_vec3Len(TPE_Vec4 v);
 TPE_Unit TPE_vec4Len(TPE_Vec4 v);
 TPE_Unit TPE_vec3DotProduct(TPE_Vec4 v1, TPE_Vec4 v2);
+void TPE_vec3CrossProduct(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result);
 void TPE_vec3Normalize(TPE_Vec4 *v);
 void TPE_vec4Normalize(TPE_Vec4 *v);
 void TPE_vec3Project(TPE_Vec4 v, TPE_Vec4 base, TPE_Vec4 *result);
@@ -410,14 +411,31 @@ void TPE_bodyGetOrientation(const TPE_Body *body, TPE_Vec4 *quaternion)
   TPE_vec4Normalize(quaternion);
 }
 
+void TPE_vec3CrossProduct(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result)
+{
+  TPE_Vec4 r;
+
+  r.x = a.y * b.z - a.z * b.y;
+  r.y = a.z * b.x - a.x * b.z;
+  r.z = a.x * b.y - a.y * b.x;
+
+  *result = r;
+}
+
 void TPE_bodyApplyVelocity(TPE_Body *body, TPE_Vec4 point, TPE_Vec4 velocity)
 {  
-  TPE_Vec4 linearVelocity;
+  TPE_Vec4 linearVelocity, angularVelocity, rotationAxis;
+
+  TPE_vec3Normalize(&point);
 
   TPE_vec3Project(velocity,point,&linearVelocity);
 
+  TPE_vec3Substract(velocity,linearVelocity,&angularVelocity);
+
   TPE_vec3Add(body->velocity,linearVelocity,&(body->velocity));
 
+  TPE_vec3CrossProduct(point,angularVelocity,&rotationAxis);
+
   // TODO: rotation
 }