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; } }