Add closest ls

test.c

@@ -97,10 +97,15 @@ int main(void)
     TPE_rotationToQuaternion(TPE_vec4(512,0,0,0),F/4,&q);
     TPE_rotatePoint(&p2,q);
 
-TPE_PRINTF_VEC4(p2);
+    #define TEST_LINE_SEGMENT_CLOSE(ax,ay,az,bx,by,bz,px,py,pz,rx,ry,rz) \
+      ASS(ass("line segment closest",TPE_vec3Dist(TPE_lineSegmentClosestPoint(\
+        TPE_vec4(ax,ay,az,0),TPE_vec4(bx,by,bz,0),TPE_vec4(px,py,pz,0)),\
+        TPE_vec4(rx,ry,rz,0)) < 10))
 
-return 0;
+    TEST_LINE_SEGMENT_CLOSE(0,0,0, 100,0,0, 50,0,0, 50,0,0)
+    TEST_LINE_SEGMENT_CLOSE(-100,-100,20, 1000,10000,20000, -3000,-5000,-1000, -100,-100,20)
 
+return 0;
   }
 
   {

test_sdl.c

@@ -348,9 +348,11 @@ int main()
 
   TPE_Unit frame = 0;
 
-bodies[1].body.position.x = 600;
+bodies[0].body.position.x = 50;
+bodies[1].body.position.x = -700;
+bodies[1].body.position.z = 200;
 
-TPE_bodySetRotation( &(bodies[0].body),TPE_vec4(0,100,255,0),1 );
+TPE_bodySetRotation( &(bodies[0].body),TPE_vec4(0,128,255,0),1);
 /*
 TPE_Vec4 quat;
 TPE_rotationToQuaternion(TPE_vec4(0,0,255,0),40,&quat);
@@ -381,6 +383,17 @@ TPE_bodySetOrientation(&(bodies[0].body),quat);
         &scr);
 
       draw2DPoint(scr.x,scr.y,255,0,0);
+
+      p2.x += n.x / 2;
+      p2.y += n.y / 2;
+      p2.z += n.z / 2;
+
+      project3DPointToScreen(
+        p2,
+        scene.camera,
+        &scr);
+
+      draw2DPoint(scr.x,scr.y,255,255,255);
     }
 
     SDL_UpdateTexture(textureSDL,NULL,pixels,S3L_RESOLUTION_X * sizeof(uint32_t));
@@ -402,6 +415,8 @@ TPE_bodySetOrientation(&(bodies[0].body),quat);
  
     S3L_rotationToDirections(scene.camera.transform.rotation,20,&camF,&camR,0);
 
+#define SHIFT_STEP 5
+
     if (state[SDL_SCANCODE_LSHIFT])
     {
       if (state[SDL_SCANCODE_UP])
@@ -416,19 +431,34 @@ TPE_bodySetOrientation(&(bodies[0].body),quat);
     else
     {
       if (state[SDL_SCANCODE_UP])
-        scene.camera.transform.rotation.x += 2;
+        scene.camera.transform.rotation.x += SHIFT_STEP;
       else if (state[SDL_SCANCODE_DOWN])
-        scene.camera.transform.rotation.x -= 2;
+        scene.camera.transform.rotation.x -= SHIFT_STEP;
       else if (state[SDL_SCANCODE_LEFT])
-        scene.camera.transform.rotation.y += 2;
+        scene.camera.transform.rotation.y += SHIFT_STEP;
       else if (state[SDL_SCANCODE_RIGHT])
-        scene.camera.transform.rotation.y -= 2;
+        scene.camera.transform.rotation.y -= SHIFT_STEP;
     }
 
+    if (state[SDL_SCANCODE_L])
+      bodies[1].body.position.x += SHIFT_STEP;
+    else if (state[SDL_SCANCODE_J])
+      bodies[1].body.position.x -= SHIFT_STEP;
+    else if (state[SDL_SCANCODE_I])
+      bodies[1].body.position.z += SHIFT_STEP;
+    else if (state[SDL_SCANCODE_K])
+      bodies[1].body.position.z -= SHIFT_STEP;
+    else if (state[SDL_SCANCODE_N])
+      bodies[1].body.position.y += SHIFT_STEP;
+    else if (state[SDL_SCANCODE_M])
+      bodies[1].body.position.y -= SHIFT_STEP;
+  
     if (state[SDL_SCANCODE_P])
-      scene.camera.transform.translation.y += 20;
+      scene.camera.transform.translation.y += SHIFT_STEP;
     else if (state[SDL_SCANCODE_O])
-      scene.camera.transform.translation.y -= 20;
+      scene.camera.transform.translation.y -= SHIFT_STEP;
+
+#undef SHIFT_STEP
 
     SDL_RenderClear(renderer);
     SDL_RenderCopy(renderer,textureSDL,NULL,NULL);

tinyphysicsengine.h

@@ -54,10 +54,11 @@ typedef int32_t TPE_Unit;
 
 #define TPE_SHAPE_POINT     0    ///< single point in space
 #define TPE_SHAPE_SPHERE    1    ///< sphere, params.: radius
-#define TPE_SHAPE_CUBOID    2    ///< cuboid, params.: width, height, depth
-#define TPE_SHAPE_PLANE     3    ///< plane, params.: width, depth
-#define TPE_SHAPE_CYLINDER  4    ///< cylinder, params.: radius, height
-#define TPE_SHAPE_TRIMESH   5    /**< triangle mesh, params.:
+#define TPE_SHAPE_CAPSULE   2    ///< capsule: radius, height
+#define TPE_SHAPE_CUBOID    3    ///< cuboid, params.: width, height, depth
+#define TPE_SHAPE_PLANE     4    ///< plane, params.: width, depth
+#define TPE_SHAPE_CYLINDER  5    ///< cylinder, params.: radius, height
+#define TPE_SHAPE_TRIMESH   6    /**< triangle mesh, params.:
                                         vertex count,
                                         triangle count
                                         vertices (int32_t pointer),
@@ -125,6 +126,9 @@ TPE_Vec4 TPE_vec3Cross(TPE_Vec4 a, TPE_Vec4 b);
 static inline TPE_Vec4 TPE_vec3Normalized(TPE_Vec4 v);
 static inline TPE_Vec4 TPE_vec3Projected(TPE_Vec4 v, TPE_Vec4 base);
 
+/** Returns the closest point on given line segment (a,b) to given point (p). */
+TPE_Vec4 TPE_lineSegmentClosestPoint(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 p);
+
 /** Converts a linear velocity of an orbiting point to the angular velocity
   (angle units per time units). This depends on the distance of the point from
   the center of rotation. */
@@ -1311,4 +1315,21 @@ TPE_Vec4 TPE_vec3Normalized(TPE_Vec4 v)
   return v;
 }
 
+TPE_Vec4 TPE_lineSegmentClosestPoint(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 p)
+{
+  TPE_Vec4 ab = TPE_vec3Minus(b,a);
+
+  TPE_Unit t = ((TPE_vec3DotProduct(ab,TPE_vec3Minus(p,a)) * 
+    TPE_FRACTIONS_PER_UNIT) / TPE_vec3DotProduct(ab,ab));
+
+  if (t < 0)
+    t = 0;
+  else if (t > TPE_FRACTIONS_PER_UNIT)
+    t = TPE_FRACTIONS_PER_UNIT;  
+
+  TPE_vec3Multiply(ab,t,&ab);
+
+  return TPE_vec3Plus(a,ab);
+}
+
 #endif // guard