From a79ad6948c026205130494db3221aa21d6d3ecfb Mon Sep 17 00:00:00 2001 From: orange Date: Mon, 2 Mar 2026 19:40:45 +0300 Subject: [PATCH] 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 31599c0..7d46188 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); + // 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); + } + } - erase_marked(faces, to_delete); - - // 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 59073d1..af7470e 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);