From c515dc89a915bb69aedd60c0c626a2552ade992c Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 13:33:18 +0300 Subject: [PATCH 01/13] Adds GjkHitInfo and refactors collision detection Introduces `GjkHitInfo` to encapsulate collision results, including the simplex. Refactors `is_collide` to return the `GjkHitInfo` struct, providing more detailed collision data. This prepares the codebase for integrating the EPA algorithm (as per the feature branch) which requires simplex information. --- include/omath/collision/gjk_algorithm.hpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/include/omath/collision/gjk_algorithm.hpp b/include/omath/collision/gjk_algorithm.hpp index c115f23..81a648c 100644 --- a/include/omath/collision/gjk_algorithm.hpp +++ b/include/omath/collision/gjk_algorithm.hpp @@ -7,6 +7,13 @@ namespace omath::collision { + template + struct GjkHitInfo final + { + bool hit{false}; + Simplex simplex; // valid only if hit == true and size==4 + }; + template class GjkAlgorithm final { @@ -23,7 +30,13 @@ namespace omath::collision [[nodiscard]] static bool is_collide(const ColliderType& collider_a, const ColliderType& collider_b) { - // Get initial support point in any direction + return is_collide_with_simplex_info(collider_a, collider_b).hit; + } + + [[nodiscard]] + static GjkHitInfo is_collide_with_simplex_info(const ColliderType& collider_a, + const ColliderType& collider_b) + { auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0}); Simplex simplex; @@ -36,12 +49,12 @@ namespace omath::collision support = find_support_vertex(collider_a, collider_b, direction); if (support.dot(direction) <= 0.f) - return false; + return {false, simplex}; simplex.push_front(support); if (simplex.handle(direction)) - return true; + return {true, simplex}; } } }; From 6d3b543648894eaa89746929841f60d1a11d41f9 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 15:29:10 +0300 Subject: [PATCH 02/13] Refactors triangle calculations for generic vectors Updates the `Triangle` class to use a generic `Vector` type instead of `Vector3`, enhancing flexibility and reusability. Changes include updating relevant function return types to use `Vector::ContainedType` and adapting length and distance calculations accordingly. This refactoring supports the ongoing work on the EPA algorithm (feature/epa_algorithm) by providing a more adaptable foundation for geometric calculations. --- include/omath/linear_algebra/triangle.hpp | 20 ++++++++++---------- include/omath/linear_algebra/vector2.hpp | 1 + include/omath/linear_algebra/vector3.hpp | 1 + include/omath/linear_algebra/vector4.hpp | 1 + 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/include/omath/linear_algebra/triangle.hpp b/include/omath/linear_algebra/triangle.hpp index b8a877c..1d8e68c 100644 --- a/include/omath/linear_algebra/triangle.hpp +++ b/include/omath/linear_algebra/triangle.hpp @@ -26,12 +26,12 @@ namespace omath { } - Vector3 m_vertex1; - Vector3 m_vertex2; - Vector3 m_vertex3; + Vector m_vertex1; + Vector m_vertex2; + Vector m_vertex3; [[nodiscard]] - constexpr Vector3 calculate_normal() const + constexpr Vector calculate_normal() const { const auto b = side_b_vector(); const auto a = side_a_vector(); @@ -40,25 +40,25 @@ namespace omath } [[nodiscard]] - float side_a_length() const + Vector::ContainedType side_a_length() const { return m_vertex1.distance_to(m_vertex2); } [[nodiscard]] - float side_b_length() const + Vector::ContainedType side_b_length() const { return m_vertex3.distance_to(m_vertex2); } [[nodiscard]] - constexpr Vector3 side_a_vector() const + constexpr Vector side_a_vector() const { return m_vertex1 - m_vertex2; } [[nodiscard]] - constexpr float hypot() const + constexpr Vector::ContainedType hypot() const { return m_vertex1.distance_to(m_vertex3); } @@ -72,12 +72,12 @@ namespace omath return std::abs(side_a * side_a + side_b * side_b - hypot_value * hypot_value) <= 0.0001f; } [[nodiscard]] - constexpr Vector3 side_b_vector() const + constexpr Vector side_b_vector() const { return m_vertex3 - m_vertex2; } [[nodiscard]] - constexpr Vector3 mid_point() const + constexpr Vector mid_point() const { return (m_vertex1 + m_vertex2 + m_vertex3) / 3; } diff --git a/include/omath/linear_algebra/vector2.hpp b/include/omath/linear_algebra/vector2.hpp index 7f16173..2ee3750 100644 --- a/include/omath/linear_algebra/vector2.hpp +++ b/include/omath/linear_algebra/vector2.hpp @@ -19,6 +19,7 @@ namespace omath class Vector2 { public: + using ContainedType = Type; Type x = static_cast(0); Type y = static_cast(0); diff --git a/include/omath/linear_algebra/vector3.hpp b/include/omath/linear_algebra/vector3.hpp index 4b1d381..1aaec5a 100644 --- a/include/omath/linear_algebra/vector3.hpp +++ b/include/omath/linear_algebra/vector3.hpp @@ -23,6 +23,7 @@ namespace omath class Vector3 : public Vector2 { public: + using ContainedType = Type; Type z = static_cast(0); constexpr Vector3(const Type& x, const Type& y, const Type& z) noexcept: Vector2(x, y), z(z) { diff --git a/include/omath/linear_algebra/vector4.hpp b/include/omath/linear_algebra/vector4.hpp index 48b0278..f045df9 100644 --- a/include/omath/linear_algebra/vector4.hpp +++ b/include/omath/linear_algebra/vector4.hpp @@ -13,6 +13,7 @@ namespace omath class Vector4 : public Vector3 { public: + using ContainedType = Type; Type w; constexpr Vector4(const Type& x, const Type& y, const Type& z, const Type& w): Vector3(x, y, z), w(w) From 06b597f37c5de3091e2bd58b5e270f2ee95cce60 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 15:57:38 +0300 Subject: [PATCH 03/13] added epa --- include/omath/collision/epa_algorithm.hpp | 277 ++++++++++++++++++++++ include/omath/collision/mesh_collider.hpp | 4 +- tests/general/unit_test_epa.cpp | 57 +++++ 3 files changed, 336 insertions(+), 2 deletions(-) create mode 100644 include/omath/collision/epa_algorithm.hpp create mode 100644 tests/general/unit_test_epa.cpp diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp new file mode 100644 index 0000000..655f31b --- /dev/null +++ b/include/omath/collision/epa_algorithm.hpp @@ -0,0 +1,277 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include // find_if +#include "simplex.hpp" + +namespace omath::collision +{ + template + concept EpaVector = requires(const V& a, const V& b, float s) { + { a - b } -> std::same_as; + { a.cross(b) } -> std::same_as; + { a.dot(b) } -> std::same_as; + { -a } -> std::same_as; + { a * s } -> std::same_as; + { a / s } -> std::same_as; + }; + + template + class Epa final + { + public: + using Vertex = typename ColliderType::VertexType; + static_assert(EpaVector, "VertexType must satisfy EpaVector concept"); + + struct Result + { + bool success{false}; + Vertex normal{}; // outward normal (from B to A) + float depth{0.0f}; + int iterations{0}; + int num_vertices{0}; + int num_faces{0}; + }; + + struct Params + { + int max_iterations{64}; + float tolerance{1e-4f}; // absolute tolerance on distance growth + }; + + // Precondition: simplex.size()==4 and contains the origin. + [[nodiscard]] + static Result solve(const ColliderType& a, + const ColliderType& b, + const Simplex& simplex, + const Params params = {}) + { + // --- Build initial polytope from simplex (4 points) --- + std::vector verts; + verts.reserve(64); + for (std::size_t i = 0; i < simplex.size(); ++i) + verts.push_back(simplex[i]); + + // Initial tetra faces (windings corrected in make_face) + std::vector faces; + faces.reserve(128); + faces.push_back(make_face(verts, 0,1,2)); + faces.push_back(make_face(verts, 0,2,3)); + faces.push_back(make_face(verts, 0,3,1)); + faces.push_back(make_face(verts, 1,3,2)); + + auto heap = rebuild_heap(faces); + + Result out{}; + + for (int it = 0; it < params.max_iterations; ++it) + { + // If heap might be stale after face edits, rebuild lazily. + if (heap.empty()) break; + // Rebuild when the "closest" face changed (simple cheap guard) + // (We could keep face handles; this is fine for small Ns.) + { + const auto top = heap.top(); + if (faces[top.idx].d != top.d) + heap = rebuild_heap(faces); + } + if (heap.empty()) break; + + const int fidx = heap.top().idx; + const Face f = faces[fidx]; + + // Get farthest point in face normal direction + const Vertex p = support_point(a, b, f.n); + const float p_dist = f.n.dot(p); + + // Converged if we can’t push the face closer than tolerance + if (p_dist - f.d <= params.tolerance) + { + out.success = true; + out.normal = f.n; + out.depth = f.d; // along unit normal + out.iterations = it + 1; + out.num_vertices = static_cast(verts.size()); + out.num_faces = static_cast(faces.size()); + return out; + } + + // Add new vertex + const int new_idx = static_cast(verts.size()); + verts.push_back(p); + + // Mark faces visible from p and collect their horizon + std::vector to_delete(faces.size(), 0); + std::vector boundary; boundary.reserve(faces.size()*2); + + for (int i = 0; i < static_cast(faces.size()); ++i) + { + if (to_delete[i]) continue; + if (visible_from(faces[i], p)) + { + const auto& rf = faces[i]; + to_delete[i] = 1; + add_edge_boundary(boundary, rf.i0, rf.i1); + add_edge_boundary(boundary, rf.i1, rf.i2); + add_edge_boundary(boundary, rf.i2, rf.i0); + } + } + + // Remove visible faces + std::vector new_faces; new_faces.reserve(faces.size() + boundary.size()); + for (int i = 0; i < static_cast(faces.size()); ++i) + if (!to_delete[i]) new_faces.push_back(faces[i]); + faces.swap(new_faces); + + // Stitch new faces around the horizon + for (const auto& e : boundary) + faces.push_back(make_face(verts, e.a, e.b, new_idx)); + + // Rebuild heap after topology change + heap = rebuild_heap(faces); + + if (!std::isfinite(verts.back().dot(verts.back()))) + break; // safety + out.iterations = it + 1; + } + + // Fallback: pick closest face as best-effort answer + if (!faces.empty()) + { + auto best = faces[0]; + for (const auto& f : faces) if (f.d < best.d) best = f; + out.success = true; + out.normal = best.n; + out.depth = best.d; + out.num_vertices = static_cast(verts.size()); + out.num_faces = static_cast(faces.size()); + } + return out; + } + + private: + struct Face + { + int i0, i1, i2; + Vertex n; // unit outward normal + float d; // n · v0 (>=0 ideally because origin is inside) + }; + + struct Edge { int a, b; }; + + struct HeapItem { float d; int idx; }; + struct HeapCmp { + bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept { + return lhs.d > rhs.d; // min-heap by distance + } + }; + using Heap = std::priority_queue, HeapCmp>; + + static Heap rebuild_heap(const std::vector& faces) + { + Heap h; + for (int i = 0; i < static_cast(faces.size()); ++i) + h.push({faces[i].d, i}); + return h; + } + + static bool visible_from(const Face& f, const Vertex& p) + { + // positive if p is in front of the face + return (f.n.dot(p) - f.d) > 1e-7f; + } + + static void add_edge_boundary(std::vector& boundary, int a, int b) + { + // Keep edges that appear only once; erase if opposite already present + auto itb = std::find_if(boundary.begin(), boundary.end(), + [&](const Edge& e){ return e.a == b && e.b == a; }); + if (itb != boundary.end()) + boundary.erase(itb); // internal edge cancels out + else + boundary.push_back({a,b}); // horizon edge (directed) + } + + static Face make_face(const std::vector& verts, int i0, int i1, int i2) + { + const Vertex& a0 = verts[i0]; + const Vertex& a1 = verts[i1]; + const Vertex& a2 = verts[i2]; + Vertex n = (a1 - a0).cross(a2 - a0); + if (n.dot(n) <= 1e-30f) { + n = any_perp_vec(a1 - a0); // degenerate guard + } + // Ensure normal points outward (away from origin): require n·a0 >= 0 + if (n.dot(a0) < 0.0f) { std::swap(i1, i2); n = -n; } + const float inv_len = 1.0f / std::sqrt(std::max(n.dot(n), 1e-30f)); + n = n * inv_len; + const float d = n.dot(a0); + return { i0, i1, i2, n, d }; + } + + static Vertex support_point(const ColliderType& a, + const ColliderType& b, + const Vertex& dir) + { + return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir); + } + + template + static constexpr bool near_zero_vec(const V& v, const float eps = 1e-7f) + { + return v.dot(v) <= eps * eps; + } + + template + static constexpr V any_perp_vec(const V& v) + { + for (const auto& dir : {V{1,0,0}, V{0,1,0}, V{0,0,1}}) + if (const auto d = v.cross(dir); !near_zero_vec(d)) return d; + return V{1,0,0}; + } + }; + + // Optional: the GJK that returns a simplex for EPA (unchanged) + template + class GjkAlgorithmWithSimplex final + { + using Vertex = typename ColliderType::VertexType; + public: + struct Hit { bool hit{false}; Simplex simplex; }; + + [[nodiscard]] + static Vertex find_support_vertex(const ColliderType& a, + const ColliderType& b, + const Vertex& dir) + { + return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir); + } + + [[nodiscard]] + static Hit collide(const ColliderType& a, const ColliderType& b) + { + auto support = find_support_vertex(a, b, {1,0,0}); + Simplex simplex; simplex.push_front(support); + auto direction = -support; + + for (;;) + { + support = find_support_vertex(a, b, direction); + if (support.dot(direction) <= 0.f) return {}; + simplex.push_front(support); + if (simplex.handle(direction)) + { + if (simplex.size() == 4) return { true, simplex }; + // rare degeneracy: reseed + support = find_support_vertex(a, b, {0,1,0}); + simplex.clear(); simplex.push_front(support); + direction = -support; + } + } + } + }; +} // namespace omath::collision diff --git a/include/omath/collision/mesh_collider.hpp b/include/omath/collision/mesh_collider.hpp index adafab3..30951d8 100644 --- a/include/omath/collision/mesh_collider.hpp +++ b/include/omath/collision/mesh_collider.hpp @@ -19,14 +19,14 @@ namespace omath::collision } [[nodiscard]] - const Vector3& find_furthest_vertex(const Vector3& direction) const + const VertexType& find_furthest_vertex(const VertexType& direction) const { return *std::ranges::max_element(m_mesh.m_vertex_buffer, [&direction](const auto& first, const auto& second) { return first.dot(direction) < second.dot(direction); }); } [[nodiscard]] - Vector3 find_abs_furthest_vertex(const Vector3& direction) const + VertexType find_abs_furthest_vertex(const VertexType& direction) const { return m_mesh.vertex_to_world_space(find_furthest_vertex(direction)); } diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp new file mode 100644 index 0000000..57da26c --- /dev/null +++ b/tests/general/unit_test_epa.cpp @@ -0,0 +1,57 @@ +// +// Created by Vlad on 11/13/2025. +// +#include "omath/collision/gjk_algorithm.hpp" +#include "omath/engines/source_engine/collider.hpp" +#include +#include +#include + +namespace +{ + const omath::source_engine::Mesh mesh = {{{-1.f, -1.f, -1.f}, + {-1.f, -1.f, 1.f}, + {-1.f, 1.f, -1.f}, + {-1.f, 1.f, 1.f}, + {1.f, 1.f, 1.f}, + {1.f, 1.f, -1.f}, + {1.f, -1.f, 1.f}, + {1.f, -1.f, -1.f}}, + {}}; +} +TEST(UnitTestEpa, TestCollisionTrue) +{ + std::vector> vbo = {{-1, -1, -1}, {-1, -1, 1}, {-1, 1, -1}, {-1, 1, 1}, + {1, 1, 1}, {1, 1, -1}, {1, -1, 1}, {1, -1, -1}}; + std::vector> vao; // not needed for GJK/EPA + + omath::source_engine::Mesh a(vbo, vao, {1, 1, 1}); + omath::source_engine::Mesh b(vbo, vao, {1, 1, 1}); + + a.set_origin({0, 0, 0}); + b.set_origin({0.5f, 0, 0}); // slight overlap + + const omath::source_engine::MeshCollider collider_a(mesh); + + omath::source_engine::MeshCollider A(a), B(b); + + // 1) GJK → final simplex + using Gjk = omath::collision::GjkAlgorithm; + + auto gjk = Gjk::is_collide_with_simplex_info(A, B); + if (!gjk.hit) + { + std::cout << "No collision\n"; + } + using Epa = omath::collision::Epa; + // 2) EPA → normal/depth + Epa::Params params; + params.max_iterations = 64; + params.tolerance = 1e-4f; + auto epa = Epa::solve(A, B, gjk.simplex, params); + + if (!epa.success) + { + std::cout << "EPA failed\n"; + } +} \ No newline at end of file From 2699053102068920a800911d23d0da5b370b4e92 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:01:42 +0300 Subject: [PATCH 04/13] fixed formating --- include/omath/collision/epa_algorithm.hpp | 155 +++++++++++++--------- 1 file changed, 90 insertions(+), 65 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 655f31b..e6a3c0b 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -1,12 +1,12 @@ #pragma once -#include +#include "simplex.hpp" +#include // find_if #include -#include -#include #include #include -#include // find_if -#include "simplex.hpp" +#include +#include +#include namespace omath::collision { @@ -16,7 +16,7 @@ namespace omath::collision { a.cross(b) } -> std::same_as; { a.dot(b) } -> std::same_as; { -a } -> std::same_as; - { a * s } -> std::same_as; + { a* s } -> std::same_as; { a / s } -> std::same_as; }; @@ -29,25 +29,23 @@ namespace omath::collision struct Result { - bool success{false}; - Vertex normal{}; // outward normal (from B to A) - float depth{0.0f}; - int iterations{0}; - int num_vertices{0}; - int num_faces{0}; + bool success{false}; + Vertex normal{}; // outward normal (from B to A) + float depth{0.0f}; + int iterations{0}; + int num_vertices{0}; + int num_faces{0}; }; struct Params { - int max_iterations{64}; - float tolerance{1e-4f}; // absolute tolerance on distance growth + int max_iterations{64}; + float tolerance{1e-4f}; // absolute tolerance on distance growth }; // Precondition: simplex.size()==4 and contains the origin. [[nodiscard]] - static Result solve(const ColliderType& a, - const ColliderType& b, - const Simplex& simplex, + static Result solve(const ColliderType& a, const ColliderType& b, const Simplex& simplex, const Params params = {}) { // --- Build initial polytope from simplex (4 points) --- @@ -59,10 +57,10 @@ namespace omath::collision // Initial tetra faces (windings corrected in make_face) std::vector faces; faces.reserve(128); - faces.push_back(make_face(verts, 0,1,2)); - faces.push_back(make_face(verts, 0,2,3)); - faces.push_back(make_face(verts, 0,3,1)); - faces.push_back(make_face(verts, 1,3,2)); + faces.push_back(make_face(verts, 0, 1, 2)); + faces.push_back(make_face(verts, 0, 2, 3)); + faces.push_back(make_face(verts, 0, 3, 1)); + faces.push_back(make_face(verts, 1, 3, 2)); auto heap = rebuild_heap(faces); @@ -71,15 +69,16 @@ namespace omath::collision for (int it = 0; it < params.max_iterations; ++it) { // If heap might be stale after face edits, rebuild lazily. - if (heap.empty()) break; + if (heap.empty()) + break; // Rebuild when the "closest" face changed (simple cheap guard) // (We could keep face handles; this is fine for small Ns.) - { - const auto top = heap.top(); - if (faces[top.idx].d != top.d) - heap = rebuild_heap(faces); - } - if (heap.empty()) break; + + if (const auto top = heap.top(); faces[top.idx].d != top.d) + heap = rebuild_heap(faces); + + if (heap.empty()) + break; const int fidx = heap.top().idx; const Face f = faces[fidx]; @@ -93,7 +92,7 @@ namespace omath::collision { out.success = true; out.normal = f.n; - out.depth = f.d; // along unit normal + out.depth = f.d; // along unit normal out.iterations = it + 1; out.num_vertices = static_cast(verts.size()); out.num_faces = static_cast(faces.size()); @@ -106,11 +105,13 @@ namespace omath::collision // Mark faces visible from p and collect their horizon std::vector to_delete(faces.size(), 0); - std::vector boundary; boundary.reserve(faces.size()*2); + std::vector boundary; + boundary.reserve(faces.size() * 2); for (int i = 0; i < static_cast(faces.size()); ++i) { - if (to_delete[i]) continue; + if (to_delete[i]) + continue; if (visible_from(faces[i], p)) { const auto& rf = faces[i]; @@ -122,9 +123,11 @@ namespace omath::collision } // Remove visible faces - std::vector new_faces; new_faces.reserve(faces.size() + boundary.size()); + std::vector new_faces; + new_faces.reserve(faces.size() + boundary.size()); for (int i = 0; i < static_cast(faces.size()); ++i) - if (!to_delete[i]) new_faces.push_back(faces[i]); + if (!to_delete[i]) + new_faces.push_back(faces[i]); faces.swap(new_faces); // Stitch new faces around the horizon @@ -143,10 +146,12 @@ namespace omath::collision if (!faces.empty()) { auto best = faces[0]; - for (const auto& f : faces) if (f.d < best.d) best = f; + for (const auto& f : faces) + if (f.d < best.d) + best = f; out.success = true; out.normal = best.n; - out.depth = best.d; + out.depth = best.d; out.num_vertices = static_cast(verts.size()); out.num_faces = static_cast(faces.size()); } @@ -157,15 +162,24 @@ namespace omath::collision struct Face { int i0, i1, i2; - Vertex n; // unit outward normal - float d; // n · v0 (>=0 ideally because origin is inside) + Vertex n; // unit outward normal + float d; // n · v0 (>=0 ideally because origin is inside) }; - struct Edge { int a, b; }; + struct Edge + { + int a, b; + }; - struct HeapItem { float d; int idx; }; - struct HeapCmp { - bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept { + struct HeapItem + { + float d; + int idx; + }; + struct HeapCmp + { + bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept + { return lhs.d > rhs.d; // min-heap by distance } }; @@ -188,12 +202,12 @@ namespace omath::collision static void add_edge_boundary(std::vector& boundary, int a, int b) { // Keep edges that appear only once; erase if opposite already present - auto itb = std::find_if(boundary.begin(), boundary.end(), - [&](const Edge& e){ return e.a == b && e.b == a; }); + auto itb = + std::find_if(boundary.begin(), boundary.end(), [&](const Edge& e) { return e.a == b && e.b == a; }); if (itb != boundary.end()) - boundary.erase(itb); // internal edge cancels out + boundary.erase(itb); // internal edge cancels out else - boundary.push_back({a,b}); // horizon edge (directed) + boundary.push_back({a, b}); // horizon edge (directed) } static Face make_face(const std::vector& verts, int i0, int i1, int i2) @@ -202,20 +216,23 @@ namespace omath::collision const Vertex& a1 = verts[i1]; const Vertex& a2 = verts[i2]; Vertex n = (a1 - a0).cross(a2 - a0); - if (n.dot(n) <= 1e-30f) { + if (n.dot(n) <= 1e-30f) + { n = any_perp_vec(a1 - a0); // degenerate guard } // Ensure normal points outward (away from origin): require n·a0 >= 0 - if (n.dot(a0) < 0.0f) { std::swap(i1, i2); n = -n; } + if (n.dot(a0) < 0.0f) + { + std::swap(i1, i2); + n = -n; + } const float inv_len = 1.0f / std::sqrt(std::max(n.dot(n), 1e-30f)); n = n * inv_len; const float d = n.dot(a0); - return { i0, i1, i2, n, d }; + return {i0, i1, i2, n, d}; } - static Vertex support_point(const ColliderType& a, - const ColliderType& b, - const Vertex& dir) + static Vertex support_point(const ColliderType& a, const ColliderType& b, const Vertex& dir) { return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir); } @@ -229,9 +246,10 @@ namespace omath::collision template static constexpr V any_perp_vec(const V& v) { - for (const auto& dir : {V{1,0,0}, V{0,1,0}, V{0,0,1}}) - if (const auto d = v.cross(dir); !near_zero_vec(d)) return d; - return V{1,0,0}; + for (const auto& dir : {V{1, 0, 0}, V{0, 1, 0}, V{0, 0, 1}}) + if (const auto d = v.cross(dir); !near_zero_vec(d)) + return d; + return V{1, 0, 0}; } }; @@ -239,14 +257,17 @@ namespace omath::collision template class GjkAlgorithmWithSimplex final { - using Vertex = typename ColliderType::VertexType; + using Vertex = ColliderType::VertexType; + public: - struct Hit { bool hit{false}; Simplex simplex; }; + struct Hit + { + bool hit{false}; + Simplex simplex; + }; [[nodiscard]] - static Vertex find_support_vertex(const ColliderType& a, - const ColliderType& b, - const Vertex& dir) + static Vertex find_support_vertex(const ColliderType& a, const ColliderType& b, const Vertex& dir) { return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir); } @@ -254,21 +275,25 @@ namespace omath::collision [[nodiscard]] static Hit collide(const ColliderType& a, const ColliderType& b) { - auto support = find_support_vertex(a, b, {1,0,0}); - Simplex simplex; simplex.push_front(support); + auto support = find_support_vertex(a, b, {1, 0, 0}); + Simplex simplex; + simplex.push_front(support); auto direction = -support; for (;;) { support = find_support_vertex(a, b, direction); - if (support.dot(direction) <= 0.f) return {}; + if (support.dot(direction) <= 0.f) + return {}; simplex.push_front(support); if (simplex.handle(direction)) { - if (simplex.size() == 4) return { true, simplex }; + if (simplex.size() == 4) + return {true, simplex}; // rare degeneracy: reseed - support = find_support_vertex(a, b, {0,1,0}); - simplex.clear(); simplex.push_front(support); + support = find_support_vertex(a, b, {0, 1, 0}); + simplex.clear(); + simplex.push_front(support); direction = -support; } } From 40e26be72e3a46edd14fe65d27a6b01ad3952b87 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:06:18 +0300 Subject: [PATCH 05/13] Refactors EPA algorithm loop Replaces the `for(;;)` loop in the EPA algorithm with a `while(true)` loop for improved readability and clarity. This change enhances the maintainability of the code without altering its functionality. --- include/omath/collision/epa_algorithm.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index e6a3c0b..2c7f7e9 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -11,7 +11,8 @@ namespace omath::collision { template - concept EpaVector = requires(const V& a, const V& b, float s) { + concept EpaVector = requires(const V& a, const V& b, float s) + { { a - b } -> std::same_as; { a.cross(b) } -> std::same_as; { a.dot(b) } -> std::same_as; @@ -280,7 +281,7 @@ namespace omath::collision simplex.push_front(support); auto direction = -support; - for (;;) + while (true) { support = find_support_vertex(a, b, direction); if (support.dot(direction) <= 0.f) From 2b21caf58f9c2ad4d19e9b8b9d56bb7b07260bc8 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:07:57 +0300 Subject: [PATCH 06/13] Refactors face initialization Replaces `push_back` with `emplace_back` when initializing faces in the EPA algorithm. This avoids unnecessary copying and improves performance during face creation. --- include/omath/collision/epa_algorithm.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 2c7f7e9..df25ccb 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -58,10 +58,10 @@ namespace omath::collision // Initial tetra faces (windings corrected in make_face) std::vector faces; faces.reserve(128); - faces.push_back(make_face(verts, 0, 1, 2)); - faces.push_back(make_face(verts, 0, 2, 3)); - faces.push_back(make_face(verts, 0, 3, 1)); - faces.push_back(make_face(verts, 1, 3, 2)); + faces.emplace_back(make_face(verts, 0, 1, 2)); + faces.emplace_back(make_face(verts, 0, 2, 3)); + faces.emplace_back(make_face(verts, 0, 3, 1)); + faces.emplace_back(make_face(verts, 1, 3, 2)); auto heap = rebuild_heap(faces); From 09fd92ccad34e4afac2be9e3579220551899366d Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:15:45 +0300 Subject: [PATCH 07/13] Refactor EPA algorithm for clarity Improves code organization and readability within the EPA algorithm implementation. Changes include renaming variables for better semantic meaning (e.g., `verts` to `vertexes`), adding `final` specifiers to structs for clarity, and enhancing function signatures with `[[nodiscard]]` where appropriate. These refactorings aim to enhance maintainability and understanding of the code without altering its core functionality. --- include/omath/collision/epa_algorithm.hpp | 47 +++++++++++++---------- 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index df25ccb..0b5e806 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -11,8 +11,7 @@ namespace omath::collision { template - concept EpaVector = requires(const V& a, const V& b, float s) - { + concept EpaVector = requires(const V& a, const V& b, float s) { { a - b } -> std::same_as; { a.cross(b) } -> std::same_as; { a.dot(b) } -> std::same_as; @@ -28,7 +27,7 @@ namespace omath::collision using Vertex = typename ColliderType::VertexType; static_assert(EpaVector, "VertexType must satisfy EpaVector concept"); - struct Result + struct Result final { bool success{false}; Vertex normal{}; // outward normal (from B to A) @@ -38,7 +37,7 @@ namespace omath::collision int num_faces{0}; }; - struct Params + struct Params final { int max_iterations{64}; float tolerance{1e-4f}; // absolute tolerance on distance growth @@ -50,18 +49,18 @@ namespace omath::collision const Params params = {}) { // --- Build initial polytope from simplex (4 points) --- - std::vector verts; - verts.reserve(64); + std::vector vertexes; + vertexes.reserve(64); for (std::size_t i = 0; i < simplex.size(); ++i) - verts.push_back(simplex[i]); + vertexes.push_back(simplex[i]); // Initial tetra faces (windings corrected in make_face) std::vector faces; faces.reserve(128); - faces.emplace_back(make_face(verts, 0, 1, 2)); - faces.emplace_back(make_face(verts, 0, 2, 3)); - faces.emplace_back(make_face(verts, 0, 3, 1)); - faces.emplace_back(make_face(verts, 1, 3, 2)); + faces.emplace_back(make_face(vertexes, 0, 1, 2)); + faces.emplace_back(make_face(vertexes, 0, 2, 3)); + faces.emplace_back(make_face(vertexes, 0, 3, 1)); + faces.emplace_back(make_face(vertexes, 1, 3, 2)); auto heap = rebuild_heap(faces); @@ -95,14 +94,14 @@ namespace omath::collision out.normal = f.n; out.depth = f.d; // along unit normal out.iterations = it + 1; - out.num_vertices = static_cast(verts.size()); + out.num_vertices = static_cast(vertexes.size()); out.num_faces = static_cast(faces.size()); return out; } // Add new vertex - const int new_idx = static_cast(verts.size()); - verts.push_back(p); + const int new_idx = static_cast(vertexes.size()); + vertexes.push_back(p); // Mark faces visible from p and collect their horizon std::vector to_delete(faces.size(), 0); @@ -133,12 +132,12 @@ namespace omath::collision // Stitch new faces around the horizon for (const auto& e : boundary) - faces.push_back(make_face(verts, e.a, e.b, new_idx)); + faces.push_back(make_face(vertexes, e.a, e.b, new_idx)); // Rebuild heap after topology change heap = rebuild_heap(faces); - if (!std::isfinite(verts.back().dot(verts.back()))) + if (!std::isfinite(vertexes.back().dot(vertexes.back()))) break; // safety out.iterations = it + 1; } @@ -153,31 +152,31 @@ namespace omath::collision out.success = true; out.normal = best.n; out.depth = best.d; - out.num_vertices = static_cast(verts.size()); + out.num_vertices = static_cast(vertexes.size()); out.num_faces = static_cast(faces.size()); } return out; } private: - struct Face + struct Face final { int i0, i1, i2; Vertex n; // unit outward normal float d; // n · v0 (>=0 ideally because origin is inside) }; - struct Edge + struct Edge final { int a, b; }; - struct HeapItem + struct HeapItem final { float d; int idx; }; - struct HeapCmp + struct HeapCmp final { bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept { @@ -186,6 +185,7 @@ namespace omath::collision }; using Heap = std::priority_queue, HeapCmp>; + [[nodiscard]] static Heap rebuild_heap(const std::vector& faces) { Heap h; @@ -194,6 +194,7 @@ namespace omath::collision return h; } + [[nodiscard]] static bool visible_from(const Face& f, const Vertex& p) { // positive if p is in front of the face @@ -211,6 +212,7 @@ namespace omath::collision boundary.push_back({a, b}); // horizon edge (directed) } + [[nodiscard]] static Face make_face(const std::vector& verts, int i0, int i1, int i2) { const Vertex& a0 = verts[i0]; @@ -233,18 +235,21 @@ namespace omath::collision return {i0, i1, i2, n, d}; } + [[nodiscard]] static Vertex support_point(const ColliderType& a, const ColliderType& b, const Vertex& dir) { return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir); } template + [[nodiscard]] static constexpr bool near_zero_vec(const V& v, const float eps = 1e-7f) { return v.dot(v) <= eps * eps; } template + [[nodiscard]] static constexpr V any_perp_vec(const V& v) { for (const auto& dir : {V{1, 0, 0}, V{0, 1, 0}, V{0, 0, 1}}) From 20aecac2aef6d5a71c140624c34ca0dda5095fa1 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:16:23 +0300 Subject: [PATCH 08/13] Refactors vertex parameter name Updates the `make_face` function to use `vertexes` instead of `verts` for clarity and consistency in naming conventions. --- include/omath/collision/epa_algorithm.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 0b5e806..55a26a1 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -213,11 +213,11 @@ namespace omath::collision } [[nodiscard]] - static Face make_face(const std::vector& verts, int i0, int i1, int i2) + static Face make_face(const std::vector& vertexes, int i0, int i1, int i2) { - const Vertex& a0 = verts[i0]; - const Vertex& a1 = verts[i1]; - const Vertex& a2 = verts[i2]; + const Vertex& a0 = vertexes[i0]; + const Vertex& a1 = vertexes[i1]; + const Vertex& a2 = vertexes[i2]; Vertex n = (a1 - a0).cross(a2 - a0); if (n.dot(n) <= 1e-30f) { From 873bdd203658227c95c5baf48437beedf3597d4b Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:34:14 +0300 Subject: [PATCH 09/13] Improves EPA test coverage Adds a comprehensive unit test for the EPA algorithm, validating collision detection, depth calculation, and separation axis determination with a cube overlap scenario. Refactors the test for clarity and robustness. --- tests/general/unit_test_epa.cpp | 122 ++++++++++++++++++-------------- 1 file changed, 70 insertions(+), 52 deletions(-) diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp index 57da26c..3660d09 100644 --- a/tests/general/unit_test_epa.cpp +++ b/tests/general/unit_test_epa.cpp @@ -1,57 +1,75 @@ -// -// Created by Vlad on 11/13/2025. -// -#include "omath/collision/gjk_algorithm.hpp" -#include "omath/engines/source_engine/collider.hpp" #include -#include -#include +#include "omath/linear_algebra/vector3.hpp" +#include "omath/collision/simplex.hpp" +#include "omath/collision/epa_algorithm.hpp" // Epa + GjkAlgorithmWithSimplex +#include "omath/engines/source_engine/mesh.hpp" +#include "omath/engines/source_engine/collider.hpp" + +using Mesh = omath::source_engine::Mesh; +using Collider = omath::source_engine::MeshCollider; +using GJK = omath::collision::GjkAlgorithmWithSimplex; +using EPA = omath::collision::Epa; -namespace -{ - const omath::source_engine::Mesh mesh = {{{-1.f, -1.f, -1.f}, - {-1.f, -1.f, 1.f}, - {-1.f, 1.f, -1.f}, - {-1.f, 1.f, 1.f}, - {1.f, 1.f, 1.f}, - {1.f, 1.f, -1.f}, - {1.f, -1.f, 1.f}, - {1.f, -1.f, -1.f}}, - {}}; -} TEST(UnitTestEpa, TestCollisionTrue) { - std::vector> vbo = {{-1, -1, -1}, {-1, -1, 1}, {-1, 1, -1}, {-1, 1, 1}, - {1, 1, 1}, {1, 1, -1}, {1, -1, 1}, {1, -1, -1}}; - std::vector> vao; // not needed for GJK/EPA - - omath::source_engine::Mesh a(vbo, vao, {1, 1, 1}); - omath::source_engine::Mesh b(vbo, vao, {1, 1, 1}); - - a.set_origin({0, 0, 0}); - b.set_origin({0.5f, 0, 0}); // slight overlap - - const omath::source_engine::MeshCollider collider_a(mesh); - - omath::source_engine::MeshCollider A(a), B(b); - - // 1) GJK → final simplex - using Gjk = omath::collision::GjkAlgorithm; - - auto gjk = Gjk::is_collide_with_simplex_info(A, B); - if (!gjk.hit) - { - std::cout << "No collision\n"; - } - using Epa = omath::collision::Epa; - // 2) EPA → normal/depth - Epa::Params params; - params.max_iterations = 64; - params.tolerance = 1e-4f; - auto epa = Epa::solve(A, B, gjk.simplex, params); - - if (!epa.success) - { - std::cout << "EPA failed\n"; - } + // Unit cube [-1,1]^3 + std::vector> vbo = { + {-1,-1,-1}, {-1,-1, 1}, {-1, 1,-1}, {-1, 1, 1}, + { 1, 1, 1}, { 1, 1,-1}, { 1,-1, 1}, { 1,-1,-1} + }; + std::vector> vao; // not needed + + Mesh a(vbo, vao, {1,1,1}); + Mesh b(vbo, vao, {1,1,1}); + + // Overlap along +X by 0.5 + a.set_origin({0,0,0}); + b.set_origin({0.5f,0,0}); + + Collider A(a), B(b); + + // GJK + auto gjk = GJK::collide(A, B); + ASSERT_TRUE(gjk.hit) << "GJK should report collision"; + + // EPA + EPA::Params params; params.max_iterations = 64; params.tolerance = 1e-4f; + auto epa = EPA::solve(A, B, gjk.simplex, params); + ASSERT_TRUE(epa.success) << "EPA should converge"; + + // Normal is unit + EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f); + + // For this setup, depth ≈ 1.5 (2 - 0.5) + EXPECT_NEAR(epa.depth, 1.5f, 1e-3f); + + // Normal axis sanity: near X axis + EXPECT_NEAR(std::abs(epa.normal.x), 1.0f, 1e-3f); + EXPECT_NEAR(epa.normal.y, 0.0f, 1e-3f); + EXPECT_NEAR(epa.normal.z, 0.0f, 1e-3f); + + // Try both signs with a tiny margin (avoid grazing contacts) + const float margin = 1.0f + 1e-3f; + const auto pen = epa.normal * epa.depth; + + Mesh b_plus = b; b_plus.set_origin(b_plus.get_origin() + pen * margin); + Mesh b_minus= b; b_minus.set_origin(b_minus.get_origin() - pen * margin); + + Collider B_plus(b_plus), B_minus(b_minus); + + const bool sep_plus = !GJK::collide(A, B_plus).hit; + const bool sep_minus = !GJK::collide(A, B_minus).hit; + + // Exactly one direction should separate + EXPECT_NE(sep_plus, sep_minus) << "Exactly one of ±penetration must separate"; + + // Optional: pick the resolving direction and assert round-trip + const auto resolve = sep_plus ? ( pen * margin) : (-pen * margin); + + Mesh b_resolved = b; b_resolved.set_origin(b_resolved.get_origin() + resolve); + EXPECT_FALSE(GJK::collide(A, Collider(b_resolved)).hit) << "Resolved position should be non-colliding"; + + // Moving the other way should still collide + Mesh b_wrong = b; b_wrong.set_origin(b_wrong.get_origin() - resolve); + EXPECT_TRUE(GJK::collide(A, Collider(b_wrong)).hit); } \ No newline at end of file From fa91f21e39efb69c80d08d7bc817f2a50273d0f4 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:39:26 +0300 Subject: [PATCH 10/13] Refactors collision detection with GJK and EPA This commit refactors the collision detection pipeline to utilize a more standard GJK algorithm and simplifies the EPA implementation. Removes the custom `GjkAlgorithmWithSimplex` in favor of the standalone `GjkAlgorithm`. This streamlines the collision detection process and enhances code clarity. Updates unit tests to align with the new GJK implementation, ensuring continued functionality and correctness. --- include/omath/collision/epa_algorithm.hpp | 47 ----------------------- tests/general/unit_test_epa.cpp | 21 +++++----- 2 files changed, 11 insertions(+), 57 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 55a26a1..1b51396 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -258,51 +258,4 @@ namespace omath::collision return V{1, 0, 0}; } }; - - // Optional: the GJK that returns a simplex for EPA (unchanged) - template - class GjkAlgorithmWithSimplex final - { - using Vertex = ColliderType::VertexType; - - public: - struct Hit - { - bool hit{false}; - Simplex simplex; - }; - - [[nodiscard]] - static Vertex find_support_vertex(const ColliderType& a, const ColliderType& b, const Vertex& dir) - { - return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir); - } - - [[nodiscard]] - static Hit collide(const ColliderType& a, const ColliderType& b) - { - auto support = find_support_vertex(a, b, {1, 0, 0}); - Simplex simplex; - simplex.push_front(support); - auto direction = -support; - - while (true) - { - support = find_support_vertex(a, b, direction); - if (support.dot(direction) <= 0.f) - return {}; - simplex.push_front(support); - if (simplex.handle(direction)) - { - if (simplex.size() == 4) - return {true, simplex}; - // rare degeneracy: reseed - support = find_support_vertex(a, b, {0, 1, 0}); - simplex.clear(); - simplex.push_front(support); - direction = -support; - } - } - } - }; } // namespace omath::collision diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp index 3660d09..d79a8d9 100644 --- a/tests/general/unit_test_epa.cpp +++ b/tests/general/unit_test_epa.cpp @@ -1,13 +1,14 @@ -#include -#include "omath/linear_algebra/vector3.hpp" +#include "omath/collision/epa_algorithm.hpp" // Epa + GjkAlgorithmWithSimplex +#include "omath/collision/gjk_algorithm.hpp" #include "omath/collision/simplex.hpp" -#include "omath/collision/epa_algorithm.hpp" // Epa + GjkAlgorithmWithSimplex -#include "omath/engines/source_engine/mesh.hpp" #include "omath/engines/source_engine/collider.hpp" +#include "omath/engines/source_engine/mesh.hpp" +#include "omath/linear_algebra/vector3.hpp" +#include using Mesh = omath::source_engine::Mesh; using Collider = omath::source_engine::MeshCollider; -using GJK = omath::collision::GjkAlgorithmWithSimplex; +using GJK = omath::collision::GjkAlgorithm; using EPA = omath::collision::Epa; TEST(UnitTestEpa, TestCollisionTrue) @@ -29,7 +30,7 @@ TEST(UnitTestEpa, TestCollisionTrue) Collider A(a), B(b); // GJK - auto gjk = GJK::collide(A, B); + auto gjk = GJK::is_collide_with_simplex_info(A, B); ASSERT_TRUE(gjk.hit) << "GJK should report collision"; // EPA @@ -57,8 +58,8 @@ TEST(UnitTestEpa, TestCollisionTrue) Collider B_plus(b_plus), B_minus(b_minus); - const bool sep_plus = !GJK::collide(A, B_plus).hit; - const bool sep_minus = !GJK::collide(A, B_minus).hit; + const bool sep_plus = !GJK::is_collide_with_simplex_info(A, B_plus).hit; + const bool sep_minus = !GJK::is_collide_with_simplex_info(A, B_minus).hit; // Exactly one direction should separate EXPECT_NE(sep_plus, sep_minus) << "Exactly one of ±penetration must separate"; @@ -67,9 +68,9 @@ TEST(UnitTestEpa, TestCollisionTrue) const auto resolve = sep_plus ? ( pen * margin) : (-pen * margin); Mesh b_resolved = b; b_resolved.set_origin(b_resolved.get_origin() + resolve); - EXPECT_FALSE(GJK::collide(A, Collider(b_resolved)).hit) << "Resolved position should be non-colliding"; + EXPECT_FALSE(GJK::is_collide(A, Collider(b_resolved))) << "Resolved position should be non-colliding"; // Moving the other way should still collide Mesh b_wrong = b; b_wrong.set_origin(b_wrong.get_origin() - resolve); - EXPECT_TRUE(GJK::collide(A, Collider(b_wrong)).hit); + EXPECT_TRUE(GJK::is_collide(A, Collider(b_wrong))); } \ No newline at end of file From ee458a24f7cc6bff3ec94d00b97aea5ba359e8f2 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 16:56:48 +0300 Subject: [PATCH 11/13] fixed formating --- tests/general/unit_test_epa.cpp | 40 ++++++++++++++++++--------------- 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp index d79a8d9..f9d9f8d 100644 --- a/tests/general/unit_test_epa.cpp +++ b/tests/general/unit_test_epa.cpp @@ -8,24 +8,22 @@ using Mesh = omath::source_engine::Mesh; using Collider = omath::source_engine::MeshCollider; -using GJK = omath::collision::GjkAlgorithm; -using EPA = omath::collision::Epa; +using GJK = omath::collision::GjkAlgorithm; +using EPA = omath::collision::Epa; TEST(UnitTestEpa, TestCollisionTrue) { // Unit cube [-1,1]^3 - std::vector> vbo = { - {-1,-1,-1}, {-1,-1, 1}, {-1, 1,-1}, {-1, 1, 1}, - { 1, 1, 1}, { 1, 1,-1}, { 1,-1, 1}, { 1,-1,-1} - }; + std::vector> vbo = {{-1, -1, -1}, {-1, -1, 1}, {-1, 1, -1}, {-1, 1, 1}, + {1, 1, 1}, {1, 1, -1}, {1, -1, 1}, {1, -1, -1}}; std::vector> vao; // not needed - Mesh a(vbo, vao, {1,1,1}); - Mesh b(vbo, vao, {1,1,1}); + Mesh a(vbo, vao, {1, 1, 1}); + Mesh b(vbo, vao, {1, 1, 1}); // Overlap along +X by 0.5 - a.set_origin({0,0,0}); - b.set_origin({0.5f,0,0}); + a.set_origin({0, 0, 0}); + b.set_origin({0.5f, 0, 0}); Collider A(a), B(b); @@ -34,7 +32,9 @@ TEST(UnitTestEpa, TestCollisionTrue) ASSERT_TRUE(gjk.hit) << "GJK should report collision"; // EPA - EPA::Params params; params.max_iterations = 64; params.tolerance = 1e-4f; + EPA::Params params; + params.max_iterations = 64; + params.tolerance = 1e-4f; auto epa = EPA::solve(A, B, gjk.simplex, params); ASSERT_TRUE(epa.success) << "EPA should converge"; @@ -51,26 +51,30 @@ TEST(UnitTestEpa, TestCollisionTrue) // Try both signs with a tiny margin (avoid grazing contacts) const float margin = 1.0f + 1e-3f; - const auto pen = epa.normal * epa.depth; + const auto pen = epa.normal * epa.depth; - Mesh b_plus = b; b_plus.set_origin(b_plus.get_origin() + pen * margin); - Mesh b_minus= b; b_minus.set_origin(b_minus.get_origin() - pen * margin); + Mesh b_plus = b; + b_plus.set_origin(b_plus.get_origin() + pen * margin); + Mesh b_minus = b; + b_minus.set_origin(b_minus.get_origin() - pen * margin); Collider B_plus(b_plus), B_minus(b_minus); - const bool sep_plus = !GJK::is_collide_with_simplex_info(A, B_plus).hit; + const bool sep_plus = !GJK::is_collide_with_simplex_info(A, B_plus).hit; const bool sep_minus = !GJK::is_collide_with_simplex_info(A, B_minus).hit; // Exactly one direction should separate EXPECT_NE(sep_plus, sep_minus) << "Exactly one of ±penetration must separate"; // Optional: pick the resolving direction and assert round-trip - const auto resolve = sep_plus ? ( pen * margin) : (-pen * margin); + const auto resolve = sep_plus ? (pen * margin) : (-pen * margin); - Mesh b_resolved = b; b_resolved.set_origin(b_resolved.get_origin() + resolve); + Mesh b_resolved = b; + b_resolved.set_origin(b_resolved.get_origin() + resolve); EXPECT_FALSE(GJK::is_collide(A, Collider(b_resolved))) << "Resolved position should be non-colliding"; // Moving the other way should still collide - Mesh b_wrong = b; b_wrong.set_origin(b_wrong.get_origin() - resolve); + Mesh b_wrong = b; + b_wrong.set_origin(b_wrong.get_origin() - resolve); EXPECT_TRUE(GJK::is_collide(A, Collider(b_wrong))); } \ No newline at end of file From 88d4447b2041235ace36291b833cbd0af4a1624f Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 17:22:54 +0300 Subject: [PATCH 12/13] add another test --- tests/general/unit_test_epa.cpp | 61 ++++++++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp index f9d9f8d..adc468a 100644 --- a/tests/general/unit_test_epa.cpp +++ b/tests/general/unit_test_epa.cpp @@ -77,4 +77,63 @@ TEST(UnitTestEpa, TestCollisionTrue) Mesh b_wrong = b; b_wrong.set_origin(b_wrong.get_origin() - resolve); EXPECT_TRUE(GJK::is_collide(A, Collider(b_wrong))); -} \ No newline at end of file +} +TEST(UnitTestEpa, TestCollisionTrue2) +{ + std::vector> vbo = {{-1, -1, -1}, {-1, -1, 1}, {-1, 1, -1}, {-1, 1, 1}, + {1, 1, 1}, {1, 1, -1}, {1, -1, 1}, {1, -1, -1}}; + std::vector> vao; // not needed + + Mesh a(vbo, vao, {1, 1, 1}); + Mesh b(vbo, vao, {1, 1, 1}); + + // Overlap along +X by 0.5 + a.set_origin({0, 0, 0}); + b.set_origin({0.5f, 0, 0}); + + Collider A(a), B(b); + + // --- GJK must detect collision and provide simplex --- + auto gjk = GJK::is_collide_with_simplex_info(A, B); + ASSERT_TRUE(gjk.hit) << "GJK should report collision for overlapping cubes"; + + // --- EPA penetration --- + EPA::Params params; + params.max_iterations = 64; + params.tolerance = 1e-4f; + auto epa = EPA::solve(A, B, gjk.simplex, params); + ASSERT_TRUE(epa.success) << "EPA should converge"; + + // Normal is unit-length + EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f); + + // For centers at 0 and +0.5 and half-extent 1 -> depth ≈ 1.5 + EXPECT_NEAR(epa.depth, 1.5f, 1e-3f); + + // Axis sanity: mostly X + EXPECT_NEAR(std::abs(epa.normal.x), 1.0f, 1e-3f); + EXPECT_NEAR(epa.normal.y, 0.0f, 1e-3f); + EXPECT_NEAR(epa.normal.z, 0.0f, 1e-3f); + + // Choose a deterministic sign: orient penetration from A toward B + const auto centers = b.get_origin() - a.get_origin(); // (0.5, 0, 0) + float sign = (epa.normal.dot(centers) >= 0.0f) ? +1.0f : -1.0f; + + constexpr float margin = 1.0f + 1e-3f; // tiny slack to avoid grazing + const auto pen = epa.normal * epa.depth * sign; + + // Apply once: B + pen must separate; the opposite must still collide + Mesh b_resolved = b; + b_resolved.set_origin(b_resolved.get_origin() + pen * margin); + EXPECT_FALSE(GJK::is_collide(A, Collider(b_resolved))) << "Applying penetration should separate"; + + Mesh b_wrong = b; + b_wrong.set_origin(b_wrong.get_origin() - pen * margin); + EXPECT_TRUE(GJK::is_collide(A, Collider(b_wrong))) << "Opposite direction should still intersect"; + + // Some book-keeping sanity + EXPECT_GT(epa.iterations, 0); + EXPECT_LT(epa.iterations, params.max_iterations); + EXPECT_GE(epa.num_faces, 4); + EXPECT_GT(epa.num_vertices, 4); +} From 798caa2b0d145bfd30400bc05cad075485f213aa Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 17:34:39 +0300 Subject: [PATCH 13/13] made ref --- include/omath/3d_primitives/mesh.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/omath/3d_primitives/mesh.hpp b/include/omath/3d_primitives/mesh.hpp index cc09a5b..3779776 100644 --- a/include/omath/3d_primitives/mesh.hpp +++ b/include/omath/3d_primitives/mesh.hpp @@ -25,7 +25,7 @@ namespace omath::primitives Vao m_vertex_array_object; Mesh(Vbo vbo, Vao vao, const Vector3 scale = {1, 1, 1,}) - : m_vertex_buffer(std::move(vbo)), m_vertex_array_object(std::move(vao)), m_scale(scale) + : m_vertex_buffer(std::move(vbo)), m_vertex_array_object(std::move(vao)), m_scale(std::move(scale)) { } void set_origin(const Vector3& new_origin)