From 06b597f37c5de3091e2bd58b5e270f2ee95cce60 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 15:57:38 +0300 Subject: [PATCH] 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