collision: add closest point segment segment

include/collision.h

@@ -79,4 +79,92 @@ namespace collision {
 
     return true;
   }
+
+  static inline float clamp(float f, float min, float max)
+  {
+    if (f < min) return min;
+    if (f > max) return max;
+    return f;
+  }
+
+  static inline void closest_point_segment_segment(XMVECTOR const & a1, XMVECTOR const & b1, // segment 1
+                                                   XMVECTOR const & a2, XMVECTOR const & b2, // segment 2
+                                                   float & t1, // (s)
+                                                   float & t2, // (t)
+                                                   XMVECTOR & c1, // closest point to segment 1
+                                                   XMVECTOR & c2) // closest point to segment 2
+  {
+    const float epsilon = 1.192092896e-7f;
+
+    XMVECTOR d1 = b1 - a1;
+    XMVECTOR d2 = b2 - a2;
+    XMVECTOR r = a1 - a2;
+    float l1 = XMVectorGetX(XMVector3Dot(d1, d1)); // squared length of segment 1 (a)
+    float l2 = XMVectorGetX(XMVector3Dot(d2, d2)); // squared length of segment 2 (e)
+    float d1r = XMVectorGetX(XMVector3Dot(d1, r)); // (c)
+    float d2r = XMVectorGetX(XMVector3Dot(d2, r)); // (f)
+
+    if (l1 <= epsilon && l2 <= epsilon) {
+      // both segments are points
+      t1 = 0.0f;
+      t2 = 0.0f;
+      c1 = a1;
+      c2 = a2;
+      return;
+    }
+    if (l1 <= epsilon) {
+      // segment 1 is a point
+      t1 = 0.0f;
+      t2 = clamp(d2r / l2, 0.0f, 1.0f);
+    } else if (l2 <= epsilon) {
+      // segment 2 is a point
+      t1 = clamp(-d1r / l1, 0.0, 1.0f);
+      t2 = 0.0f;
+    } else {
+      float b = XMVectorGetX(XMVector3Dot(d1, d2));
+      float denom = l1 * l2 - b * b;
+      if (denom != 0.0f) {
+        // segments are not parallel
+        t1 = clamp((b * d2r - d1r * l2) / denom, 0.0f, 1.0f);
+      } else {
+        t1 = 0.0f;
+      }
+      t2 = (b * t1 + d2r) / l2;
+      if (t2 < 0.0f) {
+        t2 = 0.0f;
+        t1 = clamp(-d1r / l1, 0.0f, 1.0f);
+      } else if (t2 > 1.0f) {
+        t2 = 1.0f;
+        t1 = clamp((b - d1r) / l1, 0.0f, 1.0f);
+      }
+    }
+
+    c1 = a1 + d1 * t1;
+    c2 = a2 + d2 * t2;
+  }
+
+  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),
+                expand.max + XMVectorReplicate(sphere.radius));
+
+    // intersect ray against expand
+    XMVECTOR point;
+    bool intersection = intersect_ray_aabb(sphere.center, direction, expand, t, point);
+    if (!intersection || t > 1.0f)
+      return false;
+
+    XMVECTOR lt = XMVectorLess(point, aabb.min);
+    int u = 0;
+    if (XMVectorGetX(lt) != 0) u |= 1;
+    if (XMVectorGetY(lt) != 0) u |= 2;
+    if (XMVectorGetZ(lt) != 0) u |= 4;
+    XMVECTOR gt = XMVectorGreater(point, aabb.max);
+    int v = 0;
+    if (XMVectorGetX(gt) != 0) v |= 1;
+    if (XMVectorGetY(gt) != 0) v |= 2;
+    if (XMVectorGetZ(gt) != 0) v |= 4;
+
+    int mask = u + v;
+  }
 }

include/collision_scene.h

@@ -4,5 +4,7 @@ namespace collision_scene {
   void load();
   void draw();
   void update(int up, int down, int left, int right,
-              int w, int s, int a, int d);
+              int w, int s, int a, int d,
+              int t, int g, int f, int h,
+              int i, int k, int j, int l);
 }

include/test.h

