Add sphere capsule collision

make.sh

@@ -1,6 +1,6 @@
 #!/bin/sh
 
-PROGRAM=test
+PROGRAM=test_sdl
 
 rm $PROGRAM
 g++ -x c -std=c99 -g -Wall -Wextra -pedantic -O3 -Wall -Wextra -Wno-unused-parameter -Wno-missing-field-initializers -lSDL2 -o $PROGRAM $PROGRAM.c && ./$PROGRAM

test_sdl.c

@@ -281,6 +281,7 @@ void addBody(uint8_t shape, TPE_Unit param1, TPE_Unit param2, TPE_Unit param3)
   switch (shape)
   {
     case TPE_SHAPE_CYLINDER:
+    case TPE_SHAPE_CAPSULE:
       v = cylinderVertices;
       t = cylinderTriangleIndices;
       vc = CYLINDER_VERTEX_COUNT;
@@ -331,7 +332,7 @@ int main()
 
   int running = 1;
 
-  addBody(TPE_SHAPE_CYLINDER,300,1024,0);
+  addBody(TPE_SHAPE_CAPSULE,300,1024,0);
   addBody(TPE_SHAPE_SPHERE,256,0,0);
 
   //-------
@@ -352,7 +353,7 @@ 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,128,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);

tinyphysicsengine.h

@@ -608,8 +608,44 @@ TPE_Unit TPE_bodyCollides(const TPE_Body *body1, const TPE_Body *body2,
       break;
     } 
 
+    case TPE_COLLISION_TYPE(TPE_SHAPE_SPHERE,TPE_SHAPE_CAPSULE):
+    {
+      const TPE_Body *sphere;
+      const TPE_Body *capsule;
+
+      _TPE_getShapes(body1,body2,TPE_SHAPE_SPHERE,&sphere,&capsule);
+
+      TPE_Vec4 capsuleQuat = TPE_bodyGetOrientation(capsule);
+
+      // capsule endpoints:
+      TPE_Vec4 cA = TPE_vec4(0,capsule->shapeParams[1] / 2,0,0);
+      TPE_Vec4 cB = TPE_vec4(0,-1 * cA.y,0,0);
+
+      TPE_rotatePoint(&cA,capsuleQuat);
+      TPE_rotatePoint(&cB,capsuleQuat);
+
+      TPE_vec3Add(cA,capsule->position,&cA);
+      TPE_vec3Add(cB,capsule->position,&cB);
+
+      TPE_Body sphere2; // sphere at the capsule's closest point
+
+      TPE_bodyInit(&sphere2);
+      sphere2.shape = TPE_SHAPE_SPHERE;
+      sphere2.shapeParams[0] = capsule->shapeParams[0];
+      sphere2.position = TPE_lineSegmentClosestPoint(cA,cB,sphere->position);
+
+      uint8_t swap = sphere == body2;
+
+      return TPE_bodyCollides(swap ? &sphere2 : sphere,swap ? sphere : &sphere2,
+        collisionPoint,collisionNormal);
+
+      break;
+    }
+
     case TPE_COLLISION_TYPE(TPE_SHAPE_SPHERE,TPE_SHAPE_CYLINDER):
     {
+      // TODO: would this be better to do via sphere-capsule collision?
+
       const TPE_Body *sphere;
       const TPE_Body *cylinder;