collision: capsule capsule collision

include/collision.h

@@ -17,6 +17,12 @@ namespace collision {
     float radius;
   };
 
+  struct Capsule {
+    XMVECTOR a;
+    XMVECTOR b;
+    float radius;
+  };
+
   static inline AABB cube_aabb(XMVECTOR const & center, float half)
   {
     half = fabsf(half);
@@ -143,6 +149,31 @@ namespace collision {
     c2 = a2 + d2 * t2;
   }
 
+  static inline bool intersect_capsule_capsule(Capsule const & capsule1, Capsule const & capsule2,
+                                               XMVECTOR & p1, XMVECTOR & p2)
+  {
+    float t1;
+    float t2;
+    XMVECTOR c1; // closest point in capsule1
+    XMVECTOR c2; // closest point in capsule2
+    closest_point_segment_segment(capsule1.a, capsule1.b,
+                                  capsule2.a, capsule2.b,
+                                  t1, t2,
+                                  c1, c2);
+    float distance2 = XMVectorGetX(XMVector3Dot(c1 - c2, c1 - c2));
+    float radius = capsule1.radius + capsule2.radius;
+    if (distance2 >= radius * radius)
+      return false;
+
+    float length = XMVectorGetX(XMVector3Length(c1 - c2));
+    XMVECTOR normal = XMVector3NormalizeEst(c2 - c1);
+    printf("length2 %f\n", capsule1.radius - length);
+    p1 = c1 + normal * capsule1.radius;
+    p2 = c2 + normal;
+
+    return true;
+  }
+
   static inline bool intersect_moving_sphere_aabb(Sphere const & sphere, XMVECTOR const & direction, AABB const & aabb, float & t)
   {
     AABB expand(expand.min - XMVectorReplicate(sphere.radius),

src/collision_scene.cpp

@@ -294,11 +294,29 @@ namespace collision_scene {
     collision::closest_point_segment_segment(point_position[0], point_position[1],
                                              point_position[2], point_position[3],
                                              t1, t2, c1, c2);
-    glUniform3f(location.uniform.base_color, 1.0, 0.0, 0.0);
+    glUniform3f(location.uniform.base_color, 0.0, 0.5, 1.0);
     draw_sphere(transform, c1, point_radius);
-    glUniform3f(location.uniform.base_color, 1.0, 0.5, 0.0);
+    glUniform3f(location.uniform.base_color, 0.0, 1.0, 0.5);
     draw_sphere(transform, c2, point_radius);
 
+    collision::Capsule capsule1(point_position[0], point_position[1], 0.5);
+    collision::Capsule capsule2(point_position[2], point_position[3], 0.0);
+    glUniform3f(location.uniform.base_color, 0, 0.5, 1.0);
+    draw_capsule(transform, capsule1.a, capsule1.b, capsule1.radius);
+    glUniform3f(location.uniform.base_color, 0, 1.0, 0.5);
+    //draw_capsule(transform, capsule2.a, capsule2.b, capsule2.radius);
+
+    XMVECTOR c1_point;
+    XMVECTOR c2_point;
+    bool collided = collision::intersect_capsule_capsule(capsule1, capsule2, c1_point, c2_point);
+    if (collided) {
+      glUniform3f(location.uniform.base_color, 1.0, 0.0, 0.0);
+      draw_sphere(transform, c1_point, point_radius);
+
+      glUniform3f(location.uniform.base_color, 1.0, 0.5, 0.0);
+      draw_sphere(transform, c2_point, point_radius);
+    }
+
     // cube
     /*
     float cube_half = 0.5;