From 043b5c588d519cd00522f6bf2b11030b3b1b324e Mon Sep 17 00:00:00 2001 From: Orange Date: Sun, 9 Nov 2025 15:39:11 +0300 Subject: [PATCH] updated --- include/omath/collision/gjk_algorithm.hpp | 8 +- include/omath/collision/simplex.hpp | 228 ++++++++++++---------- 2 files changed, 126 insertions(+), 110 deletions(-) diff --git a/include/omath/collision/gjk_algorithm.hpp b/include/omath/collision/gjk_algorithm.hpp index cb8645a..b2c7a48 100644 --- a/include/omath/collision/gjk_algorithm.hpp +++ b/include/omath/collision/gjk_algorithm.hpp @@ -25,8 +25,8 @@ namespace omath::collision // Get initial support point in any direction auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0}); - Simplex points; - points.push_front(support); + Simplex simplex; + simplex.push_front(support); auto direction = -support; @@ -37,9 +37,9 @@ namespace omath::collision if (support.dot(direction) <= 0.f) return false; - points.push_front(support); + simplex.push_front(support); - if (handle_simplex(points, direction)) + if (simplex.handle(direction)) return true; } } diff --git a/include/omath/collision/simplex.hpp b/include/omath/collision/simplex.hpp index 703afb4..e59c132 100644 --- a/include/omath/collision/simplex.hpp +++ b/include/omath/collision/simplex.hpp @@ -8,152 +8,168 @@ namespace omath::collision { + template> class Simplex { - std::array, 4> m_points; - int m_size; + std::array m_points; + std::size_t m_size; public: - Simplex(): m_size(0) + constexpr Simplex(): m_size(0) { } - Simplex& operator=(const std::initializer_list> list) + constexpr Simplex& operator=(const std::initializer_list& list) { m_size = 0; - for (const Vector3& point : list) + for (const VectorType& point : list) m_points[m_size++] = point; return *this; } - void push_front(const Vector3& point) + constexpr void push_front(const VectorType& point) { m_points = {point, m_points[0], m_points[1], m_points[2]}; - m_size = std::min(m_size + 1, 4); + m_size = std::min(m_size + 1, 4); } - Vector3& operator[](const std::size_t i) + constexpr const VectorType& operator[](const std::size_t i) const { return m_points[i]; } - [[nodiscard]] std::size_t size() const + [[nodiscard]] + constexpr std::size_t size() const { return m_size; } - [[nodiscard]] auto begin() const + [[nodiscard]] + constexpr auto begin() const { return m_points.begin(); } - [[nodiscard]] auto end() const + [[nodiscard]] + constexpr auto end() const { return m_points.end() - (4 - m_size); } - }; - - bool handle_line(Simplex& points, Vector3& direction) - { - const Vector3& a = points[0]; - const Vector3& b = points[1]; - - const Vector3 ab = b - a; - // ReSharper disable once CppTooWideScopeInitStatement - const Vector3 ao = -a; - - if (ab.point_to_same_direction(ao)) - direction = ab.cross(ao).cross(ab); - else + [[nodiscard]] + constexpr bool handle(VectorType& direction) { - points = {a}; - direction = ao; - } - - return false; - } - - bool handle_triangle(Simplex& points, Vector3& direction) - { - const Vector3& a = points[0]; - const Vector3& b = points[1]; - const Vector3& c = points[2]; - - const Vector3 ab = b - a; - const Vector3 ac = c - a; - const Vector3 ao = -a; - - const Vector3 abc = ab.cross(ac); - - if (abc.cross(ac).point_to_same_direction(ao)) - { - if (ac.point_to_same_direction(ao)) + switch (m_points.size()) { - points = {a, c}; - direction = ac.cross(ao).cross(ac); - - return false; + case 2: + return handle_line(direction); + case 3: + return handle_triangle(direction); + case 4: + return handle_tetrahedron(direction); + default: + std::unreachable(); } - return handle_line(points = {a, b}, direction); } - - if (ab.cross(abc).point_to_same_direction(ao)) - return handle_line(points = {a, b}, direction); - - if (abc.point_to_same_direction(ao)) + private: + [[nodiscard]] + constexpr bool handle_line(VectorType& direction) { - direction = abc; - } - else - { - points = {a, c, b}; - direction = -abc; - } + const auto& a = m_points[0]; + const auto& b = m_points[1]; - return false; - } + const auto ab = b - a; + // ReSharper disable once CppTooWideScopeInitStatement + const auto ao = -a; - bool handle_tetrahedron(Simplex& simplex, Vector3& direction) - { - const auto& a = simplex[0]; - const auto& b = simplex[1]; - const auto& c = simplex[2]; - const auto& d = simplex[3]; + if (ab.point_to_same_direction(ao)) + direction = ab.cross(ao).cross(ab); + else + { + *this = {a}; + direction = ao; + } - const Vector3 ab = b - a; - const Vector3 ac = c - a; - const Vector3 ad = d - a; - const Vector3 ao = -a; - - const Vector3 abc = ab.cross(ac); - const Vector3 acd = ac.cross(ad); - const Vector3 adb = ad.cross(ab); - - if (abc.point_to_same_direction(ao)) - return handle_triangle(simplex = {a, b, c}, direction); - - if (acd.point_to_same_direction(ao)) - return handle_triangle(simplex = {a, c, d}, direction); - - if (adb.point_to_same_direction(ao)) - return handle_triangle(simplex = {a, d, b}, direction); - - return true; - } - - [[nodiscard]] - bool handle_simplex(Simplex& points, Vector3& direction) - { - switch (points.size()) - { - case 2: - return handle_line(points, direction); - case 3: - return handle_triangle(points, direction); - case 4: - return handle_tetrahedron(points, direction); - default: return false; } - } + [[nodiscard]] + constexpr bool handle_triangle(VectorType& direction) + { + const auto& a = m_points[0]; + const auto& b = m_points[1]; + const auto& c = m_points[2]; + + const auto ab = b - a; + const auto ac = c - a; + const auto ao = -a; + + const auto abc = ab.cross(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); + } + + if (ab.cross(abc).point_to_same_direction(ao)) + { + *this = {a, b}; + return handle_line(direction); + } + if (abc.point_to_same_direction(ao)) + { + direction = abc; + } + else + { + *this = {a, c, b}; + direction = -abc; + } + + return false; + } + [[nodiscard]] + constexpr bool handle_tetrahedron(VectorType& direction) + { + const auto& a = m_points[0]; + const auto& b = m_points[1]; + const auto& c = m_points[2]; + const auto& d = m_points[3]; + + const auto ab = b - a; + const auto ac = c - a; + const auto ad = d - a; + const auto ao = -a; + + const auto abc = ab.cross(ac); + const auto acd = ac.cross(ad); + const auto adb = ad.cross(ab); + + if (abc.point_to_same_direction(ao)) + { + *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); + } + return true; + } + }; + } // namespace omath::collision \ No newline at end of file