@@ -7,7 +7,9 @@ extern "C" {
   void load(const char * source_path);
   void draw();
   void update_keyboard(int up, int down, int left, int right,
-                       int w, int s, int a, int d);
+                       int w, int s, int a, int d,
+                       int t, int g, int f, int h,
+                       int i, int k, int j, int l);
   void update_mouse(int x, int y);
   void update_joystick(int joystick_index,
                        float lx, float ly, float rx, float ry, float tl, float tr,

main.lua

@@ -12,7 +12,9 @@ void load(const char * source_path);
 void update_window(int width, int height);
 void draw();
 void update_keyboard(int up, int down, int left, int right,
-                     int w, int s, int a, int d);
+                     int w, int s, int a, int d,
+                     int t, int g, int f, int h,
+                     int i, int k, int j, int l);
 void update_mouse(int x, int y);
 void update_joystick(int joystick_index,
                      float lx, float ly, float rx, float ry, float tl, float tr,
@@ -66,7 +68,18 @@ local update = function(time)
    local s = love.keyboard.isDown("s")
    local a = love.keyboard.isDown("a")
    local d = love.keyboard.isDown("d")
-   test.update_keyboard(up, down, left, right, w, s, a, d);
+   local t = love.keyboard.isDown("t")
+   local g = love.keyboard.isDown("g")
+   local f = love.keyboard.isDown("f")
+   local h = love.keyboard.isDown("h")
+   local i = love.keyboard.isDown("i")
+   local k = love.keyboard.isDown("k")
+   local j = love.keyboard.isDown("j")
+   local l = love.keyboard.isDown("l")
+   test.update_keyboard(up, down, left, right,
+                        w, s, a, d,
+                        t, g, f, h,
+                        i, k, j, l);
 
    test.update(time)
 end

src/collision_scene.cpp

@@ -26,8 +26,7 @@ namespace collision_scene {
 
   static unsigned int index_buffer;
 
-  static XMVECTOR point_position;
+  static XMVECTOR point_position[4];
-  static XMVECTOR point_1_position;
 
   static unsigned int ray_vertex_buffer;
 
@@ -150,8 +149,8 @@ namespace collision_scene {
 
     load_index_buffer();
 
-    point_position = XMVectorSet(0, 0, 0, 1);
+    for (int i = 0; i < 4; i++)
-    point_1_position = XMVectorSet(0, 0, 0, 1);
+      point_position[i] = XMVectorSet(0, 0, 0, 1);
 
     // ray buffer
     glGenBuffers(1, &ray_vertex_buffer);
@@ -183,19 +182,32 @@ namespace collision_scene {
     return XMMatrixOrthographicRH(10, 10, 0, 10);
   }
 
+  const float point_radius = 0.05f;
+
   void update(int up, int down, int left, int right,
-              int w, int s, int a, int d)
+              int w, int s, int a, int d,
+              int t, int g, int f, int h,
+              int i, int k, int j, int l)
   {
     float rate = 0.05f;
-    float forward = (rate * up + -rate * down);
-    float strafe = (-rate * left + rate * right);
 
-    float forward_1 = (rate * w + -rate * s);
+    float forward[4];
-    float strafe_1 = (-rate * a + rate * d);
+    float strafe[4];
 
-    point_position = XMVector3Transform(point_position, XMMatrixTranslation(strafe, forward, 0));
+    forward[0] = (rate * up + -rate * down);
+    strafe[0] = (-rate * left + rate * right);
 
-    point_1_position = XMVector3Transform(point_1_position, XMMatrixTranslation(strafe_1, forward_1, 0));
+    forward[1] = (rate * w + -rate * s);
+    strafe[1] = (-rate * a + rate * d);
+
+    forward[2] = (rate * t + -rate * g);
+    strafe[2] = (-rate * f + rate * h);
+
+    forward[3] = (rate * i + -rate * k);
+    strafe[3] = (-rate * j + rate * l);
+
+    for (int i = 0; i < 4; i++)
+      point_position[i] = XMVector3Transform(point_position[i], XMMatrixTranslation(strafe[i], forward[i], 0));
   }
 
   static inline void set_transform(XMMATRIX const & transform)
@@ -227,9 +239,17 @@ namespace collision_scene {
     glDrawElementsBaseVertex(GL_LINE_STRIP, 33, GL_UNSIGNED_SHORT, (void*)(circle_base_index), circle_base_vertex);
   }
 
-  void draw_capsule(XMVECTOR a, XMVECTOR b, float radius)
+  void draw_capsule(XMMATRIX const & transform, XMVECTOR a, XMVECTOR b, float radius)
   {
+    draw_sphere(transform, a, radius);
+    draw_sphere(transform, b, radius);
 
+    XMVECTOR abn = XMVector3Normalize(a - b);
+    XMVECTOR p = XMVectorSet(0, 0, 1, 0);
+    XMVECTOR pxabn = XMVector3Cross(abn, p);
+
+    draw_line(transform, a + pxabn * radius, b + pxabn * radius);
+    draw_line(transform, a - pxabn * radius, b - pxabn * radius);
   }
 
   void draw()
@@ -245,16 +265,42 @@ namespace collision_scene {
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffer);
 
     XMMATRIX transform = view() * projection();
-    XMFLOAT4X4 float_transform;
-    XMStoreFloat4x4(&float_transform, transform);
 
     // grid
-    glUniformMatrix4fv(location.uniform.transform, 1, false, (float *)&float_transform);
+    set_transform(transform);
     glUniform3f(location.uniform.base_color, 0, 1, 0);
     glUniform1i(location.uniform.use_grid_transform, 1);
     glDrawArraysInstanced(GL_LINES, 0, 4, 7);
 
+    glUniform1i(location.uniform.use_grid_transform, 0);
+
+    // segments
+    glUniform3f(location.uniform.base_color, 0, 0.5, 1.0);
+    draw_line(transform, point_position[0], point_position[1]);
+    glUniform3f(location.uniform.base_color, 0, 1.0, 0.5);
+    draw_line(transform, point_position[2], point_position[3]);
+
+    // points
+    glUniform3f(location.uniform.base_color, 0, 0.5, 1.0);
+    draw_sphere(transform, point_position[0], point_radius);
+    draw_sphere(transform, point_position[1], point_radius);
+
+    glUniform3f(location.uniform.base_color, 0, 1.0, 0.5);
+    draw_sphere(transform, point_position[2], point_radius);
+    draw_sphere(transform, point_position[3], point_radius);
+
+    float t1, t2;
+    XMVECTOR c1, c2;
+    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);
+    draw_sphere(transform, c1, point_radius);
+    glUniform3f(location.uniform.base_color, 1.0, 0.5, 0.0);
+    draw_sphere(transform, c2, point_radius);
+
     // cube
+    /*
     float cube_half = 0.5;
     XMVECTOR cube_position = XMVectorSet(1, 0, 0, 0);
     XMMATRIX cube_transform
@@ -269,7 +315,6 @@ namespace collision_scene {
     glDrawElementsBaseVertex(GL_LINE_STRIP, 5, GL_UNSIGNED_SHORT, (void*)(cube_base_index), cube_base_vertex);
 
     // circle
-    const float point_radius = 0.05f;
 
     glUniform3f(location.uniform.base_color, 1.0, 0.5, 0.0);
     draw_sphere(transform, point_position, point_radius);
@@ -288,5 +333,11 @@ namespace collision_scene {
       glUniform3f(location.uniform.base_color, 0.5, 0.5, 0.5);
       draw_line(transform, point_position, point_position + direction * 20.0f);
     }
+
+    glUniform3f(location.uniform.base_color, 0.5, 0.0, 0.5);
+    XMVECTOR ca = XMVectorSet(1, 2.5, 0, 0);
+    XMVECTOR cb = XMVectorSet(-1, 2, 0, 0);
+    draw_capsule(transform, ca, cb, 0.5);
+    */
   }
 }

src/test.cpp

@@ -288,13 +288,17 @@ void load(const char * source_path)
 }
 
 void update_keyboard(int up, int down, int left, int right,
-                     int w, int s, int a, int d)
+                     int w, int s, int a, int d,
+                     int t, int g, int f, int h,
+                     int i, int k, int j, int l)
 {
   //float forward = (0.1f * up + -0.1f * down);
   //float strafe = (-0.1f * left + 0.1f * right);
   //view::third_person::apply_translation(forward, strafe, 0);
   collision_scene::update(up, down, left, right,
-                          w, s, a, d);
+                          w, s, a, d,
+                          t, g, f, h,
+                          i, k, j, l);
 }
 
 const int max_joysticks = 8;