From afc0720f0895ddefc40ea1336f4a4df194e73a97 Mon Sep 17 00:00:00 2001 From: Orange Date: Sun, 9 Nov 2025 16:02:13 +0300 Subject: [PATCH] Refactor: Simplify GJK simplex handling Removes the separate `Simplex` class and integrates its functionality directly into the `GjkAlgorithm`. This simplifies the code and reduces unnecessary overhead. Updates tests to align with refactored implementation. --- include/omath/collision/gjk_algorithm.hpp | 10 +- include/omath/collision/mesh_collider.hpp | 1 + include/omath/collision/simplex.hpp | 143 +++++++++++++++------- tests/general/unit_test_gjk.cpp | 6 +- 4 files changed, 110 insertions(+), 50 deletions(-) diff --git a/include/omath/collision/gjk_algorithm.hpp b/include/omath/collision/gjk_algorithm.hpp index b2c7a48..d494cb1 100644 --- a/include/omath/collision/gjk_algorithm.hpp +++ b/include/omath/collision/gjk_algorithm.hpp @@ -9,12 +9,14 @@ namespace omath::collision { + template class GjkAlgorithm final { public: [[nodiscard]] - static Vector3 find_support_vertex(const MeshCollider& collider_a, const MeshCollider& collider_b, - const Vector3& direction) + static MeshCollider::VertexType find_support_vertex(const ColliderType& collider_a, + const ColliderType& collider_b, + const MeshCollider::VertexType& direction) { return collider_a.find_abs_furthest_vertex(direction) - collider_b.find_abs_furthest_vertex(-direction); } @@ -25,7 +27,7 @@ namespace omath::collision // Get initial support point in any direction auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0}); - Simplex simplex; + Simplex simplex; simplex.push_front(support); auto direction = -support; @@ -44,4 +46,4 @@ namespace omath::collision } } }; -}// namespace omath::collision \ No newline at end of file +} // namespace omath::collision \ No newline at end of file diff --git a/include/omath/collision/mesh_collider.hpp b/include/omath/collision/mesh_collider.hpp index f422a3e..e0b4ea3 100644 --- a/include/omath/collision/mesh_collider.hpp +++ b/include/omath/collision/mesh_collider.hpp @@ -12,6 +12,7 @@ namespace omath::collision class MeshCollider { public: + using VertexType = Vector3; MeshCollider(const std::vector>& vertexes, const Vector3 origin) : m_vertexes(vertexes), m_origin(origin) { diff --git a/include/omath/collision/simplex.hpp b/include/omath/collision/simplex.hpp index e585508..140c193 100644 --- a/include/omath/collision/simplex.hpp +++ b/include/omath/collision/simplex.hpp @@ -1,65 +1,114 @@ -// -// Created by Vlad on 11/9/2025. -// - #pragma once #include "omath/linear_algebra/vector3.hpp" #include +#include +#include +#include namespace omath::collision { - template> + + // Minimal structural contract for the vector type used by GJK. + template + concept GjkVector = requires(const V& a, const V& b) { + { -a } -> std::same_as; + { a - b } -> std::same_as; + { a.cross(b) } -> std::same_as; + { a.point_to_same_direction(b) } -> std::same_as; + }; + + template> class Simplex final { - std::array m_points; - std::size_t m_size; + std::array m_points{}; // value-initialized + std::size_t m_size{0}; public: - constexpr Simplex(): m_size(0) - { - } + static constexpr std::size_t capacity = 4; - constexpr Simplex& operator=(const std::initializer_list& list) + constexpr Simplex() = default; + + // Keep your convenient "{a, b, c}" assignments, but guard size. + constexpr Simplex& operator=(std::initializer_list list) noexcept { + assert(list.size() <= capacity && "Simplex can have at most 4 points"); m_size = 0; - - for (const VectorType& point : list) - m_points[m_size++] = point; - + for (const auto& p : list) + m_points[m_size++] = p; return *this; } - constexpr void push_front(const VectorType& point) + // Safe push_front: only shifts the valid range; no reads from uninitialized slots. + constexpr void push_front(const VectorType& p) noexcept { - m_points = {point, m_points[0], m_points[1], m_points[2]}; - m_size = std::min(m_size + 1, 4); + const std::size_t limit = (m_size < capacity) ? m_size : capacity - 1; + for (std::size_t i = limit; i > 0; --i) + m_points[i] = m_points[i - 1]; + m_points[0] = p; + if (m_size < capacity) + ++m_size; } - constexpr const VectorType& operator[](const std::size_t i) const + // Accessors + constexpr const VectorType& operator[](std::size_t i) const noexcept { return m_points[i]; } - [[nodiscard]] - constexpr std::size_t size() const + constexpr VectorType& operator[](std::size_t i) noexcept + { + return m_points[i]; + } + + [[nodiscard]] constexpr std::size_t size() const noexcept { return m_size; } - [[nodiscard]] - constexpr auto begin() const + [[nodiscard]] constexpr bool empty() const noexcept + { + return m_size == 0; + } + + [[nodiscard]] constexpr const VectorType& front() const noexcept + { + return m_points[0]; + } + + [[nodiscard]] constexpr const VectorType& back() const noexcept + { + return m_points[m_size - 1]; + } + + [[nodiscard]] constexpr const VectorType* data() const noexcept + { + return m_points.data(); + } + + [[nodiscard]] constexpr auto begin() const noexcept { return m_points.begin(); } - [[nodiscard]] - constexpr auto end() const + + [[nodiscard]] constexpr auto end() const noexcept { - return m_points.end() - (4 - m_size); + return m_points.begin() + m_size; } - [[nodiscard]] - constexpr bool handle(VectorType& direction) + + constexpr void clear() noexcept { - switch (size()) + m_size = 0; + } + + // GJK step: updates simplex + next search direction. + // Returns true iff the origin lies inside the tetrahedron. + [[nodiscard]] constexpr bool handle(VectorType& direction) noexcept + { + switch (m_size) { + case 0: + return false; + case 1: + return handle_point(direction); case 2: return handle_line(direction); case 3: @@ -70,29 +119,36 @@ namespace omath::collision std::unreachable(); } } + private: - [[nodiscard]] - constexpr bool handle_line(VectorType& direction) + [[nodiscard]] constexpr bool handle_point(VectorType& direction) noexcept + { + const auto& a = m_points[0]; + direction = -a; + return false; + } + + [[nodiscard]] constexpr bool handle_line(VectorType& direction) noexcept { const auto& a = m_points[0]; const auto& b = m_points[1]; const auto ab = b - a; - // ReSharper disable once CppTooWideScopeInitStatement const auto ao = -a; if (ab.point_to_same_direction(ao)) + { direction = ab.cross(ao).cross(ab); + } else { *this = {a}; direction = ao; } - return false; } - [[nodiscard]] - constexpr bool handle_triangle(VectorType& direction) + + [[nodiscard]] constexpr bool handle_triangle(VectorType& direction) noexcept { const auto& a = m_points[0]; const auto& b = m_points[1]; @@ -104,38 +160,40 @@ namespace omath::collision const auto abc = ab.cross(ac); + // Region AC if (abc.cross(ac).point_to_same_direction(ao)) { if (ac.point_to_same_direction(ao)) { *this = {a, c}; direction = ac.cross(ao).cross(ac); - return false; } *this = {a, b}; return handle_line(direction); } + // Region AB if (ab.cross(abc).point_to_same_direction(ao)) { *this = {a, b}; return handle_line(direction); } + + // Above or below triangle if (abc.point_to_same_direction(ao)) { direction = abc; } else { - *this = {a, c, b}; + *this = {a, c, b}; // flip winding direction = -abc; } - return false; } - [[nodiscard]] - constexpr bool handle_tetrahedron(VectorType& direction) + + [[nodiscard]] constexpr bool handle_tetrahedron(VectorType& direction) noexcept { const auto& a = m_points[0]; const auto& b = m_points[1]; @@ -156,20 +214,19 @@ namespace omath::collision *this = {a, b, c}; return handle_triangle(direction); } - if (acd.point_to_same_direction(ao)) { *this = {a, c, d}; return handle_triangle(direction); } - if (adb.point_to_same_direction(ao)) { *this = {a, d, b}; return handle_triangle(direction); } + // Origin inside tetrahedron return true; } }; -} // namespace omath::collision \ No newline at end of file +} // namespace omath::collision diff --git a/tests/general/unit_test_gjk.cpp b/tests/general/unit_test_gjk.cpp index 45259d4..bd0fdac 100644 --- a/tests/general/unit_test_gjk.cpp +++ b/tests/general/unit_test_gjk.cpp @@ -20,7 +20,7 @@ TEST(UnitTestGjk, TestCollisionTrue) const omath::collision::MeshCollider collider_a(mesh, {0.f, 0.f, 0.f}); const omath::collision::MeshCollider collider_b(mesh, {0.f, 0.5f, 0.f}); - const auto result = omath::collision::GjkAlgorithm::is_collide(collider_a, collider_b); + const auto result = omath::collision::GjkAlgorithm<>::is_collide(collider_a, collider_b); EXPECT_TRUE(result); } @@ -38,9 +38,9 @@ TEST(UnitTestGjk, TestCollisionFalse) }; const omath::collision::MeshCollider collider_a(mesh, {0.f, 0.f, 0.f}); - const omath::collision::MeshCollider collider_b(mesh, {0.f, 4.1f, 0.f}); + const omath::collision::MeshCollider collider_b(mesh, {0.f, 2.1f, 0.f}); - const auto result = omath::collision::GjkAlgorithm::is_collide(collider_a, collider_b); + const auto result = omath::collision::GjkAlgorithm<>::is_collide(collider_a, collider_b); EXPECT_FALSE(result); } \ No newline at end of file