From e38d9338d9cf38e70e24c3747d755e625fb237f1 Mon Sep 17 00:00:00 2001 From: Zack Buhman Date: Fri, 13 Mar 2026 21:46:17 -0500 Subject: [PATCH] collision: add closest point segment segment --- include/collision.h | 88 +++++++++++++++++++++++++++++++++++++++ include/collision_scene.h | 4 +- include/test.h | 4 +- main.lua | 17 +++++++- src/collision_scene.cpp | 83 +++++++++++++++++++++++++++++------- src/test.cpp | 8 +++- 6 files changed, 182 insertions(+), 22 deletions(-) diff --git a/include/collision.h b/include/collision.h index 38e2029..717c23f 100644 --- a/include/collision.h +++ b/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; + } } diff --git a/include/collision_scene.h b/include/collision_scene.h index 95a2915..7a66550 100644 --- a/include/collision_scene.h +++ b/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); } diff --git a/include/test.h b/include/test.h index 1e10271..e0b9c41 100644 --- a/include/test.h +++ b/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, diff --git a/main.lua b/main.lua index e7f665e..0b24812 100644 --- a/main.lua +++ b/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 diff --git a/src/collision_scene.cpp b/src/collision_scene.cpp index 115cb4b..39648a6 100644 --- a/src/collision_scene.cpp +++ b/src/collision_scene.cpp @@ -26,8 +26,7 @@ namespace collision_scene { static unsigned int index_buffer; - static XMVECTOR point_position; - static XMVECTOR point_1_position; + static XMVECTOR point_position[4]; static unsigned int ray_vertex_buffer; @@ -150,8 +149,8 @@ namespace collision_scene { load_index_buffer(); - point_position = XMVectorSet(0, 0, 0, 1); - point_1_position = XMVectorSet(0, 0, 0, 1); + for (int i = 0; i < 4; i++) + 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 strafe_1 = (-rate * a + rate * d); + float forward[4]; + 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); + */ } } diff --git a/src/test.cpp b/src/test.cpp index 3883701..0714cfb 100644 --- a/src/test.cpp +++ b/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;