From ea2c7c3d7f630e46e5ea2dc5d0b895b342d09927 Mon Sep 17 00:00:00 2001 From: orange Date: Mon, 2 Mar 2026 19:40:37 +0300 Subject: [PATCH 01/10] added benchmark --- benchmark/benchmark_collision.cpp | 161 ++++++++++++++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 benchmark/benchmark_collision.cpp diff --git a/benchmark/benchmark_collision.cpp b/benchmark/benchmark_collision.cpp new file mode 100644 index 00000000..23ed4058 --- /dev/null +++ b/benchmark/benchmark_collision.cpp @@ -0,0 +1,161 @@ +// +// Created by Vlad on 3/2/2026. +// +#include +#include +#include +#include +#include +#include + +using Mesh = omath::source_engine::Mesh; +using Collider = omath::source_engine::MeshCollider; +using Gjk = omath::collision::GjkAlgorithm; +using Epa = omath::collision::Epa; + +namespace +{ + // Unit cube with half-extent 1 — 8 vertices in [-1,1]^3. + const std::vector> k_cube_vbo = { + { { -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 }, {}, {} }, + }; + const std::vector> k_empty_vao{}; +} // namespace + +// --------------------------------------------------------------------------- +// GJK benchmarks +// --------------------------------------------------------------------------- + +// Separated cubes — origin distance 2.1, no overlap. +// Exercises the early-exit path and the centroid-based initial direction. +static void BM_Gjk_Separated(benchmark::State& state) +{ + const Collider a{Mesh{k_cube_vbo, k_empty_vao}}; + + Mesh mesh_b{k_cube_vbo, k_empty_vao}; + mesh_b.set_origin({0.f, 2.1f, 0.f}); + const Collider b{mesh_b}; + + for ([[maybe_unused]] auto _ : state) + benchmark::DoNotOptimize(Gjk::is_collide(a, b)); +} + +// Overlapping cubes — B offset by 0.5 along X, ~1.5 units penetration depth. +static void BM_Gjk_Overlapping(benchmark::State& state) +{ + const Collider a{Mesh{k_cube_vbo, k_empty_vao}}; + + Mesh mesh_b{k_cube_vbo, k_empty_vao}; + mesh_b.set_origin({0.5f, 0.f, 0.f}); + const Collider b{mesh_b}; + + for ([[maybe_unused]] auto _ : state) + benchmark::DoNotOptimize(Gjk::is_collide(a, b)); +} + +// Identical cubes at the same origin — deep overlap / worst case for GJK. +static void BM_Gjk_SameOrigin(benchmark::State& state) +{ + const Collider a{Mesh{k_cube_vbo, k_empty_vao}}; + const Collider b{Mesh{k_cube_vbo, k_empty_vao}}; + + for ([[maybe_unused]] auto _ : state) + benchmark::DoNotOptimize(Gjk::is_collide(a, b)); +} + +// --------------------------------------------------------------------------- +// EPA benchmarks +// --------------------------------------------------------------------------- + +// EPA with a pre-allocated monotonic buffer (reset each iteration). +// Isolates algorithmic cost from allocator overhead. +static void BM_Epa_MonotonicBuffer(benchmark::State& state) +{ + const Collider a{Mesh{k_cube_vbo, k_empty_vao}}; + + Mesh mesh_b{k_cube_vbo, k_empty_vao}; + mesh_b.set_origin({0.5f, 0.f, 0.f}); + const Collider b{mesh_b}; + + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + if (!hit) + return; // shouldn't happen, but guard for safety + + constexpr Epa::Params params{.max_iterations = 64, .tolerance = 1e-4f}; + + // Pre-allocate a 32 KiB stack buffer — enough for typical polytope growth. + constexpr std::size_t k_buf_size = 32768; + alignas(std::max_align_t) char buf[k_buf_size]; + std::pmr::monotonic_buffer_resource mr{buf, k_buf_size, std::pmr::null_memory_resource()}; + + for ([[maybe_unused]] auto _ : state) + { + mr.release(); // reset the buffer without touching the upstream resource + benchmark::DoNotOptimize(Epa::solve(a, b, simplex, params, mr)); + } +} + +// EPA with the default (malloc-backed) memory resource. +// Shows total cost including allocator pressure. +static void BM_Epa_DefaultResource(benchmark::State& state) +{ + const Collider a{Mesh{k_cube_vbo, k_empty_vao}}; + + Mesh mesh_b{k_cube_vbo, k_empty_vao}; + mesh_b.set_origin({0.5f, 0.f, 0.f}); + const Collider b{mesh_b}; + + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + if (!hit) + return; + + constexpr Epa::Params params{.max_iterations = 64, .tolerance = 1e-4f}; + + for ([[maybe_unused]] auto _ : state) + benchmark::DoNotOptimize(Epa::solve(a, b, simplex, params)); +} + +// --------------------------------------------------------------------------- +// Combined GJK + EPA pipeline +// --------------------------------------------------------------------------- + +// Full collision pipeline: GJK detects contact, EPA resolves penetration. +// This is the hot path in a physics engine tick. +static void BM_GjkEpa_Pipeline(benchmark::State& state) +{ + const Collider a{Mesh{k_cube_vbo, k_empty_vao}}; + + Mesh mesh_b{k_cube_vbo, k_empty_vao}; + mesh_b.set_origin({0.5f, 0.f, 0.f}); + const Collider b{mesh_b}; + + constexpr Epa::Params params{.max_iterations = 64, .tolerance = 1e-4f}; + + constexpr std::size_t k_buf_size = 32768; + alignas(std::max_align_t) char buf[k_buf_size]; + std::pmr::monotonic_buffer_resource mr{buf, k_buf_size, std::pmr::null_memory_resource()}; + + for ([[maybe_unused]] auto _ : state) + { + mr.release(); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + if (hit) + benchmark::DoNotOptimize(Epa::solve(a, b, simplex, params, mr)); + } +} + +BENCHMARK(BM_Gjk_Separated)->Iterations(100'000); +BENCHMARK(BM_Gjk_Overlapping)->Iterations(100'000); +BENCHMARK(BM_Gjk_SameOrigin)->Iterations(100'000); + +BENCHMARK(BM_Epa_MonotonicBuffer)->Iterations(100'000); +BENCHMARK(BM_Epa_DefaultResource)->Iterations(100'000); + +BENCHMARK(BM_GjkEpa_Pipeline)->Iterations(100'000); From a79ad6948c026205130494db3221aa21d6d3ecfb Mon Sep 17 00:00:00 2001 From: orange Date: Mon, 2 Mar 2026 19:40:45 +0300 Subject: [PATCH 02/10] optimized --- include/omath/collision/epa_algorithm.hpp | 144 +++++++++------------- include/omath/collision/gjk_algorithm.hpp | 15 ++- 2 files changed, 72 insertions(+), 87 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 31599c05..7d461888 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -50,89 +50,102 @@ namespace omath::collision int max_iterations{64}; FloatingType tolerance{1e-4}; // absolute tolerance on distance growth }; + // Precondition: simplex.size()==4 and contains the origin. [[nodiscard]] static std::optional solve(const ColliderInterfaceType& a, const ColliderInterfaceType& b, const Simplex& simplex, const Params params = {}, std::pmr::memory_resource& mem_resource = *std::pmr::get_default_resource()) { - // --- Build initial polytope from simplex (4 points) --- std::pmr::vector vertexes = build_initial_polytope_from_simplex(simplex, mem_resource); - - // Initial tetra faces (windings corrected in make_face) std::pmr::vector faces = create_initial_tetra_faces(mem_resource, vertexes); - auto heap = rebuild_heap(faces, mem_resource); + // Build initial min-heap by distance. + Heap heap = rebuild_heap(faces, mem_resource); Result out{}; + // Hoisted outside the loop to reuse the allocation across iterations. + std::pmr::vector boundary{&mem_resource}; + boundary.reserve(16); + 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.) - - if (const auto top = heap.top(); faces[top.idx].d != top.d) - heap = rebuild_heap(faces, mem_resource); + // Lazily discard stale (deleted or index-mismatched) heap entries. + while (!heap.empty()) + { + const auto& top = heap.top(); + if (!faces[top.idx].deleted && faces[top.idx].d == top.d) + break; + heap.pop(); + } if (heap.empty()) break; - // FIXME: STORE REF VALUE, DO NOT USE - // AFTER IF STATEMENT BLOCK const Face& face = faces[heap.top().idx]; - // Get the furthest point in face normal direction const VectorType p = support_point(a, b, face.n); const auto p_dist = face.n.dot(p); - // Converged if we can’t push the face closer than tolerance + // Converged: new support can't push the face closer than tolerance. if (p_dist - face.d <= params.tolerance) { out.normal = face.n; - out.depth = face.d; // along unit normal + out.depth = face.d; out.iterations = it + 1; out.num_vertices = static_cast(vertexes.size()); out.num_faces = static_cast(faces.size()); - out.penetration_vector = out.normal * out.depth; return out; } - // Add new vertex const int new_idx = static_cast(vertexes.size()); vertexes.emplace_back(p); - const auto [to_delete, boundary] = mark_visible_and_collect_horizon(faces, p); - - erase_marked(faces, to_delete); + // Tombstone visible faces and collect the horizon boundary. + // This avoids copying the faces array (O(n)) each iteration. + boundary.clear(); + for (auto& f : faces) + { + if (!f.deleted && visible_from(f, p)) + { + f.deleted = true; + add_edge_boundary(boundary, f.i0, f.i1); + add_edge_boundary(boundary, f.i1, f.i2); + add_edge_boundary(boundary, f.i2, f.i0); + } + } - // Stitch new faces around the horizon + // Stitch new faces around the horizon and push them directly onto the + // heap — no full O(n log n) rebuild needed. for (const auto& e : boundary) + { + const int fi = static_cast(faces.size()); faces.emplace_back(make_face(vertexes, e.a, e.b, new_idx)); - - // Rebuild heap after topology change - heap = rebuild_heap(faces, mem_resource); + heap.emplace(faces.back().d, fi); + } if (!std::isfinite(vertexes.back().dot(vertexes.back()))) break; // safety + out.iterations = it + 1; } - if (faces.empty()) + // Find the best surviving (non-deleted) face. + const Face* best = nullptr; + for (const auto& f : faces) + if (!f.deleted && (best == nullptr || f.d < best->d)) + best = &f; + + if (!best) return std::nullopt; - const auto best = *std::ranges::min_element(faces, [](const auto& first, const auto& second) - { return first.d < second.d; }); - out.normal = best.n; - out.depth = best.d; + out.normal = best->n; + out.depth = best->d; out.num_vertices = static_cast(vertexes.size()); out.num_faces = static_cast(faces.size()); - out.penetration_vector = out.normal * out.depth; - return out; } @@ -140,8 +153,9 @@ namespace omath::collision struct Face final { int i0, i1, i2; - VectorType n; // unit outward normal - FloatingType d; // n · v0 (>=0 ideally because origin is inside) + VectorType n; // unit outward normal + FloatingType d; // n · v0 (>= 0 ideally because origin is inside) + bool deleted{false}; // tombstone flag — avoids O(n) compaction per iteration }; struct Edge final @@ -154,6 +168,7 @@ namespace omath::collision FloatingType d; int idx; }; + struct HeapCmp final { [[nodiscard]] @@ -169,31 +184,28 @@ namespace omath::collision static Heap rebuild_heap(const std::pmr::vector& faces, auto& memory_resource) { std::pmr::vector storage{&memory_resource}; - storage.reserve(faces.size()); // optional but recommended - + storage.reserve(faces.size()); Heap h{HeapCmp{}, std::move(storage)}; - for (int i = 0; i < static_cast(faces.size()); ++i) - h.emplace(faces[i].d, i); - - return h; // allocator is preserved + if (!faces[i].deleted) + h.emplace(faces[i].d, i); + return h; } [[nodiscard]] static bool visible_from(const Face& f, const VectorType& p) { - // positive if p is in front of the face return f.n.dot(p) - f.d > static_cast(1e-7); } static void add_edge_boundary(std::pmr::vector& boundary, int a, int b) { - // Keep edges that appear only once; erase if opposite already present + // Keep edges that appear only once; cancel if opposite already present. auto itb = std::ranges::find_if(boundary, [&](const Edge& e) { return e.a == b && e.b == a; }); if (itb != boundary.end()) - boundary.erase(itb); // internal edge cancels out + boundary.erase(itb); else - boundary.emplace_back(a, b); // horizon edge (directed) + boundary.emplace_back(a, b); } [[nodiscard]] @@ -204,9 +216,7 @@ namespace omath::collision const VectorType& a2 = vertexes[i2]; VectorType n = (a1 - a0).cross(a2 - a0); if (n.dot(n) <= static_cast(1e-30)) - { n = any_perp_vec(a1 - a0); // degenerate guard - } // Ensure normal points outward (away from origin): require n·a0 >= 0 if (n.dot(a0) < static_cast(0.0)) { @@ -243,6 +253,7 @@ namespace omath::collision return d; return V{1, 0, 0}; } + [[nodiscard]] static std::pmr::vector create_initial_tetra_faces(std::pmr::memory_resource& mem_resource, const std::pmr::vector& vertexes) @@ -262,48 +273,9 @@ namespace omath::collision { std::pmr::vector vertexes{&mem_resource}; vertexes.reserve(simplex.size()); - for (std::size_t i = 0; i < simplex.size(); ++i) vertexes.emplace_back(simplex[i]); - return vertexes; } - static void erase_marked(std::pmr::vector& faces, const std::pmr::vector& to_delete) - { - auto* mr = faces.get_allocator().resource(); // keep same resource - std::pmr::vector kept{mr}; - kept.reserve(faces.size()); - - for (std::size_t i = 0; i < faces.size(); ++i) - if (!to_delete[i]) - kept.emplace_back(faces[i]); - - faces.swap(kept); - } - struct Horizon - { - std::pmr::vector to_delete; - std::pmr::vector boundary; - }; - - static Horizon mark_visible_and_collect_horizon(const std::pmr::vector& faces, const VectorType& p) - { - auto* mr = faces.get_allocator().resource(); - - Horizon horizon{std::pmr::vector(faces.size(), false, mr), std::pmr::vector(mr)}; - horizon.boundary.reserve(faces.size()); - - for (std::size_t i = 0; i < faces.size(); ++i) - if (visible_from(faces[i], p)) - { - const auto& rf = faces[i]; - horizon.to_delete[i] = true; - add_edge_boundary(horizon.boundary, rf.i0, rf.i1); - add_edge_boundary(horizon.boundary, rf.i1, rf.i2); - add_edge_boundary(horizon.boundary, rf.i2, rf.i0); - } - - return horizon; - } }; } // namespace omath::collision diff --git a/include/omath/collision/gjk_algorithm.hpp b/include/omath/collision/gjk_algorithm.hpp index 59073d16..af7470e5 100644 --- a/include/omath/collision/gjk_algorithm.hpp +++ b/include/omath/collision/gjk_algorithm.hpp @@ -43,7 +43,20 @@ namespace omath::collision const ColliderInterfaceType& collider_b, const GjkSettings& settings = {}) { - auto support = find_support_vertex(collider_a, collider_b, VectorType{1, 0, 0}); + // Use centroid difference as initial direction — greatly reduces iterations for separated shapes. + VectorType initial_dir; + if constexpr (requires { collider_b.get_origin() - collider_a.get_origin(); }) + { + initial_dir = collider_b.get_origin() - collider_a.get_origin(); + if (initial_dir.dot(initial_dir) < settings.epsilon * settings.epsilon) + initial_dir = VectorType{1, 0, 0}; + } + else + { + initial_dir = VectorType{1, 0, 0}; + } + + auto support = find_support_vertex(collider_a, collider_b, initial_dir); Simplex simplex; simplex.push_front(support); From 414b2af289c82539c658ff22ccfc3b6f4bcae8e9 Mon Sep 17 00:00:00 2001 From: orange Date: Mon, 2 Mar 2026 19:58:31 +0300 Subject: [PATCH 03/10] added gjk tests --- tests/general/unit_test_gjk_comprehensive.cpp | 277 ++++++++++++++++++ 1 file changed, 277 insertions(+) create mode 100644 tests/general/unit_test_gjk_comprehensive.cpp diff --git a/tests/general/unit_test_gjk_comprehensive.cpp b/tests/general/unit_test_gjk_comprehensive.cpp new file mode 100644 index 00000000..46f07b86 --- /dev/null +++ b/tests/general/unit_test_gjk_comprehensive.cpp @@ -0,0 +1,277 @@ +// +// Comprehensive GJK tests. +// Covers: all 6 axis directions, diagonal cases, boundary touching, +// asymmetric sizes, nesting, symmetry, simplex info, far separation. +// +#include +#include +#include +#include + +using Mesh = omath::source_engine::Mesh; +using Collider = omath::source_engine::MeshCollider; +using Gjk = omath::collision::GjkAlgorithm; +using Vec3 = omath::Vector3; + +namespace +{ + // Unit cube [-1, 1]^3 in local space. + const std::vector> k_cube_vbo = { + { { -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 }, {}, {} }, + }; + const std::vector> k_empty_ebo{}; + + Collider make_cube(const Vec3& origin = {}, const Vec3& scale = { 1, 1, 1 }) + { + Mesh m{ k_cube_vbo, k_empty_ebo, scale }; + m.set_origin(origin); + return Collider{ m }; + } +} // namespace + +// --------------------------------------------------------------------------- +// Separation — expect false +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, Separated_AlongPosX) +{ + // A extends to x=1, B starts at x=1.1 → clear gap + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 2.1f, 0, 0 }))); +} + +TEST(GjkComprehensive, Separated_AlongNegX) +{ + // B to the left of A + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ -2.1f, 0, 0 }))); +} + +TEST(GjkComprehensive, Separated_AlongPosY) +{ + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 0, 2.1f, 0 }))); +} + +TEST(GjkComprehensive, Separated_AlongNegY) +{ + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 0, -2.1f, 0 }))); +} + +TEST(GjkComprehensive, Separated_AlongPosZ) +{ + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 2.1f }))); +} + +TEST(GjkComprehensive, Separated_AlongNegZ) +{ + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, -2.1f }))); +} + +TEST(GjkComprehensive, Separated_AlongDiagonal) +{ + // All components exceed 2.0 — no overlap on any axis + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 2.1f, 2.1f, 2.1f }))); +} + +TEST(GjkComprehensive, Separated_LargeDistance) +{ + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 100.f, 0, 0 }))); +} + +TEST(GjkComprehensive, Separated_AsymmetricSizes) +{ + // Big (scale 2, half-ext 2), small (scale 0.5, half-ext 0.5) at 2.6 → gap of 0.1 + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }, { 2, 2, 2 }), make_cube({ 2.6f, 0, 0 }, { 0.5f, 0.5f, 0.5f }))); +} + +// --------------------------------------------------------------------------- +// Overlap — expect true +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, Overlapping_AlongPosX) +{ + // B offset 1.5 → overlap depth 0.5 in X + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 1.5f, 0, 0 }))); +} + +TEST(GjkComprehensive, Overlapping_AlongNegX) +{ + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ -1.5f, 0, 0 }))); +} + +TEST(GjkComprehensive, Overlapping_AlongPosZ) +{ + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 1.5f }))); +} + +TEST(GjkComprehensive, Overlapping_AlongNegZ) +{ + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, -1.5f }))); +} + +TEST(GjkComprehensive, Overlapping_AlongDiagonalXY) +{ + // Minkowski sum extends ±2 on each axis; offset (1,1,0) is inside + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 1.f, 1.f, 0.f }))); +} + +TEST(GjkComprehensive, Overlapping_AlongDiagonalXYZ) +{ + // All three axes overlap: (1,1,1) is inside the Minkowski sum + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 1.f, 1.f, 1.f }))); +} + +TEST(GjkComprehensive, FullyNested_SmallInsideBig) +{ + // Small cube (half-ext 0.5) fully inside big cube (half-ext 2) + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }, { 2, 2, 2 }), make_cube({ 0, 0, 0 }, { 0.5f, 0.5f, 0.5f }))); +} + +TEST(GjkComprehensive, FullyNested_OffCenter) +{ + // Small at (0.5, 0, 0) still fully inside big (half-ext 2) + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }, { 2, 2, 2 }), make_cube({ 0.5f, 0, 0 }, { 0.5f, 0.5f, 0.5f }))); +} + +TEST(GjkComprehensive, Overlapping_AsymmetricSizes) +{ + // Big (scale 2, half-ext 2) and small (scale 0.5, half-ext 0.5) at 2.0 → overlap 0.5 in X + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }, { 2, 2, 2 }), make_cube({ 2.0f, 0, 0 }, { 0.5f, 0.5f, 0.5f }))); +} + +// --------------------------------------------------------------------------- +// Boundary cases +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, BoundaryCase_JustColliding) +{ + // B at 1.999 — 0.001 overlap in X + EXPECT_TRUE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 1.999f, 0, 0 }))); +} + +TEST(GjkComprehensive, BoundaryCase_JustSeparated) +{ + // B at 2.001 — 0.001 gap in X + EXPECT_FALSE(Gjk::is_collide(make_cube({ 0, 0, 0 }), make_cube({ 2.001f, 0, 0 }))); +} + +// --------------------------------------------------------------------------- +// Symmetry +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, Symmetry_WhenColliding) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.5f, 0, 0 }); + EXPECT_EQ(Gjk::is_collide(a, b), Gjk::is_collide(b, a)); +} + +TEST(GjkComprehensive, Symmetry_WhenSeparated) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 2.1f, 0.5f, 0 }); + EXPECT_EQ(Gjk::is_collide(a, b), Gjk::is_collide(b, a)); +} + +TEST(GjkComprehensive, Symmetry_DiagonalSeparation) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.5f, 1.5f, 1.5f }); + EXPECT_EQ(Gjk::is_collide(a, b), Gjk::is_collide(b, a)); +} + +// --------------------------------------------------------------------------- +// Simplex info +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, SimplexInfo_HitProducesSimplex4) +{ + // On collision the simplex must be a full tetrahedron (4 points) + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_TRUE(hit); + EXPECT_EQ(simplex.size(), 4u); +} + +TEST(GjkComprehensive, SimplexInfo_MissProducesLessThan4) +{ + // On non-collision the simplex can never be a full tetrahedron + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(make_cube({ 0, 0, 0 }), make_cube({ 2.1f, 0, 0 })); + EXPECT_FALSE(hit); + EXPECT_LT(simplex.size(), 4u); +} + +TEST(GjkComprehensive, SimplexInfo_HitAlongY) +{ + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.5f, 0 })); + EXPECT_TRUE(hit); + EXPECT_EQ(simplex.size(), 4u); +} + +TEST(GjkComprehensive, SimplexInfo_HitAlongZ) +{ + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 1.5f })); + EXPECT_TRUE(hit); + EXPECT_EQ(simplex.size(), 4u); +} + +TEST(GjkComprehensive, SimplexInfo_MissAlongDiagonal) +{ + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(make_cube({ 0, 0, 0 }), make_cube({ 2.1f, 2.1f, 2.1f })); + EXPECT_FALSE(hit); + EXPECT_LT(simplex.size(), 4u); +} + +// --------------------------------------------------------------------------- +// Non-trivial geometry — tetrahedron shaped colliders +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, TetrahedronShapes_Overlapping) +{ + // A rough tetrahedron mesh; two of them close enough to overlap + const std::vector> tet_vbo = { + { { 0.f, 1.f, 0.f }, {}, {} }, + { { -1.f, -1.f, 1.f }, {}, {} }, + { { 1.f, -1.f, 1.f }, {}, {} }, + { { 0.f, -1.f, -1.f }, {}, {} }, + }; + + Mesh m_a{ tet_vbo, k_empty_ebo }; + Mesh m_b{ tet_vbo, k_empty_ebo }; + m_b.set_origin({ 0.5f, 0.f, 0.f }); + + EXPECT_TRUE(Gjk::is_collide(Collider{ m_a }, Collider{ m_b })); +} + +TEST(GjkComprehensive, TetrahedronShapes_Separated) +{ + const std::vector> tet_vbo = { + { { 0.f, 1.f, 0.f }, {}, {} }, + { { -1.f, -1.f, 1.f }, {}, {} }, + { { 1.f, -1.f, 1.f }, {}, {} }, + { { 0.f, -1.f, -1.f }, {}, {} }, + }; + + Mesh m_a{ tet_vbo, k_empty_ebo }; + Mesh m_b{ tet_vbo, k_empty_ebo }; + m_b.set_origin({ 3.f, 0.f, 0.f }); + + EXPECT_FALSE(Gjk::is_collide(Collider{ m_a }, Collider{ m_b })); +} + +// --------------------------------------------------------------------------- +// Determinism +// --------------------------------------------------------------------------- + +TEST(GjkComprehensive, Deterministic_SameResultOnRepeatedCalls) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.2f, 0.3f, 0.1f }); + const bool first = Gjk::is_collide(a, b); + for (int i = 0; i < 10; ++i) + EXPECT_EQ(Gjk::is_collide(a, b), first); +} From 529322fe34a21772445cae030139b4651ba5a73e Mon Sep 17 00:00:00 2001 From: Orange Date: Tue, 3 Mar 2026 08:14:12 +0300 Subject: [PATCH 04/10] decomposed method --- include/omath/collision/epa_algorithm.hpp | 66 ++++++++++++++--------- 1 file changed, 41 insertions(+), 25 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 7d461888..4b4bc7db 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -50,7 +50,6 @@ namespace omath::collision int max_iterations{64}; FloatingType tolerance{1e-4}; // absolute tolerance on distance growth }; - // Precondition: simplex.size()==4 and contains the origin. [[nodiscard]] static std::optional solve(const ColliderInterfaceType& a, const ColliderInterfaceType& b, @@ -72,13 +71,7 @@ namespace omath::collision for (int it = 0; it < params.max_iterations; ++it) { // Lazily discard stale (deleted or index-mismatched) heap entries. - while (!heap.empty()) - { - const auto& top = heap.top(); - if (!faces[top.idx].deleted && faces[top.idx].d == top.d) - break; - heap.pop(); - } + discard_stale_heap_entries(faces, heap); if (heap.empty()) break; @@ -105,17 +98,7 @@ namespace omath::collision // Tombstone visible faces and collect the horizon boundary. // This avoids copying the faces array (O(n)) each iteration. - boundary.clear(); - for (auto& f : faces) - { - if (!f.deleted && visible_from(f, p)) - { - f.deleted = true; - add_edge_boundary(boundary, f.i0, f.i1); - add_edge_boundary(boundary, f.i1, f.i2); - add_edge_boundary(boundary, f.i2, f.i0); - } - } + tombstone_visible_faces(faces, boundary, p); // Stitch new faces around the horizon and push them directly onto the // heap — no full O(n log n) rebuild needed. @@ -133,10 +116,7 @@ namespace omath::collision } // Find the best surviving (non-deleted) face. - const Face* best = nullptr; - for (const auto& f : faces) - if (!f.deleted && (best == nullptr || f.d < best->d)) - best = &f; + const Face* best = find_best_surviving_face(faces); if (!best) return std::nullopt; @@ -153,8 +133,8 @@ namespace omath::collision struct Face final { int i0, i1, i2; - VectorType n; // unit outward normal - FloatingType d; // n · v0 (>= 0 ideally because origin is inside) + VectorType n; // unit outward normal + FloatingType d; // n · v0 (>= 0 ideally because origin is inside) bool deleted{false}; // tombstone flag — avoids O(n) compaction per iteration }; @@ -277,5 +257,41 @@ namespace omath::collision vertexes.emplace_back(simplex[i]); return vertexes; } + + static const Face* find_best_surviving_face(const std::pmr::vector& faces) + { + const Face* best = nullptr; + for (const auto& f : faces) + if (!f.deleted && (best == nullptr || f.d < best->d)) + best = &f; + return best; + } + static void tombstone_visible_faces(std::pmr::vector& faces, std::pmr::vector& boundary, + const VectorType& p) + { + boundary.clear(); + for (auto& f : faces) + { + if (!f.deleted && visible_from(f, p)) + { + f.deleted = true; + add_edge_boundary(boundary, f.i0, f.i1); + add_edge_boundary(boundary, f.i1, f.i2); + add_edge_boundary(boundary, f.i2, f.i0); + } + } + } + + static void discard_stale_heap_entries(const std::pmr::vector& faces, + std::priority_queue, HeapCmp>& heap) + { + while (!heap.empty()) + { + const auto& top = heap.top(); + if (!faces[top.idx].deleted && faces[top.idx].d == top.d) + break; + heap.pop(); + } + } }; } // namespace omath::collision From 2c70288a8f4bad2a6f03a84defff28839a501f5b Mon Sep 17 00:00:00 2001 From: orange Date: Tue, 3 Mar 2026 08:27:26 +0300 Subject: [PATCH 05/10] added epa tests --- tests/general/unit_test_epa_comprehensive.cpp | 471 ++++++++++++++++++ 1 file changed, 471 insertions(+) create mode 100644 tests/general/unit_test_epa_comprehensive.cpp diff --git a/tests/general/unit_test_epa_comprehensive.cpp b/tests/general/unit_test_epa_comprehensive.cpp new file mode 100644 index 00000000..c0211647 --- /dev/null +++ b/tests/general/unit_test_epa_comprehensive.cpp @@ -0,0 +1,471 @@ +// +// Comprehensive EPA tests. +// Covers: all 3 axis directions, multiple depth levels, penetration-vector +// round-trips, depth monotonicity, symmetry, asymmetric sizes, memory +// resource variants, tolerance sensitivity, and iteration bookkeeping. +// +#include +#include +#include +#include +#include +#include +#include + +using Mesh = omath::source_engine::Mesh; +using Collider = omath::source_engine::MeshCollider; +using Gjk = omath::collision::GjkAlgorithm; +using Epa = omath::collision::Epa; +using Vec3 = omath::Vector3; + +namespace +{ + const std::vector> k_cube_vbo = { + { { -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 }, {}, {} }, + }; + const std::vector> k_empty_ebo{}; + + constexpr Epa::Params k_default_params{ .max_iterations = 64, .tolerance = 1e-4f }; + + Collider make_cube(const Vec3& origin = {}, const Vec3& scale = { 1, 1, 1 }) + { + Mesh m{ k_cube_vbo, k_empty_ebo, scale }; + m.set_origin(origin); + return Collider{ m }; + } + + // Run GJK then EPA; asserts GJK hit and EPA converged. + Epa::Result solve(const Collider& a, const Collider& b, + const Epa::Params& params = k_default_params) + { + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + EXPECT_TRUE(hit) << "GJK must detect collision before EPA can run"; + auto result = Epa::solve(a, b, simplex, params); + EXPECT_TRUE(result.has_value()) << "EPA must converge"; + return *result; + } +} // namespace + +// --------------------------------------------------------------------------- +// Normal direction per axis +// --------------------------------------------------------------------------- + +// For two unit cubes (half-extent 1) with B offset by d along an axis: +// depth = 2 - d (distance from origin to nearest face of Minkowski diff) +// normal component along that axis ≈ ±1 + +TEST(EpaComprehensive, NormalAlongX_Positive) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_NEAR(std::abs(r.normal.x), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongX_Negative) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ -0.5f, 0, 0 })); + EXPECT_NEAR(std::abs(r.normal.x), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongY_Positive) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0.5f, 0 })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.y), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongY_Negative) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, -0.5f, 0 })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.y), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongZ_Positive) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 0.5f })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.z), 1.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongZ_Negative) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, -0.5f })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.z), 1.f, 1e-3f); +} + +// --------------------------------------------------------------------------- +// Depth correctness (depth = 2 - offset for unit cubes) +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Depth_ShallowOverlap) +{ + // offset 1.9 → depth 0.1 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.9f, 0, 0 })); + EXPECT_NEAR(r.depth, 0.1f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_QuarterOverlap) +{ + // offset 1.5 → depth 0.5 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.5f, 0, 0 })); + EXPECT_NEAR(r.depth, 0.5f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_HalfOverlap) +{ + // offset 1.0 → depth 1.0 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.0f, 0, 0 })); + EXPECT_NEAR(r.depth, 1.0f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_ThreeQuarterOverlap) +{ + // offset 0.5 → depth 1.5 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_NEAR(r.depth, 1.5f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_AlongY_HalfOverlap) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.0f, 0 })); + EXPECT_NEAR(r.depth, 1.0f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_AlongZ_HalfOverlap) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 1.0f })); + EXPECT_NEAR(r.depth, 1.0f, 1e-2f); +} + +// --------------------------------------------------------------------------- +// Depth monotonicity — deeper overlap → larger depth +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, DepthMonotonic_AlongX) +{ + const float d1 = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.9f, 0, 0 })).depth; // ~0.1 + const float d2 = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.5f, 0, 0 })).depth; // ~0.5 + const float d3 = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.0f, 0, 0 })).depth; // ~1.0 + const float d4 = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })).depth; // ~1.5 + + EXPECT_LT(d1, d2); + EXPECT_LT(d2, d3); + EXPECT_LT(d3, d4); +} + +// --------------------------------------------------------------------------- +// Normal is a unit vector +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, NormalIsUnit_AlongX) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_NEAR(r.normal.dot(r.normal), 1.f, 1e-5f); +} + +TEST(EpaComprehensive, NormalIsUnit_AlongY) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.2f, 0 })); + EXPECT_NEAR(r.normal.dot(r.normal), 1.f, 1e-5f); +} + +TEST(EpaComprehensive, NormalIsUnit_AlongZ) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 0.8f })); + EXPECT_NEAR(r.normal.dot(r.normal), 1.f, 1e-5f); +} + +// --------------------------------------------------------------------------- +// Penetration vector = normal * depth +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, PenetrationVectorLength_EqualsDepth) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + const float pen_len = std::sqrt(r.penetration_vector.dot(r.penetration_vector)); + EXPECT_NEAR(pen_len, r.depth, 1e-5f); +} + +TEST(EpaComprehensive, PenetrationVectorDirection_ParallelToNormal) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.0f, 0 })); + // penetration_vector = normal * depth → cross product must be ~zero + const auto cross = r.penetration_vector.cross(r.normal); + EXPECT_NEAR(cross.dot(cross), 0.f, 1e-8f); +} + +// --------------------------------------------------------------------------- +// Round-trip: applying penetration_vector separates the shapes +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, RoundTrip_AlongX) +{ + const auto a = make_cube({ 0, 0, 0 }); + Mesh mesh_b{ k_cube_vbo, k_empty_ebo }; + mesh_b.set_origin({ 0.5f, 0, 0 }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + // Move B along the penetration vector; it should separate from A + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })) << "Applying pen vector must separate"; + + // Moving the wrong way must still collide + Mesh mesh_wrong{ k_cube_vbo, k_empty_ebo }; + mesh_wrong.set_origin(mesh_b.get_origin() - r.penetration_vector * margin); + EXPECT_TRUE(Gjk::is_collide(a, Collider{ mesh_wrong })) << "Opposite direction must still collide"; +} + +TEST(EpaComprehensive, RoundTrip_AlongY) +{ + const auto a = make_cube({ 0, 0, 0 }); + Mesh mesh_b{ k_cube_vbo, k_empty_ebo }; + mesh_b.set_origin({ 0, 0.8f, 0 }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })); + + Mesh mesh_wrong{ k_cube_vbo, k_empty_ebo }; + mesh_wrong.set_origin(mesh_b.get_origin() - r.penetration_vector * margin); + EXPECT_TRUE(Gjk::is_collide(a, Collider{ mesh_wrong })); +} + +TEST(EpaComprehensive, RoundTrip_AlongZ) +{ + const auto a = make_cube({ 0, 0, 0 }); + Mesh mesh_b{ k_cube_vbo, k_empty_ebo }; + mesh_b.set_origin({ 0, 0, 1.2f }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })); +} + +// --------------------------------------------------------------------------- +// Symmetry — swapping A and B preserves depth +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Symmetry_DepthIsIndependentOfOrder) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + + const float depth_ab = solve(a, b).depth; + const float depth_ba = solve(b, a).depth; + + EXPECT_NEAR(depth_ab, depth_ba, 1e-2f); +} + +TEST(EpaComprehensive, Symmetry_NormalsAreOpposite) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + + const Vec3 n_ab = solve(a, b).normal; + const Vec3 n_ba = solve(b, a).normal; + + // The normals should be anti-parallel: n_ab · n_ba ≈ -1 + EXPECT_NEAR(n_ab.dot(n_ba), -1.f, 1e-3f); +} + +// --------------------------------------------------------------------------- +// Asymmetric sizes +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, LargeVsSmall_DepthCorrect) +{ + // Big (half-ext 2) at origin, small (half-ext 0.5) at (2.0, 0, 0) + // Minkowski diff closest face in X at distance 0.5 + const auto r = solve(make_cube({ 0, 0, 0 }, { 2, 2, 2 }), make_cube({ 2.0f, 0, 0 }, { 0.5f, 0.5f, 0.5f })); + EXPECT_NEAR(r.depth, 0.5f, 1e-2f); + EXPECT_NEAR(std::abs(r.normal.x), 1.f, 1e-3f); +} + +TEST(EpaComprehensive, LargeVsSmall_RoundTrip) +{ + const auto a = make_cube({ 0, 0, 0 }, { 2, 2, 2 }); + + Mesh mesh_b{ k_cube_vbo, k_empty_ebo, { 0.5f, 0.5f, 0.5f } }; + mesh_b.set_origin({ 2.0f, 0, 0 }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo, { 0.5f, 0.5f, 0.5f } }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })); +} + +// --------------------------------------------------------------------------- +// Memory resource variants +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, MonotonicBuffer_ConvergesCorrectly) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + constexpr std::size_t k_buf = 32768; + alignas(std::max_align_t) char buf[k_buf]; + std::pmr::monotonic_buffer_resource mr{ buf, k_buf, std::pmr::null_memory_resource() }; + + const auto r = Epa::solve(a, b, simplex, k_default_params, mr); + ASSERT_TRUE(r.has_value()); + EXPECT_NEAR(r->depth, 1.5f, 1e-2f); +} + +TEST(EpaComprehensive, MonotonicBuffer_MultipleReleaseCycles) +{ + // Verify mr.release() correctly resets the buffer across multiple calls + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + constexpr std::size_t k_buf = 32768; + alignas(std::max_align_t) char buf[k_buf]; + std::pmr::monotonic_buffer_resource mr{ buf, k_buf, std::pmr::null_memory_resource() }; + + float first_depth = 0.f; + for (int i = 0; i < 5; ++i) + { + mr.release(); + const auto r = Epa::solve(a, b, simplex, k_default_params, mr); + ASSERT_TRUE(r.has_value()) << "solve must converge on iteration " << i; + if (i == 0) + first_depth = r->depth; + else + EXPECT_NEAR(r->depth, first_depth, 1e-6f) << "depth must be deterministic"; + } +} + +TEST(EpaComprehensive, DefaultResource_ConvergesCorrectly) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.0f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + const auto r = Epa::solve(a, b, simplex); + ASSERT_TRUE(r.has_value()); + EXPECT_NEAR(r->depth, 1.0f, 1e-2f); +} + +// --------------------------------------------------------------------------- +// Tolerance sensitivity +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, TighterTolerance_MoreAccurateDepth) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.0f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + const Epa::Params loose{ .max_iterations = 64, .tolerance = 1e-2f }; + const Epa::Params tight{ .max_iterations = 64, .tolerance = 1e-5f }; + + const auto r_loose = Epa::solve(a, b, simplex, loose); + const auto r_tight = Epa::solve(a, b, simplex, tight); + ASSERT_TRUE(r_loose.has_value()); + ASSERT_TRUE(r_tight.has_value()); + + // Tighter tolerance must yield a result at least as accurate + EXPECT_LE(std::abs(r_tight->depth - 1.0f), std::abs(r_loose->depth - 1.0f) + 1e-4f); +} + +// --------------------------------------------------------------------------- +// Bookkeeping fields +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Bookkeeping_IterationsInBounds) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto r = solve(a, b); + + EXPECT_GT(r.iterations, 0); + EXPECT_LE(r.iterations, k_default_params.max_iterations); +} + +TEST(EpaComprehensive, Bookkeeping_FacesAndVerticesGrow) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto r = solve(a, b); + + // Started with a tetrahedron (4 faces, 4 vertices); EPA must have expanded it + EXPECT_GE(r.num_faces, 4); + EXPECT_GE(r.num_vertices, 4); +} + +TEST(EpaComprehensive, Bookkeeping_MaxIterationsRespected) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + constexpr Epa::Params tight{ .max_iterations = 3, .tolerance = 1e-10f }; + const auto r = Epa::solve(a, b, simplex, tight); + + // Must return something (fallback best-face path) and respect the cap + if (r.has_value()) + EXPECT_LE(r->iterations, tight.max_iterations); +} + +// --------------------------------------------------------------------------- +// Determinism +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Deterministic_SameResultOnRepeatedCalls) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.7f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + const auto first = Epa::solve(a, b, simplex); + ASSERT_TRUE(first.has_value()); + + for (int i = 0; i < 5; ++i) + { + const auto r = Epa::solve(a, b, simplex); + ASSERT_TRUE(r.has_value()); + EXPECT_NEAR(r->depth, first->depth, 1e-6f); + EXPECT_NEAR(r->normal.x, first->normal.x, 1e-6f); + EXPECT_NEAR(r->normal.y, first->normal.y, 1e-6f); + EXPECT_NEAR(r->normal.z, first->normal.z, 1e-6f); + } +} From dee705a391c144b8dfde1ae2ab14dd9e231270a2 Mon Sep 17 00:00:00 2001 From: orange Date: Tue, 3 Mar 2026 08:43:30 +0300 Subject: [PATCH 06/10] improvement --- include/omath/collision/epa_algorithm.hpp | 35 ++++++++++++++++------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 4b4bc7db..798dddea 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -64,9 +65,9 @@ namespace omath::collision Result out{}; - // Hoisted outside the loop to reuse the allocation across iterations. - std::pmr::vector boundary{&mem_resource}; - boundary.reserve(16); + // Hoisted outside the loop to reuse bucket allocation across iterations. + // Initial bucket count 16 covers a typical horizon without rehashing. + BoundaryMap boundary{16, &mem_resource}; for (int it = 0; it < params.max_iterations; ++it) { @@ -102,7 +103,7 @@ namespace omath::collision // Stitch new faces around the horizon and push them directly onto the // heap — no full O(n log n) rebuild needed. - for (const auto& e : boundary) + for (const auto& [key, e] : boundary) { const int fi = static_cast(faces.size()); faces.emplace_back(make_face(vertexes, e.a, e.b, new_idx)); @@ -160,6 +161,16 @@ namespace omath::collision using Heap = std::priority_queue, HeapCmp>; + // Horizon boundary: maps packed(a,b) → Edge. + // Opposite edges cancel in O(1) via hash lookup instead of O(h) linear scan. + using BoundaryMap = std::pmr::unordered_map; + + [[nodiscard]] + static constexpr int64_t pack_edge(int a, int b) noexcept + { + return (static_cast(a) << 32) | static_cast(b); + } + [[nodiscard]] static Heap rebuild_heap(const std::pmr::vector& faces, auto& memory_resource) { @@ -178,14 +189,16 @@ namespace omath::collision return f.n.dot(p) - f.d > static_cast(1e-7); } - static void add_edge_boundary(std::pmr::vector& boundary, int a, int b) + static void add_edge_boundary(BoundaryMap& boundary, int a, int b) { - // Keep edges that appear only once; cancel if opposite already present. - auto itb = std::ranges::find_if(boundary, [&](const Edge& e) { return e.a == b && e.b == a; }); - if (itb != boundary.end()) - boundary.erase(itb); + // O(1) cancel: if the opposite edge (b→a) is already in the map it is an + // internal edge shared by two visible faces and must be removed. + // Otherwise this is a horizon edge and we insert it. + const int64_t rev = pack_edge(b, a); + if (const auto it = boundary.find(rev); it != boundary.end()) + boundary.erase(it); else - boundary.emplace_back(a, b); + boundary.emplace(pack_edge(a, b), Edge{a, b}); } [[nodiscard]] @@ -266,7 +279,7 @@ namespace omath::collision best = &f; return best; } - static void tombstone_visible_faces(std::pmr::vector& faces, std::pmr::vector& boundary, + static void tombstone_visible_faces(std::pmr::vector& faces, BoundaryMap& boundary, const VectorType& p) { boundary.clear(); From 11fe49e801794bfb03c33708a19dd2ab030d9bc5 Mon Sep 17 00:00:00 2001 From: orange Date: Tue, 3 Mar 2026 08:51:13 +0300 Subject: [PATCH 07/10] added const --- include/omath/collision/epa_algorithm.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 798dddea..8013c74d 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -166,7 +166,7 @@ namespace omath::collision using BoundaryMap = std::pmr::unordered_map; [[nodiscard]] - static constexpr int64_t pack_edge(int a, int b) noexcept + static constexpr int64_t pack_edge(const int a, const int b) noexcept { return (static_cast(a) << 32) | static_cast(b); } From 2dafc8a49d510f9b3b30d37c20c4189fcc5e4593 Mon Sep 17 00:00:00 2001 From: orange Date: Tue, 3 Mar 2026 09:22:11 +0300 Subject: [PATCH 08/10] added additional method --- include/omath/collision/mesh_collider.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/include/omath/collision/mesh_collider.hpp b/include/omath/collision/mesh_collider.hpp index accbe570..a907237d 100644 --- a/include/omath/collision/mesh_collider.hpp +++ b/include/omath/collision/mesh_collider.hpp @@ -42,6 +42,14 @@ namespace omath::collision m_mesh.set_origin(new_origin); } + const MeshType& get_mesh() const + { + return m_mesh; + } + MeshType& get_mesh() + { + return m_mesh; + } private: [[nodiscard]] const VertexType& find_furthest_vertex(const VectorType& direction) const From 68f4c8cc721bbcea8e6a631ec08ebc8e1b2088d2 Mon Sep 17 00:00:00 2001 From: orange Date: Tue, 3 Mar 2026 09:38:05 +0300 Subject: [PATCH 09/10] added nodiscard --- include/omath/collision/mesh_collider.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/omath/collision/mesh_collider.hpp b/include/omath/collision/mesh_collider.hpp index a907237d..c02bdabb 100644 --- a/include/omath/collision/mesh_collider.hpp +++ b/include/omath/collision/mesh_collider.hpp @@ -42,10 +42,12 @@ namespace omath::collision m_mesh.set_origin(new_origin); } + [[nodiscard]] const MeshType& get_mesh() const { return m_mesh; } + [[nodiscard]] MeshType& get_mesh() { return m_mesh; From 7373e6d3df6c9fd08a9393977887ff7f81cb1b24 Mon Sep 17 00:00:00 2001 From: orange Date: Tue, 3 Mar 2026 10:00:46 +0300 Subject: [PATCH 10/10] added std namspace to int64_t type --- 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 8013c74d..660a2b1a 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -163,12 +163,12 @@ namespace omath::collision // Horizon boundary: maps packed(a,b) → Edge. // Opposite edges cancel in O(1) via hash lookup instead of O(h) linear scan. - using BoundaryMap = std::pmr::unordered_map; + using BoundaryMap = std::pmr::unordered_map; [[nodiscard]] - static constexpr int64_t pack_edge(const int a, const int b) noexcept + static constexpr std::int64_t pack_edge(const int a, const int b) noexcept { - return (static_cast(a) << 32) | static_cast(b); + return (static_cast(a) << 32) | static_cast(b); } [[nodiscard]] @@ -194,7 +194,7 @@ namespace omath::collision // O(1) cancel: if the opposite edge (b→a) is already in the map it is an // internal edge shared by two visible faces and must be removed. // Otherwise this is a horizon edge and we insert it. - const int64_t rev = pack_edge(b, a); + const std::int64_t rev = pack_edge(b, a); if (const auto it = boundary.find(rev); it != boundary.end()) boundary.erase(it); else