collision: ray aabb

include/collision.h

@@ -1,5 +1,8 @@
 #pragma once
 
+#include <float.h>
+#include <stdio.h>
+
 #include "directxmath/directxmath.h"
 
 namespace collision {
@@ -9,6 +12,11 @@ namespace collision {
     XMVECTOR max;
   };
 
+  struct Sphere {
+    XMVECTOR center;
+    float radius;
+  };
+
   static inline AABB cube_aabb(XMVECTOR const & center, float half)
   {
     half = fabsf(half);
@@ -22,4 +30,53 @@ namespace collision {
     return XMVectorClamp(p, aabb.min, aabb.max);
   }
 
+  static inline bool test_sphere_aabb(Sphere const & sphere, AABB const & aabb, XMVECTOR & point)
+  {
+    point = closest_point_point_aabb(sphere.center, aabb);
+
+    XMVECTOR v = point - sphere.center;
+    return XMVectorGetX(XMVector3Dot(v, v)) <= sphere.radius * sphere.radius;
+  }
+
+  static inline void vector_swap_control(XMVECTOR & a, XMVECTOR & b, XMVECTOR const & control)
+  {
+    XMVECTOR tmp = XMVectorSelect(a, b, control);
+    b = XMVectorSelect(b, a, control);
+    a = tmp;
+  }
+
+  static inline bool intersect_ray_aabb(XMVECTOR const & point, XMVECTOR const & direction, AABB const & aabb, float & t_result, XMVECTOR & v_result)
+  {
+    // check parallel
+    XMVECTOR parallel = XMVectorLess(XMVectorAbs(direction), XMVectorSplatEpsilon());
+    XMVECTOR point_lt_aabb = XMVectorLess(point, aabb.min);
+    XMVECTOR point_gt_aabb = XMVectorGreater(point, aabb.max);
+    if (XMVectorGetX(parallel) != 0) // if (x < epsilon)
+      if (XMVectorGetX(point_lt_aabb) != 0 || XMVectorGetX(point_gt_aabb) != 0)
+        return false; // direction x is parallel and point x outside aabb
+    if (XMVectorGetY(parallel) != 0) // if (y < epsilon)
+      if (XMVectorGetY(point_lt_aabb) != 0 || XMVectorGetY(point_gt_aabb) != 0)
+        return false; // direction y is parallel and point y outside aabb
+    if (XMVectorGetZ(parallel) != 0) // if (z < epsilon)
+      if (XMVectorGetZ(point_lt_aabb) != 0 || XMVectorGetZ(point_gt_aabb) != 0)
+        return false; // direction z is parallel and point z outside aabb
+
+    XMVECTOR direction_reciprocal = XMVectorReciprocalEst(direction);
+    XMVECTOR t1 = (aabb.min - point) * direction_reciprocal;
+    XMVECTOR t2 = (aabb.max - point) * direction_reciprocal;
+    vector_swap_control(t2, t1, XMVectorGreater(t1, t2));
+
+    float tmin = XMVectorGetX(t1);
+    if (XMVectorGetY(t1) > tmin) tmin = XMVectorGetY(t1);
+    if (XMVectorGetZ(t1) > tmin) tmin = XMVectorGetZ(t1);
+    float tmax = XMVectorGetX(t2);
+    if (XMVectorGetY(t2) < tmax) tmax = XMVectorGetY(t2);
+    if (XMVectorGetZ(t2) < tmax) tmax = XMVectorGetZ(t2);
+    if (tmin > tmax) return false; // no intersection
+
+    t_result = tmin;
+    v_result = point + direction * tmin;
+
+    return true;
+  }
 }

include/collision_scene.h

@@ -3,5 +3,6 @@
 namespace collision_scene {
   void load();
   void draw();
-  void update(int up, int down, int left, int right);
+  void update(int up, int down, int left, int right,
+              int w, int s, int a, int d);
 }

include/test.h

@@ -6,7 +6,8 @@ extern "C" {
 
   void load(const char * source_path);
   void draw();
-  void update_keyboard(int up, int down, int left, int right);
+  void update_keyboard(int up, int down, int left, int right,
+                       int w, int s, int a, int d);
   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

@@ -11,7 +11,8 @@ function init()
 void load(const char * source_path);
 void update_window(int width, int height);
 void draw();
-void update_keyboard(int kbup, int kbdown, int kbleft, int kbright);
+void update_keyboard(int up, int down, int left, int right,
+                     int w, int s, int a, int d);
 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,
@@ -61,7 +62,11 @@ local update = function(time)
    local down = love.keyboard.isDown("down")
    local left = love.keyboard.isDown("left")
    local right = love.keyboard.isDown("right")
-   test.update_keyboard(up, down, left, right);
+   local w = love.keyboard.isDown("w")
+   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);
 
    test.update(time)
 end

src/collision_scene.cpp

@@ -27,6 +27,9 @@ namespace collision_scene {
   static unsigned int index_buffer;
 
   static XMVECTOR point_position;
+  static XMVECTOR point_1_position;
+
+  static unsigned int ray_vertex_buffer;
 
   #pragma GCC diagnostic push
   #pragma GCC diagnostic ignored "-Wnarrowing"
@@ -148,6 +151,23 @@ namespace collision_scene {
     load_index_buffer();
 
     point_position = XMVectorSet(0, 0, 0, 1);
+    point_1_position = XMVectorSet(0, 0, 0, 1);
+
+    // ray buffer
+    glGenBuffers(1, &ray_vertex_buffer);
+  }
+
+  void load_ray_vertex_buffer(XMVECTOR const & a, XMVECTOR const & b)
+  {
+    _Float16 data[] = {
+      (_Float16)XMVectorGetX(a), (_Float16)XMVectorGetY(a),
+      (_Float16)XMVectorGetX(b), (_Float16)XMVectorGetY(b),
+    };
+
+    glBindBuffer(GL_ARRAY_BUFFER, ray_vertex_buffer);
+    glBufferData(GL_ARRAY_BUFFER, 2 * 4, data, GL_DYNAMIC_DRAW);
+
+    glBindBuffer(GL_ARRAY_BUFFER, 0);
   }
 
   static inline XMMATRIX view()
@@ -163,13 +183,19 @@ namespace collision_scene {
     return XMMatrixOrthographicRH(10, 10, 0, 10);
   }
 
-  void update(int up, int down, int left, int right)
+  void update(int up, int down, int left, int right,
+              int w, int s, int a, int d)
   {
-    float rate = 0.1f;
+    float rate = 0.05f;
     float forward = (rate * up + -rate * down);
     float strafe = (-rate * left + rate * right);
 
+    float forward_1 = (rate * w + -rate * s);
+    float strafe_1 = (-rate * a + rate * d);
+
     point_position = XMVector3Transform(point_position, XMMatrixTranslation(strafe, forward, 0));
+
+    point_1_position = XMVector3Transform(point_1_position, XMMatrixTranslation(strafe_1, forward_1, 0));
   }
 
   static inline void set_transform(XMMATRIX const & transform)
@@ -179,6 +205,33 @@ namespace collision_scene {
     glUniformMatrix4fv(location.uniform.transform, 1, false, (float *)&float_transform);
   }
 
+  const int circle_base_vertex = 8;
+  const int circle_base_index = 5 * (sizeof (unsigned short));
+
+  void draw_line(XMMATRIX const & transform, XMVECTOR const & a, XMVECTOR const & b)
+  {
+    load_ray_vertex_buffer(a, b);
+    set_transform(transform);
+    glBindVertexBuffer(0, ray_vertex_buffer, 0, per_vertex_size);
+    glDrawArrays(GL_LINES, 0, 4);
+  }
+
+  void draw_sphere(XMMATRIX const & transform, XMVECTOR const & center, float radius)
+  {
+    XMMATRIX sphere_transform
+      = XMMatrixScaling(radius, radius, radius)
+      * XMMatrixTranslationFromVector(center)
+      * transform;
+    set_transform(sphere_transform);
+    glBindVertexBuffer(0, per_vertex_buffer, 0, per_vertex_size);
+    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()
   {
     glUseProgram(program);
@@ -216,31 +269,24 @@ namespace collision_scene {
     glDrawElementsBaseVertex(GL_LINE_STRIP, 5, GL_UNSIGNED_SHORT, (void*)(cube_base_index), cube_base_vertex);
 
     // circle
-    const int circle_base_vertex = 8;
+    const float point_radius = 0.05f;
-    const int circle_base_index = 5 * (sizeof (unsigned short));
 
-    XMMATRIX point_scale = XMMatrixScaling(0.05, 0.05, 0.05);
-
-    // point
     glUniform3f(location.uniform.base_color, 1.0, 0.5, 0.0);
+    draw_sphere(transform, point_position, point_radius);
+    draw_sphere(transform, point_1_position, point_radius);
 
-    XMMATRIX point_transform
+    XMVECTOR direction = XMVector3Normalize(point_1_position - point_position);
-      = point_scale
-      * XMMatrixTranslationFromVector(point_position)
-      * transform;
-    set_transform(point_transform);
-    glDrawElementsBaseVertex(GL_LINE_STRIP, 33, GL_UNSIGNED_SHORT, (void*)(circle_base_index), circle_base_vertex);
-
-    // closest point
-    glUniform3f(location.uniform.base_color, 1.0, 0.0, 0.0);
-
     collision::AABB cube_aabb = collision::cube_aabb(cube_position, cube_half);
-    XMVECTOR closest_point_position = collision::closest_point_point_aabb(point_position, cube_aabb);
+    XMVECTOR intersection_point;
-    XMMATRIX closest_point_transform
+    float t;
-      = point_scale
+    bool intersection = intersect_ray_aabb(point_position, direction, cube_aabb, t, intersection_point);
-      * XMMatrixTranslationFromVector(closest_point_position)
+    if (intersection && t > 0.0f) {
-      * transform;
+      glUniform3f(location.uniform.base_color, 0.9, 0.0, 0.0);
-    set_transform(closest_point_transform);
+      draw_line(transform, point_position, intersection_point);
-    glDrawElementsBaseVertex(GL_LINE_STRIP, 33, GL_UNSIGNED_SHORT, (void*)(circle_base_index), circle_base_vertex);
+      draw_sphere(transform, intersection_point, point_radius);
+    } else {
+      glUniform3f(location.uniform.base_color, 0.5, 0.5, 0.5);
+      draw_line(transform, point_position, point_position + direction * 20.0f);
+    }
   }
 }

src/test.cpp

@@ -287,12 +287,14 @@ void load(const char * source_path)
   collision_scene::load();
 }
 
-void update_keyboard(int up, int down, int left, int right)
+void update_keyboard(int up, int down, int left, int right,
+                     int w, int s, int a, int d)
 {
   //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);
+  collision_scene::update(up, down, left, right,
+                          w, s, a, d);
 }
 
 const int max_joysticks = 8;