From d22f7e472c6be0123dddbcde9bab4fa4604e70aa Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 16 Sep 2026 17:00:38 -0400 Subject: [PATCH 1/6] Make the collision candidates trivially copyable The broad phase stores millions of candidates and every one carried a vptr: EdgeEdgeCandidate was 16 bytes for 8 bytes of payload, and being polymorphic it was not trivially copyable, so the memcpy fast path in merge_thread_local_vectors was dead code on every candidate path. - `StencilMixin` (candidates/stencil_mixin.hpp) holds the stencil operations that were spread across CollisionStencil, reaching the primitives through a CRTP self-cast. It has no data members and no virtual functions, so deriving from it costs nothing in size. `StencilMath` holds the statics, compiled once rather than once per stencil. - The five candidate types derive from the mixin rather than CollisionStencil, and are now 8 bytes and trivially copyable (PlaneVertexCandidate excepted: it holds an Eigen::Hyperplane). - `StencilAdapter` and `DTypeStencilAdapter` (candidates/stencil_adapter.hpp) give a candidate the polymorphic interface back, and the collision types derive from those. The distance type that the edge-edge, edge-vertex and face-vertex collisions cache used to be a virtual their candidate's own math called, so the math now takes it as a defaulted argument and the adapter supplies it. - `Candidates::visit()` replaces `operator[]`, which can no longer hand out a CollisionStencil&; `NormalCollisions::visit()` mirrors it so `semi_implicit_stiffness` is written once for both. - Python keeps its API: each candidate binds the stencil operations through `define_stencil_methods`, and `Candidates.__getitem__` returns the concrete type by reference, now with the container's lifetime tied to it. Edge-edge detection on the CPU, median of three runs of 20 samples: | Scene | Before | After | Speedup | |-------------------|------------|-----------|---------| | Cloth-Funnel | 0.82 ms | 0.41 ms | 1.97x | | Armadillo-Rollers | 28.16 ms | 10.79 ms | 2.61x | | Rod-Twist | 11.55 ms | 5.68 ms | 2.04x | | Cloth-Ball | 29.97 ms | 13.09 ms | 2.29x | | N-Body-Simulation | 123.28 ms | 42.14 ms | 2.93x | | Puffer-Ball | 1129.95 ms | 329.71 ms | 3.43x | LBVH build, which touches no candidates, is unchanged at 0.98-1.06x. Both stages of detection gain: on Cloth-Ball the traversal goes from 8.19 to 6.35 ms because the thread-local vectors accumulate half as many bytes, and the merge from 17.25 to 6.37 ms because it is now a memcpy. Co-Authored-By: Claude Opus 5 --- python/src/candidates/candidates.cpp | 14 +- python/src/candidates/edge_edge.cpp | 14 +- python/src/candidates/edge_vertex.cpp | 14 +- python/src/candidates/face_vertex.cpp | 14 +- python/src/candidates/plane_vertex.cpp | 19 +- python/src/candidates/stencil_methods.hpp | 180 ++++++++ python/src/candidates/vertex_vertex.cpp | 15 +- python/src/collisions/normal/edge_edge.cpp | 2 +- python/src/collisions/normal/edge_vertex.cpp | 2 +- python/src/collisions/normal/face_vertex.cpp | 2 +- python/src/collisions/normal/plane_vertex.cpp | 3 +- .../src/collisions/normal/vertex_vertex.cpp | 2 +- .../src/collisions/tangential/edge_edge.cpp | 2 +- .../src/collisions/tangential/edge_vertex.cpp | 4 +- .../src/collisions/tangential/face_vertex.cpp | 4 +- .../collisions/tangential/plane_vertex.cpp | 2 +- .../collisions/tangential/vertex_vertex.cpp | 2 +- src/ipc/barrier/adaptive_stiffness.cpp | 92 ++-- src/ipc/barrier/adaptive_stiffness.hpp | 35 +- src/ipc/candidates/CMakeLists.txt | 3 +- src/ipc/candidates/candidates.cpp | 128 ++---- src/ipc/candidates/candidates.hpp | 68 ++- src/ipc/candidates/collision_stencil.cpp | 149 ------ src/ipc/candidates/collision_stencil.hpp | 286 +----------- src/ipc/candidates/edge_edge.cpp | 26 +- src/ipc/candidates/edge_edge.hpp | 61 ++- src/ipc/candidates/edge_vertex.cpp | 30 +- src/ipc/candidates/edge_vertex.hpp | 57 ++- src/ipc/candidates/face_vertex.cpp | 26 +- src/ipc/candidates/face_vertex.hpp | 61 ++- src/ipc/candidates/plane_vertex.hpp | 49 +- src/ipc/candidates/stencil_adapter.hpp | 209 +++++++++ src/ipc/candidates/stencil_mixin.hpp | 423 ++++++++++++++++++ src/ipc/candidates/vertex_vertex.hpp | 55 ++- src/ipc/collisions/normal/edge_edge.cpp | 6 +- src/ipc/collisions/normal/edge_edge.hpp | 3 +- src/ipc/collisions/normal/edge_vertex.hpp | 8 +- src/ipc/collisions/normal/face_vertex.hpp | 8 +- .../collisions/normal/normal_collisions.hpp | 14 + src/ipc/collisions/normal/plane_vertex.hpp | 8 +- src/ipc/collisions/normal/vertex_vertex.hpp | 8 +- src/ipc/collisions/tangential/edge_edge.cpp | 2 +- src/ipc/collisions/tangential/edge_edge.hpp | 9 +- src/ipc/collisions/tangential/edge_vertex.cpp | 2 +- src/ipc/collisions/tangential/edge_vertex.hpp | 9 +- src/ipc/collisions/tangential/face_vertex.cpp | 2 +- src/ipc/collisions/tangential/face_vertex.hpp | 9 +- .../collisions/tangential/plane_vertex.cpp | 2 +- .../collisions/tangential/plane_vertex.hpp | 9 +- .../collisions/tangential/vertex_vertex.cpp | 2 +- .../collisions/tangential/vertex_vertex.hpp | 9 +- src/ipc/esp/esp_collisions.cpp | 7 +- src/ipc/gcp/gcp_collisions.cpp | 5 +- src/ipc/ogc/trust_region.cpp | 19 +- .../src/tests/candidates/test_candidates.cpp | 68 +-- .../test_plane_vertex_collisions.cpp | 11 +- .../test_distance_vector_methods.cpp | 8 +- 57 files changed, 1423 insertions(+), 858 deletions(-) create mode 100644 python/src/candidates/stencil_methods.hpp delete mode 100644 src/ipc/candidates/collision_stencil.cpp create mode 100644 src/ipc/candidates/stencil_adapter.hpp create mode 100644 src/ipc/candidates/stencil_mixin.hpp diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index 872665c2e..44c28cafe 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -51,10 +51,18 @@ void define_candidates(py::module_& m) .def("clear", &Candidates::clear) .def( "__getitem__", - [](Candidates& self, size_t i) -> CollisionStencil& { - return self[i]; + // Python is dynamically typed, so the concrete candidate type is + // returned rather than a common base. The reference points into the + // vector that holds it, with the Candidates object kept alive for + // as long as the reference lives. + [](py::object self, size_t i) { + return self.cast().visit(i, [&](auto& candidate) { + return py::cast( + &candidate, py::return_value_policy::reference_internal, + self); + }); }, - py::return_value_policy::reference) + "i"_a) .def( "is_step_collision_free", &Candidates::is_step_collision_free, R"ipc_Qu8mg5v7( diff --git a/python/src/candidates/edge_edge.cpp b/python/src/candidates/edge_edge.cpp index 7b36e053b..8194d8a82 100644 --- a/python/src/candidates/edge_edge.cpp +++ b/python/src/candidates/edge_edge.cpp @@ -1,13 +1,15 @@ #include +#include "stencil_methods.hpp" + #include using namespace ipc; void define_edge_edge_candidate(py::module_& m) { - py::class_(m, "EdgeEdgeCandidate") - .def(py::init(), "edge0_id"_a, "edge1_id"_a) + auto cls = py::class_(m, "EdgeEdgeCandidate"); + cls.def(py::init(), "edge0_id"_a, "edge1_id"_a) .def( py::init([](std::tuple edge_ids) { return std::make_unique( @@ -38,4 +40,12 @@ void define_edge_edge_candidate(py::module_& m) py::implicitly_convertible< std::tuple, EdgeEdgeCandidate>(); + + define_stencil_methods(cls); + + // The adapter the collision types derive from; it carries the + // polymorphic stencil interface the POD candidate deliberately + // lacks. + py::class_( + m, "EdgeEdgeStencil"); } diff --git a/python/src/candidates/edge_vertex.cpp b/python/src/candidates/edge_vertex.cpp index 84d7dd7f5..b7eab6d25 100644 --- a/python/src/candidates/edge_vertex.cpp +++ b/python/src/candidates/edge_vertex.cpp @@ -1,13 +1,15 @@ #include +#include "stencil_methods.hpp" + #include using namespace ipc; void define_edge_vertex_candidate(py::module_& m) { - py::class_(m, "EdgeVertexCandidate") - .def(py::init(), "edge_id"_a, "vertex_id"_a) + auto cls = py::class_(m, "EdgeVertexCandidate"); + cls.def(py::init(), "edge_id"_a, "vertex_id"_a) .def( py::init([](std::tuple edge_and_vertex_id) { return std::make_unique( @@ -40,4 +42,12 @@ void define_edge_vertex_candidate(py::module_& m) py::implicitly_convertible< std::tuple, EdgeVertexCandidate>(); + + define_stencil_methods(cls); + + // The adapter the collision types derive from; it carries the + // polymorphic stencil interface the POD candidate deliberately + // lacks. + py::class_( + m, "EdgeVertexStencil"); } diff --git a/python/src/candidates/face_vertex.cpp b/python/src/candidates/face_vertex.cpp index 06aad4a60..8c82f37f7 100644 --- a/python/src/candidates/face_vertex.cpp +++ b/python/src/candidates/face_vertex.cpp @@ -1,13 +1,15 @@ #include +#include "stencil_methods.hpp" + #include using namespace ipc; void define_face_vertex_candidate(py::module_& m) { - py::class_(m, "FaceVertexCandidate") - .def(py::init(), "face_id"_a, "vertex_id"_a) + auto cls = py::class_(m, "FaceVertexCandidate"); + cls.def(py::init(), "face_id"_a, "vertex_id"_a) .def( py::init([](std::tuple face_and_vertex_id) { return std::make_unique( @@ -40,4 +42,12 @@ void define_face_vertex_candidate(py::module_& m) py::implicitly_convertible< std::tuple, FaceVertexCandidate>(); + + define_stencil_methods(cls); + + // The adapter the collision types derive from; it carries the + // polymorphic stencil interface the POD candidate deliberately + // lacks. + py::class_( + m, "FaceVertexStencil"); } diff --git a/python/src/candidates/plane_vertex.cpp b/python/src/candidates/plane_vertex.cpp index 07f6eb30f..918176177 100644 --- a/python/src/candidates/plane_vertex.cpp +++ b/python/src/candidates/plane_vertex.cpp @@ -2,17 +2,26 @@ #include +#include "stencil_methods.hpp" + using namespace ipc; void define_plane_vertex_candidate(py::module_& m) { - py::class_( - m, "PlaneVertexCandidate") - .def( - py::init, index_t>(), "plane"_a, - "vertex_id"_a) + auto cls = py::class_(m, "PlaneVertexCandidate"); + cls.def( + py::init, index_t>(), "plane"_a, + "vertex_id"_a) .def_readwrite( "plane", &PlaneVertexCandidate::plane, "Plane of the candidate") .def_readwrite( "vertex_id", &PlaneVertexCandidate::vertex_id, "ID of the vertex"); + + define_stencil_methods(cls); + + // The adapter the collision types derive from; it carries the + // polymorphic stencil interface the POD candidate deliberately + // lacks. + py::class_( + m, "PlaneVertexStencil"); } \ No newline at end of file diff --git a/python/src/candidates/stencil_methods.hpp b/python/src/candidates/stencil_methods.hpp new file mode 100644 index 000000000..25214b935 --- /dev/null +++ b/python/src/candidates/stencil_methods.hpp @@ -0,0 +1,180 @@ +#pragma once + +#include + +#include + +#include +#include + +namespace ipc { + +/// @brief Bind the stencil operations onto a candidate class. +/// +/// The candidate types are no longer CollisionStencils -- the broad phase +/// stores millions of them and a vptr would double their size -- so Python no +/// longer inherits these from that base and each candidate binds them itself. +/// Lambdas rather than py::overload_cast, because the candidates whose +/// distance type can be known a priori take an extra defaulted argument and so +/// do not match the CollisionStencil signatures. +/// +/// The full docstrings live on CollisionStencil; these are the same functions. +/// +/// @tparam C The candidate type. +/// @param cls The pybind11 class to add the methods to. +template void define_stencil_methods(PyClass& cls) +{ + using CR12 = Eigen::ConstRef; + using CRXd = Eigen::ConstRef; + using CRXi = Eigen::ConstRef; + + cls.def( + "num_vertices", [](const C& self) { return self.num_vertices(); }, + "Get the number of vertices in the collision stencil.") + .def( + "dim", [](const C& self, const int ndof) { return self.dim(ndof); }, + "Get the dimension of the collision stencil.", "ndof"_a) + .def( + "vertex_ids", + [](const C& self, CRXi edges, CRXi faces) { + return self.vertex_ids(edges, faces); + }, + "Get the vertex IDs of the collision stencil.", "edges"_a, + "faces"_a) + .def( + "vertices", + [](const C& self, CRXd V, CRXi edges, CRXi faces) { + return self.vertices(V, edges, faces); + }, + "Get the vertex attributes of the collision stencil.", "vertices"_a, + "edges"_a, "faces"_a) + .def( + "dof", + [](const C& self, CRXd X, CRXi edges, CRXi faces) { + return self.dof(X, edges, faces); + }, + "Select this stencil's DOF from the full matrix of DOF.", "X"_a, + "edges"_a, "faces"_a) + // -- distance ------------------------------------------------------ + .def( + "compute_distance", + [](const C& self, CR12 positions) { + return self.compute_distance(positions); + }, + "Compute the distance of the stencil.", "positions"_a) + .def( + "compute_distance", + [](const C& self, CRXd V, CRXi edges, CRXi faces) { + return self.compute_distance(V, edges, faces); + }, + "Compute the distance of the stencil.", "vertices"_a, "edges"_a, + "faces"_a) + .def( + "compute_distance_gradient", + [](const C& self, CR12 positions) { + return self.compute_distance_gradient(positions); + }, + "Compute the distance gradient of the stencil.", "positions"_a) + .def( + "compute_distance_gradient", + [](const C& self, CRXd V, CRXi edges, CRXi faces) { + return self.compute_distance_gradient(V, edges, faces); + }, + "Compute the distance gradient of the stencil.", "vertices"_a, + "edges"_a, "faces"_a) + .def( + "compute_distance_hessian", + [](const C& self, CR12 positions) { + return self.compute_distance_hessian(positions); + }, + "Compute the distance Hessian of the stencil.", "positions"_a) + .def( + "compute_distance_hessian", + [](const C& self, CRXd V, CRXi edges, CRXi faces) { + return self.compute_distance_hessian(V, edges, faces); + }, + "Compute the distance Hessian of the stencil.", "vertices"_a, + "edges"_a, "faces"_a) + .def( + "compute_coefficients", + [](const C& self, CR12 positions) { + return self.compute_coefficients(positions); + }, + "Compute the coefficients of the stencil.", "positions"_a) + .def( + "compute_coefficients", + [](const C& self, CRXd V, CRXi edges, CRXi faces) { + return self.compute_coefficients(V, edges, faces); + }, + "Compute the coefficients of the stencil.", "vertices"_a, "edges"_a, + "faces"_a) + // -- distance vector ----------------------------------------------- + .def( + "compute_distance_vector", + [](const C& self, CR12 positions) { + return self.compute_distance_vector(positions); + }, + "Compute the distance vector of the stencil: t = sum(c_i * x_i).", + "positions"_a) + .def( + "compute_distance_vector", + [](const C& self, CRXd V, CRXi edges, CRXi faces) { + return self.compute_distance_vector(V, edges, faces); + }, + "Compute the distance vector of the stencil.", "vertices"_a, + "edges"_a, "faces"_a) + .def( + "compute_distance_vector_with_coefficients", + [](const C& self, CR12 positions) { + VectorMax4d coeffs; + VectorMax3d dv = + self.compute_distance_vector(positions, coeffs); + return std::make_tuple(dv, coeffs); + }, + "Compute the distance vector and the coefficients together.", + "positions"_a) + .def( + "compute_distance_vector_jacobian", + [](const C& self, CR12 positions) { + return self.compute_distance_vector_jacobian(positions); + }, + "Compute the Jacobian of the distance vector w.r.t. positions.", + "positions"_a) + // -- CCD ----------------------------------------------------------- + .def( + "ccd", + [](const C& self, CR12 vertices_t0, CR12 vertices_t1, + const double min_distance, const double tmax, + const NarrowPhaseCCD& narrow_phase_ccd) { + double toi; + bool r = self.ccd( + vertices_t0, vertices_t1, toi, min_distance, tmax, + narrow_phase_ccd); + return std::make_tuple(r, toi); + }, + "Perform narrow-phase CCD on the candidate.", "vertices_t0"_a, + "vertices_t1"_a, "min_distance"_a = 0.0, "tmax"_a = 1.0, + "narrow_phase_ccd"_a = DEFAULT_NARROW_PHASE_CCD) + .def( + "print_ccd_query", + [](const C& self, CR12 vertices_t0, CR12 vertices_t1) -> void { + self.write_ccd_query(std::cout, vertices_t0, vertices_t1); + }, + "Print the CCD query to cout.", "vertices_t0"_a, "vertices_t1"_a) + // -- type-independent helpers -------------------------------------- + .def_static( + "diag_distance_vector_outer", &C::diag_distance_vector_outer, + "Compute diag((dt/dx)(dt/dx)^T) efficiently (Eq. 11).", "coeffs"_a, + "dim"_a) + .def_static( + "diag_distance_vector_t_outer", &C::diag_distance_vector_t_outer, + "Compute diag((dt/dx * t)(dt/dx * t)^T) efficiently (Eq. 12).", + "coeffs"_a, "distance_vector"_a) + .def_static( + "contract_distance_vector_jacobian", + &C::contract_distance_vector_jacobian, + "Compute p^T (dt/dx) efficiently as sum(c_i * p_i) (Eqs. 13-14).", + "coeffs"_a, "p"_a, "dim"_a); +} + +} // namespace ipc diff --git a/python/src/candidates/vertex_vertex.cpp b/python/src/candidates/vertex_vertex.cpp index 36b1ac908..61c6b9edd 100644 --- a/python/src/candidates/vertex_vertex.cpp +++ b/python/src/candidates/vertex_vertex.cpp @@ -1,14 +1,15 @@ #include +#include "stencil_methods.hpp" + #include using namespace ipc; void define_vertex_vertex_candidate(py::module_& m) { - py::class_( - m, "VertexVertexCandidate") - .def(py::init(), "vertex0_id"_a, "vertex1_id"_a) + auto cls = py::class_(m, "VertexVertexCandidate"); + cls.def(py::init(), "vertex0_id"_a, "vertex1_id"_a) .def( py::init([](std::tuple vertex_ids) { return std::make_unique( @@ -42,4 +43,12 @@ void define_vertex_vertex_candidate(py::module_& m) py::implicitly_convertible< std::tuple, VertexVertexCandidate>(); + + define_stencil_methods(cls); + + // The adapter the collision types derive from; it carries the + // polymorphic stencil interface the POD candidate deliberately + // lacks. + py::class_( + m, "VertexVertexStencil"); } diff --git a/python/src/collisions/normal/edge_edge.cpp b/python/src/collisions/normal/edge_edge.cpp index 3a77f345e..197a262bd 100644 --- a/python/src/collisions/normal/edge_edge.cpp +++ b/python/src/collisions/normal/edge_edge.cpp @@ -6,7 +6,7 @@ using namespace ipc; void define_edge_edge_normal_collision(py::module_& m) { - py::class_( + py::class_( m, "EdgeEdgeNormalCollision") .def( py::init< diff --git a/python/src/collisions/normal/edge_vertex.cpp b/python/src/collisions/normal/edge_vertex.cpp index 96e941460..4facac540 100644 --- a/python/src/collisions/normal/edge_vertex.cpp +++ b/python/src/collisions/normal/edge_vertex.cpp @@ -6,7 +6,7 @@ using namespace ipc; void define_edge_vertex_normal_collision(py::module_& m) { - py::class_( + py::class_( m, "EdgeVertexNormalCollision") .def(py::init(), "edge_id"_a, "vertex_id"_a) .def(py::init(), "candidate"_a); diff --git a/python/src/collisions/normal/face_vertex.cpp b/python/src/collisions/normal/face_vertex.cpp index 3bc4e83e8..162e81016 100644 --- a/python/src/collisions/normal/face_vertex.cpp +++ b/python/src/collisions/normal/face_vertex.cpp @@ -6,7 +6,7 @@ using namespace ipc; void define_face_vertex_normal_collision(py::module_& m) { - py::class_( + py::class_( m, "FaceVertexNormalCollision") .def(py::init(), "", "face_id"_a, "vertex_id"_a) .def(py::init(), "candidate"_a); diff --git a/python/src/collisions/normal/plane_vertex.cpp b/python/src/collisions/normal/plane_vertex.cpp index abfe60ed0..6416c64f1 100644 --- a/python/src/collisions/normal/plane_vertex.cpp +++ b/python/src/collisions/normal/plane_vertex.cpp @@ -6,8 +6,7 @@ using namespace ipc; void define_plane_vertex_normal_collision(py::module_& m) { - py::class_< - PlaneVertexNormalCollision, PlaneVertexCandidate, NormalCollision>( + py::class_( m, "PlaneVertexNormalCollision") .def( py::init, index_t>(), "plane"_a, diff --git a/python/src/collisions/normal/vertex_vertex.cpp b/python/src/collisions/normal/vertex_vertex.cpp index c33261b39..f0b78a309 100644 --- a/python/src/collisions/normal/vertex_vertex.cpp +++ b/python/src/collisions/normal/vertex_vertex.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_vertex_vertex_normal_collision(py::module_& m) { py::class_< - VertexVertexNormalCollision, VertexVertexCandidate, NormalCollision>( + VertexVertexNormalCollision, VertexVertexStencil, NormalCollision>( m, "VertexVertexNormalCollision") .def(py::init(), "", "vertex0_id"_a, "vertex1_id"_a) .def(py::init(), "vv_candidate"_a); diff --git a/python/src/collisions/tangential/edge_edge.cpp b/python/src/collisions/tangential/edge_edge.cpp index 8441f75aa..f8d1516c3 100644 --- a/python/src/collisions/tangential/edge_edge.cpp +++ b/python/src/collisions/tangential/edge_edge.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_edge_edge_tangential_collision(py::module_& m) { py::class_< - EdgeEdgeTangentialCollision, EdgeEdgeCandidate, TangentialCollision>( + EdgeEdgeTangentialCollision, EdgeEdgeStencil, TangentialCollision>( m, "EdgeEdgeTangentialCollision") .def(py::init(), "collision"_a) .def( diff --git a/python/src/collisions/tangential/edge_vertex.cpp b/python/src/collisions/tangential/edge_vertex.cpp index 265cb2fff..9c98d020f 100644 --- a/python/src/collisions/tangential/edge_vertex.cpp +++ b/python/src/collisions/tangential/edge_vertex.cpp @@ -7,8 +7,8 @@ using namespace ipc; void define_edge_vertex_tangential_collision(py::module_& m) { py::class_< - EdgeVertexTangentialCollision, EdgeVertexCandidate, - TangentialCollision>(m, "EdgeVertexTangentialCollision") + EdgeVertexTangentialCollision, EdgeVertexStencil, TangentialCollision>( + m, "EdgeVertexTangentialCollision") .def(py::init(), "collision"_a) .def( py::init< diff --git a/python/src/collisions/tangential/face_vertex.cpp b/python/src/collisions/tangential/face_vertex.cpp index 315460f1e..f2c5d4f18 100644 --- a/python/src/collisions/tangential/face_vertex.cpp +++ b/python/src/collisions/tangential/face_vertex.cpp @@ -7,8 +7,8 @@ using namespace ipc; void define_face_vertex_tangential_collision(py::module_& m) { py::class_< - FaceVertexTangentialCollision, FaceVertexCandidate, - TangentialCollision>(m, "FaceVertexTangentialCollision") + FaceVertexTangentialCollision, FaceVertexStencil, TangentialCollision>( + m, "FaceVertexTangentialCollision") .def(py::init(), "collision"_a) .def( py::init< diff --git a/python/src/collisions/tangential/plane_vertex.cpp b/python/src/collisions/tangential/plane_vertex.cpp index 1a6ffe363..a800360eb 100644 --- a/python/src/collisions/tangential/plane_vertex.cpp +++ b/python/src/collisions/tangential/plane_vertex.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_plane_vertex_tangential_collision(py::module_& m) { py::class_< - PlaneVertexTangentialCollision, PlaneVertexCandidate, + PlaneVertexTangentialCollision, PlaneVertexStencil, TangentialCollision>(m, "PlaneVertexTangentialCollision") .def(py::init(), "collision"_a) .def( diff --git a/python/src/collisions/tangential/vertex_vertex.cpp b/python/src/collisions/tangential/vertex_vertex.cpp index f6a9c5142..5e19be7b3 100644 --- a/python/src/collisions/tangential/vertex_vertex.cpp +++ b/python/src/collisions/tangential/vertex_vertex.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_vertex_vertex_tangential_collision(py::module_& m) { py::class_< - VertexVertexTangentialCollision, VertexVertexCandidate, + VertexVertexTangentialCollision, VertexVertexStencil, TangentialCollision>(m, "VertexVertexTangentialCollision") .def(py::init(), "collision"_a) .def( diff --git a/src/ipc/barrier/adaptive_stiffness.cpp b/src/ipc/barrier/adaptive_stiffness.cpp index a77a4cf69..4aa697fb3 100644 --- a/src/ipc/barrier/adaptive_stiffness.cpp +++ b/src/ipc/barrier/adaptive_stiffness.cpp @@ -85,42 +85,6 @@ double update_barrier_stiffness( // License: Apache v2.0 // -double semi_implicit_stiffness( - const CollisionStencil& stencil, - Eigen::ConstRef vertices, - Eigen::ConstRef mass, - Eigen::ConstRef local_hess, - const double dmin) -{ - const unsigned N = stencil.num_vertices(); - assert(vertices.size() % N == 0); - const unsigned dim = stencil.dim(vertices.size()); - - const VectorMax4d value = stencil.compute_coefficients(vertices); - - // Compute the contact normal (i.e., the vector from the ) - VectorMax3d normal = VectorMax3d::Zero(dim); - for (unsigned i = 0; i < N; ++i) { - normal += value[i] * vertices.segment(dim * i, dim); - } - - // d² - const double distance = normal.norm() - dmin; - const double distance_sqr = distance * distance; - - // average mass: mᵢ = cᵀMc / ‖c‖² - const double avg_mass = - value.dot(mass.asDiagonal() * value) / value.squaredNorm(); - - VectorMax12d w = VectorMax12d::Zero(dim * N); - for (unsigned i = 0; i < N; ++i) { - w.segment(dim * i, dim) = value[i] * normal; - } - w.normalize(); - - return avg_mass / distance_sqr + w.dot(local_hess * w); -} - template Eigen::VectorXd semi_implicit_stiffness( const CollisionMesh& mesh, @@ -141,40 +105,42 @@ Eigen::VectorXd semi_implicit_stiffness( Eigen::VectorXd stiffnesses(collisions.size()); for (size_t ci = 0; ci < collisions.size(); ci++) { - const CollisionStencil& collision = collisions[ci]; - const unsigned N = collision.num_vertices(); + stiffnesses[ci] = collisions.visit(ci, [&](const auto& collision) { + const unsigned N = collision.num_vertices(); - const VectorMax12d positions = - collision.dof(vertices, mesh.edges(), mesh.faces()); + const VectorMax12d positions = + collision.dof(vertices, mesh.edges(), mesh.faces()); - std::array vertex_ids = - collision.vertex_ids(mesh.edges(), mesh.faces()); - if (hess.rows() == mesh.full_ndof()) { - for (int i = 0; i < N; i++) { - vertex_ids[i] = mesh.to_full_vertex_id(vertex_ids[i]); + std::array vertex_ids = + collision.vertex_ids(mesh.edges(), mesh.faces()); + if (hess.rows() == mesh.full_ndof()) { + for (int i = 0; i < N; i++) { + vertex_ids[i] = mesh.to_full_vertex_id(vertex_ids[i]); + } + } + + VectorMax4d local_mass(collision.num_vertices()); + for (unsigned i = 0; i < collision.num_vertices(); i++) { + local_mass[i] = vertex_masses[vertex_ids[i]]; } - } - - VectorMax4d local_mass(collision.num_vertices()); - for (unsigned i = 0; i < collision.num_vertices(); i++) { - local_mass[i] = vertex_masses[vertex_ids[i]]; - } - - MatrixMax12d local_hess = MatrixMax12d::Zero(dim * N, dim * N); - for (unsigned i = 0; i < N; ++i) { - for (unsigned j = 0; j < N; ++j) { - for (unsigned k = 0; k < dim; ++k) { - for (unsigned l = 0; l < dim; ++l) { - // NOTE: Assumes DOF are flattened in row-major order - local_hess(dim * i + k, dim * j + l) = hess.coeff( - dim * vertex_ids[i] + k, dim * vertex_ids[j] + l); + + MatrixMax12d local_hess = MatrixMax12d::Zero(dim * N, dim * N); + for (unsigned i = 0; i < N; ++i) { + for (unsigned j = 0; j < N; ++j) { + for (unsigned k = 0; k < dim; ++k) { + for (unsigned l = 0; l < dim; ++l) { + // NOTE: Assumes DOF are flattened in row-major order + local_hess(dim * i + k, dim * j + l) = hess.coeff( + dim * vertex_ids[i] + k, + dim * vertex_ids[j] + l); + } } } } - } - stiffnesses[ci] = semi_implicit_stiffness( - collision, positions, local_mass, local_hess, dmin); + return semi_implicit_stiffness( + collision, positions, local_mass, local_hess, dmin); + }); } return stiffnesses; diff --git a/src/ipc/barrier/adaptive_stiffness.hpp b/src/ipc/barrier/adaptive_stiffness.hpp index 306a5e0c8..70a72e4db 100644 --- a/src/ipc/barrier/adaptive_stiffness.hpp +++ b/src/ipc/barrier/adaptive_stiffness.hpp @@ -59,12 +59,43 @@ double update_barrier_stiffness( /// @param local_hess Local hessian of the elasticity energy function. /// @param dmin Minimum distance between elements. /// @return The semi-implicit stiffness. +/// @tparam StencilT The stencil type; any candidate or collision. +template double semi_implicit_stiffness( - const CollisionStencil& stencil, + const StencilT& stencil, Eigen::ConstRef vertices, Eigen::ConstRef mass, Eigen::ConstRef local_hess, - const double dmin); + const double dmin) +{ + const unsigned N = stencil.num_vertices(); + assert(vertices.size() % N == 0); + const unsigned dim = stencil.dim(vertices.size()); + + const VectorMax4d value = stencil.compute_coefficients(vertices); + + // Compute the contact normal (i.e., the vector from the ) + VectorMax3d normal = VectorMax3d::Zero(dim); + for (unsigned i = 0; i < N; ++i) { + normal += value[i] * vertices.segment(dim * i, dim); + } + + // d² + const double distance = normal.norm() - dmin; + const double distance_sqr = distance * distance; + + // average mass: mᵢ = cᵀMc / ‖c‖² + const double avg_mass = + value.dot(mass.asDiagonal() * value) / value.squaredNorm(); + + VectorMax12d w = VectorMax12d::Zero(dim * N); + for (unsigned i = 0; i < N; ++i) { + w.segment(dim * i, dim) = value[i] * normal; + } + w.normalize(); + + return avg_mass / distance_sqr + w.dot(local_hess * w); +} /// @brief Compute the semi-implicit stiffness's for all collisions. /// @note See [Ando 2024] for details. diff --git a/src/ipc/candidates/CMakeLists.txt b/src/ipc/candidates/CMakeLists.txt index be0fc3139..a7155feef 100644 --- a/src/ipc/candidates/CMakeLists.txt +++ b/src/ipc/candidates/CMakeLists.txt @@ -1,8 +1,9 @@ set(SOURCES candidates.cpp candidates.hpp - collision_stencil.cpp collision_stencil.hpp + stencil_adapter.hpp + stencil_mixin.hpp edge_edge.cpp edge_edge.hpp edge_face.cpp diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index b9da17d8e..5277e641c 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -248,13 +248,13 @@ bool Candidates::is_step_collision_free( // Narrow phase for (size_t i = 0; i < size(); i++) { - const CollisionStencil& candidate = (*this)[i]; - - double toi; - bool is_collision = candidate.ccd( - candidate.dof(vertices_t0, mesh.edges(), mesh.faces()), - candidate.dof(vertices_t1, mesh.edges(), mesh.faces()), // - toi, min_distance, /*tmax=*/1.0, narrow_phase_ccd); + const bool is_collision = visit(i, [&](const auto& candidate) { + double toi; + return candidate.ccd( + candidate.dof(vertices_t0, mesh.edges(), mesh.faces()), + candidate.dof(vertices_t1, mesh.edges(), mesh.faces()), // + toi, min_distance, /*tmax=*/1.0, narrow_phase_ccd); + }); if (is_collision) { return false; @@ -284,13 +284,13 @@ double Candidates::compute_collision_free_stepsize( tbb::parallel_for(size_t(0), size(), [&](size_t i) { double tmax = earliest_toi.load(std::memory_order_relaxed); - const CollisionStencil& candidate = (*this)[i]; - double toi = std::numeric_limits::infinity(); // output - const bool are_colliding = candidate.ccd( - candidate.dof(vertices_t0, mesh.edges(), mesh.faces()), - candidate.dof(vertices_t1, mesh.edges(), mesh.faces()), // - toi, min_distance, tmax, narrow_phase_ccd); + const bool are_colliding = visit(i, [&](const auto& candidate) { + return candidate.ccd( + candidate.dof(vertices_t0, mesh.edges(), mesh.faces()), + candidate.dof(vertices_t1, mesh.edges(), mesh.faces()), // + toi, min_distance, tmax, narrow_phase_ccd); + }); if (are_colliding) { // Update the earliest time of impact (TOI) atomically @@ -326,12 +326,15 @@ double Candidates::compute_noncandidate_conservative_stepsize( } tbb::parallel_for(size_t(0), size(), [&](size_t i) { - for (const index_t vid : (*this)[i].vertex_ids(E, F)) { - if (vid < 0) { - break; + visit(i, [&](const auto& candidate) { + for (const index_t vid : candidate.vertex_ids(E, F)) { + if (vid < 0) { + break; + } + is_vertex_a_candidates[vid].store( + true, std::memory_order_relaxed); } - is_vertex_a_candidates[vid].store(true, std::memory_order_relaxed); - } + }); }); double max_displacement = tbb::parallel_reduce( @@ -393,23 +396,24 @@ Eigen::VectorXd Candidates::compute_per_vertex_safe_distances( } tbb::parallel_for(size_t(0), size(), [&](size_t i) { - const CollisionStencil& candidate = (*this)[i]; - - const double d = sqrt(candidate.compute_distance( - vertices, mesh.edges(), mesh.faces())) - - min_distance; - - // Compute the distance for each vertex in the candidate - for (auto vid : candidate.vertex_ids(mesh.edges(), mesh.faces())) { - if (vid < 0) { - break; // No more vertices in this candidate + visit(i, [&](const auto& candidate) { + const double d = sqrt(candidate.compute_distance( + vertices, mesh.edges(), mesh.faces())) + - min_distance; + + // Compute the distance for each vertex in the candidate + for (auto vid : candidate.vertex_ids(mesh.edges(), mesh.faces())) { + if (vid < 0) { + break; // No more vertices in this candidate + } + // Update the minimum distance atomically + double old_val = + min_distances[vid].load(std::memory_order_relaxed); + while (d < old_val + && !min_distances[vid].compare_exchange_weak( + old_val, d, std::memory_order_relaxed)) { } } - // Update the minimum distance atomically - double old_val = min_distances[vid].load(std::memory_order_relaxed); - while (d < old_val - && !min_distances[vid].compare_exchange_weak( - old_val, d, std::memory_order_relaxed)) { } - } + }); }); // Convert atomic distances to a vector @@ -515,62 +519,10 @@ void Candidates::clear() m_sets.reset(); } -CollisionStencil& Candidates::operator[](size_t i) -{ - // We keep the requested index for the error message, since i is reduced - // below as we walk past each list of candidates. - const size_t requested = i; - if (i < vv_candidates.size()) { - return vv_candidates[i]; - } - i -= vv_candidates.size(); - if (i < ev_candidates.size()) { - return ev_candidates[i]; - } - i -= ev_candidates.size(); - if (i < ee_candidates.size()) { - return ee_candidates[i]; - } - i -= ee_candidates.size(); - if (i < fv_candidates.size()) { - return fv_candidates[i]; - } - i -= fv_candidates.size(); - if (i < pv_candidates.size()) { - return pv_candidates[i]; - } - log_and_throw_error( - "Candidate index {} is out of range! Total candidates: {}", requested, - size()); -} - -const CollisionStencil& Candidates::operator[](size_t i) const +void Candidates::throw_out_of_range(const size_t i, const size_t size) { - // We keep the requested index for the error message, since i is reduced - // below as we walk past each list of candidates. - const size_t requested = i; - if (i < vv_candidates.size()) { - return vv_candidates[i]; - } - i -= vv_candidates.size(); - if (i < ev_candidates.size()) { - return ev_candidates[i]; - } - i -= ev_candidates.size(); - if (i < ee_candidates.size()) { - return ee_candidates[i]; - } - i -= ee_candidates.size(); - if (i < fv_candidates.size()) { - return fv_candidates[i]; - } - i -= fv_candidates.size(); - if (i < pv_candidates.size()) { - return pv_candidates[i]; - } log_and_throw_error( - "Candidate index {} is out of range! Total candidates: {}", requested, - size()); + "Candidate index {} is out of range! Total candidates: {}", i, size); } bool Candidates::is_vertex_vertex(size_t i) const diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index 839c3f7c5..bfbacc3b7 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -67,15 +68,66 @@ class Candidates { /// @brief Clear all collision candidates. void clear(); - /// @brief Get a collision stencil by index. - /// @param i The index of the collision stencil. - /// @return A reference to the collision stencil. - CollisionStencil& operator[](size_t i); +private: + /// @brief Throw the std::out_of_range for a bad visit() index. + [[noreturn]] static void throw_out_of_range(size_t i, size_t size); - /// @brief Get a collision stencil by index. - /// @param i The index of the collision stencil. - /// @return A const reference to the collision stencil. - const CollisionStencil& operator[](size_t i) const; + /// @brief The body of both visit() overloads. + /// + /// Self is Candidates or const Candidates, so the visitor is handed a + /// mutable or const candidate to match the object it was called on. + template + static decltype(auto) visit_impl(Self& self, size_t i, Visitor&& f) + { + // We keep the requested index for the error message, since i is + // reduced below as we walk past each list of candidates. + const size_t requested = i; + if (i < self.vv_candidates.size()) { + return f(self.vv_candidates[i]); + } + i -= self.vv_candidates.size(); + if (i < self.ev_candidates.size()) { + return f(self.ev_candidates[i]); + } + i -= self.ev_candidates.size(); + if (i < self.ee_candidates.size()) { + return f(self.ee_candidates[i]); + } + i -= self.ee_candidates.size(); + if (i < self.fv_candidates.size()) { + return f(self.fv_candidates[i]); + } + i -= self.fv_candidates.size(); + if (i < self.pv_candidates.size()) { + return f(self.pv_candidates[i]); + } + throw_out_of_range(requested, self.size()); + } + +public: + /// @brief Apply a visitor to the candidate at index i. + /// + /// The candidate types are not polymorphic, so they cannot be handed out + /// through a common base reference. The visitor is called with the concrete + /// candidate type instead, which also means the stencil operations it calls + /// are resolved statically. + /// + /// @param i The index of the candidate. + /// @param f The visitor, callable with each of the five candidate types. + /// @return Whatever the visitor returns. + /// @throws std::out_of_range if i is not a valid index. + template decltype(auto) visit(size_t i, Visitor&& f) + { + return visit_impl(*this, i, std::forward(f)); + } + + /// @brief Apply a visitor to the candidate at index i. + /// @see visit(size_t, Visitor&&) + template + decltype(auto) visit(size_t i, Visitor&& f) const + { + return visit_impl(*this, i, std::forward(f)); + } /// @brief Get if the collision at i is a vertex-vertex collision. /// @param i The index of the collision. diff --git a/src/ipc/candidates/collision_stencil.cpp b/src/ipc/candidates/collision_stencil.cpp deleted file mode 100644 index c41c41b27..000000000 --- a/src/ipc/candidates/collision_stencil.cpp +++ /dev/null @@ -1,149 +0,0 @@ -#include "collision_stencil.hpp" - -#include - -namespace ipc { - -VectorMax3d CollisionStencil::compute_distance_vector( - Eigen::ConstRef positions) const -{ - VectorMax4d _; // Unused output - return compute_distance_vector(positions, _); -} - -VectorMax3d CollisionStencil::compute_distance_vector( - Eigen::ConstRef positions, VectorMax4d& coeffs) const -{ - const int n = num_vertices(); - const int d = dim(positions.size()); - coeffs = compute_coefficients(positions); - VectorMax3d dv = VectorMax3d::Zero(d); - for (int i = 0; i < n; i++) { - dv += coeffs[i] * positions.segment(d * i, d); - } - return dv; -} - -MatrixMax CollisionStencil::compute_distance_vector_jacobian( - Eigen::ConstRef positions) const -{ - const int n = num_vertices(); - const int d = dim(positions.size()); - const int ndof = n * d; - const VectorMax4d c = compute_coefficients(positions); - // ∂t/∂x is ndof × dim - // Each block row i is cᵢ * I_{d×d} - MatrixMax J; - J.setZero(ndof, d); - for (int i = 0; i < n; i++) { - J.block(i * d, 0, d, d).diagonal().array() = c[i]; - } - return J; -} - -VectorMax12d CollisionStencil::diag_distance_vector_outer( - Eigen::ConstRef coeffs, const int d) -{ - const int n = coeffs.size(); - VectorMax12d diag(n * d); - for (int i = 0; i < n; i++) { - const double ci2 = coeffs[i] * coeffs[i]; - diag.segment(i * d, d).setConstant(ci2); - } - return diag; -} - -VectorMax12d CollisionStencil::diag_distance_vector_t_outer( - Eigen::ConstRef coeffs, - Eigen::ConstRef distance_vector) -{ - const int n = coeffs.size(); - const int d = distance_vector.size(); - const VectorMax3d t2 = distance_vector.array().square(); - VectorMax12d diag(n * d); - for (int i = 0; i < n; i++) { - diag.segment(i * d, d).array() = (coeffs[i] * coeffs[i]) * t2; - } - return diag; -} - -VectorMax3d CollisionStencil::contract_distance_vector_jacobian( - Eigen::ConstRef coeffs, - Eigen::ConstRef p, - const int d) -{ - const int n = coeffs.size(); - VectorMax3d result = VectorMax3d::Zero(d); - for (int i = 0; i < n; i++) { - result += coeffs[i] * p.segment(d * i, d); - } - return result; -} - -VectorMax3d CollisionStencil::compute_normal( - Eigen::ConstRef positions, - bool flip_if_negative, - double* sign) const -{ - const int dim = this->dim(positions.size()); - - VectorMax3d n = compute_unnormalized_normal(positions).normalized(); - - if (sign != nullptr) { - *sign = (positions.head(dim) - positions.tail(dim)).dot(n) < 0 ? -1 : 1; - } - - // Flip the normal if the point is on the negative side. - // Any point on the second object will do, so we use the last point. - if (flip_if_negative - && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) { - n *= -1; - } - - return n; -} - -MatrixMax CollisionStencil::compute_normal_jacobian( - Eigen::ConstRef positions, bool flip_if_negative) const -{ - const int dim = this->dim(positions.size()); - - const VectorMax3d n = compute_unnormalized_normal(positions); - - MatrixMax dn = normalization_jacobian(n) - * compute_unnormalized_normal_jacobian(positions); - - if (flip_if_negative - && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) { - dn *= -1; - } - - return dn; -} - -std::ostream& CollisionStencil::write_ccd_query( - std::ostream& out, - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1) const -{ - assert(vertices_t0.size() == vertices_t1.size()); - - const int dim = vertices_t0.size() / num_vertices(); - assert(vertices_t0.size() % num_vertices() == 0); - - for (int i = 0; i < num_vertices(); i++) { - out << vertices_t0.segment(dim * i, dim) - .transpose() - .format(OBJ_VERTEX_FORMAT); - } - - for (int i = 0; i < num_vertices(); i++) { - out << vertices_t1.segment(dim * i, dim) - .transpose() - .format(OBJ_VERTEX_FORMAT); - } - - return out; -} - -} // namespace ipc diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index 71731675c..a57e97727 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -1,21 +1,25 @@ #pragma once #include +#include #include #include #include -#include -#include +#include namespace ipc { /// @brief A stencil representing a collision between at most four vertices. -class CollisionStencil { -public: - /// @brief The maximum number of vertices in a collision stencil. - static constexpr int STENCIL_SIZE = 4; +/// +/// The runtime-polymorphic stencil, used where the stencil type is not known +/// statically. The operations built on top of the primitives below live in +/// StencilMixin, which this shares with the non-polymorphic candidate types. +class CollisionStencil : public StencilMixin { + // So the mixin can reach the protected unnormalized-normal primitives. + friend class StencilMixin; +public: virtual ~CollisionStencil() = default; /// @brief Get this as a child collision type. @@ -46,17 +50,9 @@ class CollisionStencil { /// @brief Get the number of vertices in the collision stencil. virtual int num_vertices() const = 0; + /// @brief A short name for the stencil type, for debug output. virtual std::string name() const { return "unknown"; } - /// @brief Get the dimension of the collision stencil. - /// @param ndof Number of degrees of freedom in the stencil. - /// @return The dimension of the collision stencil. - int dim(const int ndof) const - { - assert(ndof % num_vertices() == 0); - return ndof / num_vertices(); - } - /// @brief Get the vertex IDs of the collision stencil. /// @param edges Collision mesh edges /// @param faces Collision mesh faces @@ -65,149 +61,12 @@ class CollisionStencil { Eigen::ConstRef edges, Eigen::ConstRef faces) const = 0; - /// @brief Get the vertex attributes of the collision stencil. - /// @param vertices Vertex attributes - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @return The vertex positions of the collision stencil. Elements i > num_vertices() are NaN. - std::array vertices( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - constexpr double NaN = std::numeric_limits::signaling_NaN(); - - const auto vertex_ids = this->vertex_ids(edges, faces); - - std::array stencil_vertices; - for (int i = 0; i < STENCIL_SIZE; i++) { - if (vertex_ids[i] >= 0) { - stencil_vertices[i] = vertices.row(vertex_ids[i]); - } else { - stencil_vertices[i].setConstant(vertices.cols(), NaN); - } - } - - return stencil_vertices; - } - - /// @brief Select this stencil's DOF from the full matrix of DOF. - /// @param X Full matrix of DOF (rowwise). - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @return This stencil's DOF. - VectorMax12d - dof(Eigen::ConstRef X, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - const int dim = X.cols(); - VectorMax12d x(num_vertices() * dim); - const auto idx = vertex_ids(edges, faces); - for (int i = 0; i < num_vertices(); i++) { - x.segment(i * dim, dim) = X.row(idx[i]); - } - return x; - } - - /// @brief Compute the distance of the stencil. - /// @param vertices Collision mesh vertices - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @return Distance of the stencil. - double compute_distance( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - return compute_distance(dof(vertices, edges, faces)); - } - - /// @brief Compute the distance gradient of the stencil w.r.t. the stencil's vertex positions. - /// @param vertices Collision mesh vertices - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @return Distance gradient of the stencil w.r.t. the stencil's vertex positions. - VectorMax12d compute_distance_gradient( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - return compute_distance_gradient(dof(vertices, edges, faces)); - } - - /// @brief Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions. - /// @param vertices Collision mesh vertices - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @return Distance Hessian of the stencil w.r.t. the stencil's vertex positions. - MatrixMax12d compute_distance_hessian( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - return compute_distance_hessian(dof(vertices, edges, faces)); - } - - /// @brief Compute the coefficients of the stencil s.t. \f$d(x) = \|\sum c_i \mathbf{x}_i\|^2\f$. - /// @param vertices Collision mesh vertices - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @return Coefficients of the stencil. - VectorMax4d compute_coefficients( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - return compute_coefficients(dof(vertices, edges, faces)); - } - - /// @brief Compute the distance vector using the mesh vertices. - /// @param vertices Collision mesh vertices. - /// @param edges Collision mesh edges. - /// @param faces Collision mesh faces. - /// @return The distance vector (dim-dimensional). - VectorMax3d compute_distance_vector( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces) const - { - return compute_distance_vector(dof(vertices, edges, faces)); - } - - /// @brief Compute the normal of the stencil. - /// @param vertices Collision mesh vertices - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @param flip_if_negative If true, flip the normal if the point is on the negative side. - /// @param sign If not nullptr, set to the sign of the normal before any flipping. - /// @return Normal of the stencil. - VectorMax3d compute_normal( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - const bool flip_if_negative = true, - double* sign = nullptr) const - { - return compute_normal( - dof(vertices, edges, faces), flip_if_negative, sign); - } - - /// @brief Compute the Jacobian of the normal of the stencil. - /// @param vertices Collision mesh vertices - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @param flip_if_negative If true, flip the normal if the point is on the negative side. - /// @return Jacobian of the normal of the stencil. - MatrixMax compute_normal_jacobian( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - const bool flip_if_negative = true) const - { - return compute_normal_jacobian( - dof(vertices, edges, faces), flip_if_negative); - } + // The mesh-based overloads live in the mixin; naming the positions-based + // ones below would otherwise hide them. + using StencilMixin::compute_distance; + using StencilMixin::compute_distance_gradient; + using StencilMixin::compute_distance_hessian; + using StencilMixin::compute_coefficients; // ---------------------------------------------------------------------- // NOTE: The following functions take stencil vertices as output by dof() @@ -240,107 +99,6 @@ class CollisionStencil { virtual VectorMax4d compute_coefficients(Eigen::ConstRef positions) const = 0; - // ------------------------------------------------------------------ - // Efficient distance-vector based methods [Shen et al. 2024] - // - // The distance vector t = ∑ cᵢ xᵢ is the vector between the - // closest points, where cᵢ are the coefficients and xᵢ are vertex - // positions. Then d = ‖t‖ and d² = tᵀt. - // - // ∂t/∂x = [c₀ I, c₁ I, c₂ I, c₃ I]ᵀ ∈ ℝ^{n×dim} - // ∂²t/∂x² = 0 - // - // These allow efficient computation of: - // - diag(∂²b/∂x²) without forming the full Hessian - // - pᵀ(∂²b/∂x²)p without forming the full Hessian - // ------------------------------------------------------------------ - - /// @brief Compute the distance vector of the stencil: t = ∑ cᵢ xᵢ. - /// - /// The distance vector is the vector between the closest points on the - /// collision primitives. Its squared norm equals the squared distance. - /// - /// @param positions Stencil's vertex positions. - /// @note positions can be computed as stencil.dof(vertices, edges, faces) - /// @return The distance vector (dim-dimensional, i.e., 2D or 3D). - VectorMax3d - compute_distance_vector(Eigen::ConstRef positions) const; - - /// @brief Compute the distance vector and the coefficients together. - /// @param positions Stencil's vertex positions. - /// @param[out] coeffs The computed coefficients cᵢ. - /// @return The distance vector. - VectorMax3d compute_distance_vector( - Eigen::ConstRef positions, VectorMax4d& coeffs) const; - - /// @brief Compute the Jacobian of the distance vector w.r.t. positions: ∂t/∂x = [c₀I c₁I ... cₙI]ᵀ. - /// - /// Since ∂t/∂x has a very simple structure (block-diagonal with scalar - /// coefficients times identity), many operations can be done without - /// forming this matrix. This method is provided for completeness and - /// verification. - /// - /// @param positions Stencil's vertex positions. - /// @return The Jacobian ∂t/∂x ∈ ℝ^{ndof × dim}. - MatrixMax compute_distance_vector_jacobian( - Eigen::ConstRef positions) const; - - /// @brief Compute diag((∂t/∂x)(∂t/∂x)ᵀ) efficiently (Eq. 11 of the paper). - /// - /// Result is [c₀², c₀², c₀², c₁², c₁², c₁², ...] (each cᵢ² repeated dim - /// times). This is used for computing diag(∂²b/∂x²) without the full - /// Hessian. - /// - /// @param coeffs The coefficients cᵢ (from compute_coefficients). - /// @param d The spatial dimension (2 or 3). - /// @return The diagonal of (∂t/∂x)(∂t/∂x)ᵀ as a vector of size ndof. - static VectorMax12d diag_distance_vector_outer( - Eigen::ConstRef coeffs, const int d); - - /// @brief Compute diag((∂t/∂x · t)(∂t/∂x · t)ᵀ) efficiently (Eq. 12). - /// - /// Result is element-wise square of [c₀tᵀ, c₁tᵀ, ..., cₙtᵀ]. - /// This is used for computing diag(∂²b/∂x²) without the full Hessian. - /// - /// @param coeffs The coefficients cᵢ. - /// @param distance_vector The distance vector t. - /// @return The diagonal of (∂t/∂x·t)(∂t/∂x·t)ᵀ as a vector of size ndof. - static VectorMax12d diag_distance_vector_t_outer( - Eigen::ConstRef coeffs, - Eigen::ConstRef distance_vector); - - /// @brief Compute pᵀ(∂t/∂x) efficiently as ∑ cᵢ pᵢ (Eqs. 13-14). - /// - /// Given p = [p₀, p₁, ..., pₙ]ᵀ where pᵢ ∈ ℝ^dim, this computes - /// pᵀ(∂t/∂x) = ∑ cᵢ pᵢ which is a dim-dimensional vector. - /// - /// @param coeffs The coefficients cᵢ. - /// @param p A vector of size ndof (the direction for the quadratic form). - /// @param d The spatial dimension (2 or 3). - /// @return pᵀ(∂t/∂x) as a dim-dimensional vector. - static VectorMax3d contract_distance_vector_jacobian( - Eigen::ConstRef coeffs, - Eigen::ConstRef p, - const int d); - - /// @brief Compute the normal of the stencil. - /// @param positions Stencil's vertex positions. - /// @param flip_if_negative If true, flip the normal if the point is on the negative side. - /// @param sign If not nullptr, set to the sign of the normal before any flipping. - /// @return Normal of the stencil. - VectorMax3d compute_normal( - Eigen::ConstRef positions, - bool flip_if_negative = true, - double* sign = nullptr) const; - - /// @brief Compute the Jacobian of the normal of the stencil. - /// @param positions Stencil's vertex positions. - /// @param flip_if_negative If true, flip the normal if the point is on the negative side. - /// @return Jacobian of the normal of the stencil. - MatrixMax compute_normal_jacobian( - Eigen::ConstRef positions, - bool flip_if_negative = true) const; - /// @brief Perform narrow-phase CCD on the candidate. /// @param[in] vertices_t0 Stencil vertices at the start of the time step. /// @param[in] vertices_t1 Stencil vertices at the end of the time step. @@ -358,16 +116,6 @@ class CollisionStencil { const NarrowPhaseCCD& narrow_phase_ccd = DEFAULT_NARROW_PHASE_CCD) const = 0; - /// @brief Write the CCD query to a stream. - /// @param out Stream to write to. - /// @param vertices_t0 Stencil vertices at the start of the time step. - /// @param vertices_t1 Stencil vertices at the end of the time step. - /// @return The stream. - std::ostream& write_ccd_query( - std::ostream& out, - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1) const; - protected: /// @brief Compute the unnormalized normal of the stencil. /// @param positions Stencil's vertex positions. diff --git a/src/ipc/candidates/edge_edge.cpp b/src/ipc/candidates/edge_edge.cpp index fd040b146..af9f669b0 100644 --- a/src/ipc/candidates/edge_edge.cpp +++ b/src/ipc/candidates/edge_edge.cpp @@ -13,34 +13,38 @@ EdgeEdgeCandidate::EdgeEdgeCandidate(index_t _edge0_id, index_t _edge1_id) } double EdgeEdgeCandidate::compute_distance( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype) const { assert(positions.size() == 12); return edge_edge_distance( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), - positions.tail<3>(), known_dtype()); + positions.tail<3>(), dtype); } VectorMax12d EdgeEdgeCandidate::compute_distance_gradient( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype) const { assert(positions.size() == 12); return edge_edge_distance_gradient( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), - positions.tail<3>(), known_dtype()); + positions.tail<3>(), dtype); } MatrixMax12d EdgeEdgeCandidate::compute_distance_hessian( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype) const { assert(positions.size() == 12); return edge_edge_distance_hessian( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), - positions.tail<3>(), known_dtype()); + positions.tail<3>(), dtype); } VectorMax4d EdgeEdgeCandidate::compute_coefficients( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype) const { assert(positions.size() == 12); Eigen::ConstRef ea0 = positions.head<3>(); @@ -49,13 +53,13 @@ VectorMax4d EdgeEdgeCandidate::compute_coefficients( Eigen::ConstRef eb1 = positions.tail<3>(); // Project the point inside the triangle - auto dtype = known_dtype(); - if (dtype == EdgeEdgeDistanceType::AUTO) { - dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + EdgeEdgeDistanceType resolved_dtype = dtype; + if (resolved_dtype == EdgeEdgeDistanceType::AUTO) { + resolved_dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); } VectorMax4d coeffs(4); - switch (dtype) { + switch (resolved_dtype) { case EdgeEdgeDistanceType::EA0_EB0: coeffs << 1.0, 0.0, -1.0, 0.0; break; diff --git a/src/ipc/candidates/edge_edge.hpp b/src/ipc/candidates/edge_edge.hpp index 8660f63e4..d1feeacad 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -1,6 +1,7 @@ #pragma once -#include +#include +#include #include #include @@ -10,16 +11,22 @@ namespace ipc { /// @brief A candidate for edge-edge collision detection. -class EdgeEdgeCandidate : virtual public CollisionStencil { +class EdgeEdgeCandidate : public StencilMixin { + friend class StencilMixin; + public: + /// @brief Construct a candidate with indeterminate edge IDs. + /// @note Keeping this trivial is what makes the type trivially copyable. + EdgeEdgeCandidate() = default; + EdgeEdgeCandidate(index_t edge0_id, index_t edge1_id); // ------------------------------------------------------------------------ - // CollisionStencil + // Stencil - int num_vertices() const override { return 4; } + constexpr static int num_vertices() { return 4; } - std::string name() const override { return "ee"; } + static std::string name() { return "ee"; } /// @brief Get the vertex IDs for the edge-edge pair /// @param edges The edge connectivity matrix @@ -27,28 +34,34 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { /// @return An array of vertex IDs in the order: [ea0i, ea1i, eb0i, eb1i] std::array vertex_ids( Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + Eigen::ConstRef faces) const { return { { edges(edge0_id, 0), edges(edge0_id, 1), // edges(edge1_id, 0), edges(edge1_id, 1) } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::compute_distance_hessian; + using StencilMixin::compute_coefficients; + using StencilMixin::compute_distance; + using StencilMixin::compute_distance_gradient; + using StencilMixin::compute_distance_hessian; - double - compute_distance(Eigen::ConstRef positions) const override; + /// @param dtype The distance type, when it is known a priori. + /// Defaults to AUTO, which classifies the pair from the positions. + double compute_distance( + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) const; VectorMax12d compute_distance_gradient( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) const; MatrixMax12d compute_distance_hessian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) const; VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) const; // ------------------------------------------------------------------------ @@ -59,11 +72,12 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { const double min_distance = 0.0, const double tmax = 1.0, const NarrowPhaseCCD& narrow_phase_ccd = - DEFAULT_NARROW_PHASE_CCD) const override; + DEFAULT_NARROW_PHASE_CCD) const; // ------------------------------------------------------------------------ - virtual EdgeEdgeDistanceType known_dtype() const + /// @brief The distance type known a priori; AUTO for a plain candidate. + constexpr static EdgeEdgeDistanceType known_dtype() { return EdgeEdgeDistanceType::AUTO; } @@ -87,11 +101,18 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { index_t edge1_id; protected: - VectorMax3d compute_unnormalized_normal( - Eigen::ConstRef positions) const override; + VectorMax3d + compute_unnormalized_normal(Eigen::ConstRef positions) const; MatrixMax compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions) const; }; +/// @brief EdgeEdgeCandidate with the runtime-polymorphic stencil interface. +/// +/// The base of the edge-edge collision types, which pin the distance type by +/// overriding known_dtype(). +using EdgeEdgeStencil = + DTypeStencilAdapter; + } // namespace ipc diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index c02edd6cb..e33670733 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -13,49 +13,53 @@ EdgeVertexCandidate::EdgeVertexCandidate(index_t _edge_id, index_t _vertex_id) } double EdgeVertexCandidate::compute_distance( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype) const { assert(positions.size() == 6 || positions.size() == 9); const int dim = this->dim(positions.size()); return point_edge_distance( positions.head(dim), positions.segment(dim, dim), positions.tail(dim), - known_dtype()); + dtype); } VectorMax12d EdgeVertexCandidate::compute_distance_gradient( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype) const { assert(positions.size() == 6 || positions.size() == 9); // Branching here is faster (3.1x) than passing dynamic slices. if (positions.size() == 6) { return point_edge_distance_gradient( positions.head<2>(), positions.segment<2>(2), positions.tail<2>(), - known_dtype()); + dtype); } else { return point_edge_distance_gradient( positions.head<3>(), positions.segment<3>(3), positions.tail<3>(), - known_dtype()); + dtype); } } MatrixMax12d EdgeVertexCandidate::compute_distance_hessian( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype) const { assert(positions.size() == 6 || positions.size() == 9); // Branching here is faster than passing dynamic slices. if (positions.size() == 6) { return point_edge_distance_hessian( positions.head<2>(), positions.segment<2>(2), positions.tail<2>(), - known_dtype()); + dtype); } else { return point_edge_distance_hessian( positions.head<3>(), positions.segment<3>(3), positions.tail<3>(), - known_dtype()); + dtype); } } VectorMax4d EdgeVertexCandidate::compute_coefficients( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype) const { assert(positions.size() == 6 || positions.size() == 9); const int dim = this->dim(positions.size()); @@ -63,13 +67,13 @@ VectorMax4d EdgeVertexCandidate::compute_coefficients( Eigen::ConstRef t0 = positions.segment(dim, dim); Eigen::ConstRef t1 = positions.tail(dim); - auto dtype = known_dtype(); - if (dtype == PointEdgeDistanceType::AUTO) { - dtype = point_edge_distance_type(p, t0, t1); + PointEdgeDistanceType resolved_dtype = dtype; + if (resolved_dtype == PointEdgeDistanceType::AUTO) { + resolved_dtype = point_edge_distance_type(p, t0, t1); } VectorMax4d coeffs(3); - switch (dtype) { + switch (resolved_dtype) { case PointEdgeDistanceType::P_E0: coeffs << 1.0, -1.0, 0.0; break; diff --git a/src/ipc/candidates/edge_vertex.hpp b/src/ipc/candidates/edge_vertex.hpp index b48f30012..4b9ab8558 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -1,6 +1,7 @@ #pragma once -#include +#include +#include #include #include @@ -10,16 +11,22 @@ namespace ipc { /// @brief A candidate for edge-vertex collision detection. -class EdgeVertexCandidate : virtual public CollisionStencil { +class EdgeVertexCandidate : public StencilMixin { + friend class StencilMixin; + public: + /// @brief Construct a candidate with indeterminate edge IDs. + /// @note Keeping this trivial is what makes the type trivially copyable. + EdgeVertexCandidate() = default; + EdgeVertexCandidate(index_t edge_id, index_t vertex_id); // ------------------------------------------------------------------------ - // CollisionStencil + // Stencil - int num_vertices() const override { return 3; } + constexpr static int num_vertices() { return 3; } - std::string name() const override { return "ev"; } + static std::string name() { return "ev"; } /// @brief Get the vertex IDs for the edge-vertex pair /// @param edges The edge connectivity matrix @@ -27,27 +34,31 @@ class EdgeVertexCandidate : virtual public CollisionStencil { /// @return An array of vertex IDs in the order: [vi, e0i, e1i, -1] std::array vertex_ids( Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + Eigen::ConstRef faces) const { return { { vertex_id, edges(edge_id, 0), edges(edge_id, 1), -1 } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::compute_distance_hessian; + using StencilMixin::compute_coefficients; + using StencilMixin::compute_distance; + using StencilMixin::compute_distance_gradient; + using StencilMixin::compute_distance_hessian; - double - compute_distance(Eigen::ConstRef positions) const override; + double compute_distance( + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) const; VectorMax12d compute_distance_gradient( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) const; MatrixMax12d compute_distance_hessian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) const; VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) const; // ------------------------------------------------------------------------ @@ -58,11 +69,11 @@ class EdgeVertexCandidate : virtual public CollisionStencil { const double min_distance = 0.0, const double tmax = 1.0, const NarrowPhaseCCD& narrow_phase_ccd = - DEFAULT_NARROW_PHASE_CCD) const override; + DEFAULT_NARROW_PHASE_CCD) const; // ------------------------------------------------------------------------ - virtual PointEdgeDistanceType known_dtype() const + constexpr static PointEdgeDistanceType known_dtype() { return PointEdgeDistanceType::AUTO; } @@ -84,11 +95,17 @@ class EdgeVertexCandidate : virtual public CollisionStencil { index_t vertex_id; protected: - VectorMax3d compute_unnormalized_normal( - Eigen::ConstRef positions) const override; + VectorMax3d + compute_unnormalized_normal(Eigen::ConstRef positions) const; MatrixMax compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions) const; }; +/// @brief EdgeVertexCandidate with the runtime-polymorphic stencil interface. +/// +/// The base of the corresponding collision types. +using EdgeVertexStencil = + DTypeStencilAdapter; + } // namespace ipc diff --git a/src/ipc/candidates/face_vertex.cpp b/src/ipc/candidates/face_vertex.cpp index 2408ed4e4..1ba2a7363 100644 --- a/src/ipc/candidates/face_vertex.cpp +++ b/src/ipc/candidates/face_vertex.cpp @@ -13,34 +13,38 @@ FaceVertexCandidate::FaceVertexCandidate(index_t _face_id, index_t _vertex_id) } double FaceVertexCandidate::compute_distance( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype) const { assert(positions.size() == 12); return point_triangle_distance( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), - positions.tail<3>(), known_dtype()); + positions.tail<3>(), dtype); } VectorMax12d FaceVertexCandidate::compute_distance_gradient( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype) const { assert(positions.size() == 12); return point_triangle_distance_gradient( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), - positions.tail<3>(), known_dtype()); + positions.tail<3>(), dtype); } MatrixMax12d FaceVertexCandidate::compute_distance_hessian( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype) const { assert(positions.size() == 12); return point_triangle_distance_hessian( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), - positions.tail<3>(), known_dtype()); + positions.tail<3>(), dtype); } VectorMax4d FaceVertexCandidate::compute_coefficients( - Eigen::ConstRef positions) const + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype) const { assert(positions.size() == 12); Eigen::ConstRef p = positions.head<3>(); @@ -49,13 +53,13 @@ VectorMax4d FaceVertexCandidate::compute_coefficients( Eigen::ConstRef t2 = positions.tail<3>(); // Project the point inside the triangle - auto dtype = known_dtype(); - if (dtype == PointTriangleDistanceType::AUTO) { - dtype = point_triangle_distance_type(p, t0, t1, t2); + PointTriangleDistanceType resolved_dtype = dtype; + if (resolved_dtype == PointTriangleDistanceType::AUTO) { + resolved_dtype = point_triangle_distance_type(p, t0, t1, t2); } VectorMax4d coeffs(4); - switch (dtype) { + switch (resolved_dtype) { case PointTriangleDistanceType::P_T0: coeffs << 1.0, -1.0, 0.0, 0.0; break; diff --git a/src/ipc/candidates/face_vertex.hpp b/src/ipc/candidates/face_vertex.hpp index 05b3ac544..9220a5ef5 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -1,6 +1,7 @@ #pragma once -#include +#include +#include #include #include @@ -10,16 +11,22 @@ namespace ipc { /// @brief A candidate for face-vertex collision detection. -class FaceVertexCandidate : virtual public CollisionStencil { +class FaceVertexCandidate : public StencilMixin { + friend class StencilMixin; + public: + /// @brief Construct a candidate with indeterminate edge IDs. + /// @note Keeping this trivial is what makes the type trivially copyable. + FaceVertexCandidate() = default; + FaceVertexCandidate(index_t face_id, index_t vertex_id); // ------------------------------------------------------------------------ - // CollisionStencil + // Stencil - int num_vertices() const override { return 4; } + constexpr static int num_vertices() { return 4; } - std::string name() const override { return "fv"; } + static std::string name() { return "fv"; } /// @brief Get the vertex IDs for the face-vertex pair /// @param edges The edge connectivity matrix @@ -27,28 +34,36 @@ class FaceVertexCandidate : virtual public CollisionStencil { /// @return An array of vertex IDs in the order: [vi, f0i, f1i, f2i] std::array vertex_ids( Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + Eigen::ConstRef faces) const { return { { vertex_id, faces(face_id, 0), faces(face_id, 1), faces(face_id, 2) } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::compute_distance_hessian; + using StencilMixin::compute_coefficients; + using StencilMixin::compute_distance; + using StencilMixin::compute_distance_gradient; + using StencilMixin::compute_distance_hessian; - double - compute_distance(Eigen::ConstRef positions) const override; + double compute_distance( + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype = + PointTriangleDistanceType::AUTO) const; VectorMax12d compute_distance_gradient( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype = + PointTriangleDistanceType::AUTO) const; MatrixMax12d compute_distance_hessian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype = + PointTriangleDistanceType::AUTO) const; VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions, + const PointTriangleDistanceType dtype = + PointTriangleDistanceType::AUTO) const; // ------------------------------------------------------------------------ @@ -59,11 +74,11 @@ class FaceVertexCandidate : virtual public CollisionStencil { const double min_distance = 0.0, const double tmax = 1.0, const NarrowPhaseCCD& narrow_phase_ccd = - DEFAULT_NARROW_PHASE_CCD) const override; + DEFAULT_NARROW_PHASE_CCD) const; // ------------------------------------------------------------------------ - virtual PointTriangleDistanceType known_dtype() const + constexpr static PointTriangleDistanceType known_dtype() { return PointTriangleDistanceType::AUTO; } @@ -87,11 +102,17 @@ class FaceVertexCandidate : virtual public CollisionStencil { index_t vertex_id; protected: - VectorMax3d compute_unnormalized_normal( - Eigen::ConstRef positions) const override; + VectorMax3d + compute_unnormalized_normal(Eigen::ConstRef positions) const; MatrixMax compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions) const; }; +/// @brief FaceVertexCandidate with the runtime-polymorphic stencil interface. +/// +/// The base of the corresponding collision types. +using FaceVertexStencil = + DTypeStencilAdapter; + } // namespace ipc diff --git a/src/ipc/candidates/plane_vertex.hpp b/src/ipc/candidates/plane_vertex.hpp index 349186134..6e0dfd76f 100644 --- a/src/ipc/candidates/plane_vertex.hpp +++ b/src/ipc/candidates/plane_vertex.hpp @@ -1,51 +1,59 @@ #pragma once +#include +#include #include #include namespace ipc { -class PlaneVertexCandidate : virtual public CollisionStencil { +class PlaneVertexCandidate : public StencilMixin { + friend class StencilMixin; + public: + /// @brief Construct a candidate with indeterminate edge IDs. + /// @note Keeping this trivial is what makes the type trivially copyable. + PlaneVertexCandidate() = default; + PlaneVertexCandidate( const Eigen::Hyperplane& plane, const index_t vertex_id); - int num_vertices() const override { return 1; } + constexpr static int num_vertices() { return 1; } std::array vertex_ids( Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + Eigen::ConstRef faces) const { return { { vertex_id, -1, -1, -1 } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::compute_distance_hessian; + using StencilMixin::compute_coefficients; + using StencilMixin::compute_distance; + using StencilMixin::compute_distance_gradient; + using StencilMixin::compute_distance_hessian; /// @brief Compute the distance between the point and plane. /// @param point Point's position. /// @return Distance of the stencil. - double compute_distance(Eigen::ConstRef point) const override; + double compute_distance(Eigen::ConstRef point) const; /// @brief Compute the gradient of the distance w.r.t. the point's positions. /// @param point Point's position. /// @return Distance gradient w.r.t. the point's positions. - VectorMax12d compute_distance_gradient( - Eigen::ConstRef point) const override; + VectorMax12d + compute_distance_gradient(Eigen::ConstRef point) const; /// @brief Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions. /// @param point Point's position. /// @return Distance Hessian w.r.t. the point's positions. - MatrixMax12d compute_distance_hessian( - Eigen::ConstRef point) const override; + MatrixMax12d + compute_distance_hessian(Eigen::ConstRef point) const; /// @brief Compute the coefficients of the stencil. /// @param positions Vertex positions. /// @return Coefficients of the stencil. - VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; + VectorMax4d + compute_coefficients(Eigen::ConstRef positions) const; /// @brief Perform narrow-phase CCD on the candidate. /// @param[in] vertices_t0 Stencil vertices at the start of the time step. @@ -62,7 +70,7 @@ class PlaneVertexCandidate : virtual public CollisionStencil { const double min_distance = 0.0, const double tmax = 1.0, const NarrowPhaseCCD& narrow_phase_ccd = - DEFAULT_NARROW_PHASE_CCD) const override; + DEFAULT_NARROW_PHASE_CCD) const; /// @brief The plane of the candidate. Eigen::Hyperplane plane; @@ -83,14 +91,19 @@ class PlaneVertexCandidate : virtual public CollisionStencil { /// @brief Compute the normal vector of the stencil. /// @param positions Vertex positions. /// @return Normal vector of the stencil. - VectorMax3d compute_unnormalized_normal( - Eigen::ConstRef positions) const override; + VectorMax3d + compute_unnormalized_normal(Eigen::ConstRef positions) const; /// @brief Compute the Jacobian of the normal vector of the stencil. /// @param positions Vertex positions. /// @return Jacobian of the normal vector of the stencil. MatrixMax compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions) const; }; +/// @brief PlaneVertexCandidate with the runtime-polymorphic stencil interface. +/// +/// The base of the corresponding collision types. +using PlaneVertexStencil = StencilAdapter; + } // namespace ipc \ No newline at end of file diff --git a/src/ipc/candidates/stencil_adapter.hpp b/src/ipc/candidates/stencil_adapter.hpp new file mode 100644 index 000000000..f4f4b3bc4 --- /dev/null +++ b/src/ipc/candidates/stencil_adapter.hpp @@ -0,0 +1,209 @@ +#pragma once + +#include + +namespace ipc { + +/// @brief Gives a candidate the runtime-polymorphic stencil interface. +/// +/// The candidate types are deliberately not CollisionStencils: the broad phase +/// stores millions of them and a vptr would double their size. The collision +/// types are used through CollisionStencil&, so they derive from this instead, +/// which forwards every stencil operation to the candidate. +/// +/// @tparam Candidate The candidate type to adapt. +template +class StencilAdapter : public Candidate, public virtual CollisionStencil { +public: + using Candidate::Candidate; + + /// @brief Adapt an existing candidate. + StencilAdapter(const Candidate& candidate) : Candidate(candidate) { } + + // Both bases carry the mixin's operations (once through Candidate, once + // through CollisionStencil), so every shared name has to be pinned to one + // side or lookup is ambiguous. The polymorphic side is the one to keep. + using CollisionStencil::compute_coefficients; + using CollisionStencil::compute_distance; + using CollisionStencil::compute_distance_gradient; + using CollisionStencil::compute_distance_hessian; + using CollisionStencil::compute_distance_vector; + using CollisionStencil::compute_distance_vector_jacobian; + using CollisionStencil::compute_normal; + using CollisionStencil::compute_normal_jacobian; + using CollisionStencil::contract_distance_vector_jacobian; + using CollisionStencil::diag_distance_vector_outer; + using CollisionStencil::diag_distance_vector_t_outer; + using CollisionStencil::dim; + using CollisionStencil::dof; + using CollisionStencil::STENCIL_SIZE; + using CollisionStencil::vertices; + using CollisionStencil::write_ccd_query; + + int num_vertices() const override { return Candidate::num_vertices(); } + + std::string name() const override { return Candidate::name(); } + + std::array vertex_ids( + Eigen::ConstRef edges, + Eigen::ConstRef faces) const override + { + return Candidate::vertex_ids(edges, faces); + } + + double + compute_distance(Eigen::ConstRef positions) const override + { + return Candidate::compute_distance(positions); + } + + VectorMax12d compute_distance_gradient( + Eigen::ConstRef positions) const override + { + return Candidate::compute_distance_gradient(positions); + } + + MatrixMax12d compute_distance_hessian( + Eigen::ConstRef positions) const override + { + return Candidate::compute_distance_hessian(positions); + } + + VectorMax4d + compute_coefficients(Eigen::ConstRef positions) const override + { + return Candidate::compute_coefficients(positions); + } + + bool + ccd(Eigen::ConstRef vertices_t0, + Eigen::ConstRef vertices_t1, + double& toi, + const double min_distance = 0.0, + const double tmax = 1.0, + const NarrowPhaseCCD& narrow_phase_ccd = + DEFAULT_NARROW_PHASE_CCD) const override + { + return Candidate::ccd( + vertices_t0, vertices_t1, toi, min_distance, tmax, + narrow_phase_ccd); + } + +protected: + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override + { + return Candidate::compute_unnormalized_normal(positions); + } + + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override + { + return Candidate::compute_unnormalized_normal_jacobian(positions); + } +}; + +/// @brief A StencilAdapter for a candidate whose distance type can be known a priori. +/// +/// The candidate's math takes the distance type as an argument; this supplies +/// it through a virtual so a collision can pin it to the value it cached when +/// the collision set was built, which is what keeps the classification fixed +/// across a solve. +/// +/// @tparam Candidate The candidate type to adapt. +/// @tparam DType The candidate's distance type enum. +template +class DTypeStencilAdapter : public Candidate, public virtual CollisionStencil { +public: + using Candidate::Candidate; + + /// @brief Adapt an existing candidate. + DTypeStencilAdapter(const Candidate& candidate) : Candidate(candidate) { } + + // Both bases carry the mixin's operations (once through Candidate, once + // through CollisionStencil), so every shared name has to be pinned to one + // side or lookup is ambiguous. The polymorphic side is the one to keep. + using CollisionStencil::compute_coefficients; + using CollisionStencil::compute_distance; + using CollisionStencil::compute_distance_gradient; + using CollisionStencil::compute_distance_hessian; + using CollisionStencil::compute_distance_vector; + using CollisionStencil::compute_distance_vector_jacobian; + using CollisionStencil::compute_normal; + using CollisionStencil::compute_normal_jacobian; + using CollisionStencil::contract_distance_vector_jacobian; + using CollisionStencil::diag_distance_vector_outer; + using CollisionStencil::diag_distance_vector_t_outer; + using CollisionStencil::dim; + using CollisionStencil::dof; + using CollisionStencil::STENCIL_SIZE; + using CollisionStencil::vertices; + using CollisionStencil::write_ccd_query; + + /// @brief The distance type known a priori; AUTO unless a collision pins it. + virtual DType known_dtype() const { return DType::AUTO; } + + int num_vertices() const override { return Candidate::num_vertices(); } + + std::string name() const override { return Candidate::name(); } + + std::array vertex_ids( + Eigen::ConstRef edges, + Eigen::ConstRef faces) const override + { + return Candidate::vertex_ids(edges, faces); + } + + double + compute_distance(Eigen::ConstRef positions) const override + { + return Candidate::compute_distance(positions, known_dtype()); + } + + VectorMax12d compute_distance_gradient( + Eigen::ConstRef positions) const override + { + return Candidate::compute_distance_gradient(positions, known_dtype()); + } + + MatrixMax12d compute_distance_hessian( + Eigen::ConstRef positions) const override + { + return Candidate::compute_distance_hessian(positions, known_dtype()); + } + + VectorMax4d + compute_coefficients(Eigen::ConstRef positions) const override + { + return Candidate::compute_coefficients(positions, known_dtype()); + } + + bool + ccd(Eigen::ConstRef vertices_t0, + Eigen::ConstRef vertices_t1, + double& toi, + const double min_distance = 0.0, + const double tmax = 1.0, + const NarrowPhaseCCD& narrow_phase_ccd = + DEFAULT_NARROW_PHASE_CCD) const override + { + return Candidate::ccd( + vertices_t0, vertices_t1, toi, min_distance, tmax, + narrow_phase_ccd); + } + +protected: + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override + { + return Candidate::compute_unnormalized_normal(positions); + } + + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override + { + return Candidate::compute_unnormalized_normal_jacobian(positions); + } +}; + +} // namespace ipc diff --git a/src/ipc/candidates/stencil_mixin.hpp b/src/ipc/candidates/stencil_mixin.hpp new file mode 100644 index 000000000..30df834be --- /dev/null +++ b/src/ipc/candidates/stencil_mixin.hpp @@ -0,0 +1,423 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace ipc { + +/// @brief Stencil math that does not depend on the stencil type. +/// +/// Split out of StencilMixin so these are compiled once rather than once per +/// stencil type. +class StencilMath { +public: + /// @brief The maximum number of vertices in a collision stencil. + static constexpr int STENCIL_SIZE = 4; + + /// @brief Compute diag((∂t/∂x)(∂t/∂x)ᵀ) efficiently (Eq. 11 of the paper). + /// + /// Result is [c₀², c₀², c₀², c₁², c₁², c₁², ...] (each cᵢ² repeated dim + /// times). This is used for computing diag(∂²b/∂x²) without the full + /// Hessian. + /// + /// @param coeffs The coefficients cᵢ (from compute_coefficients). + /// @param d The spatial dimension (2 or 3). + /// @return The diagonal of (∂t/∂x)(∂t/∂x)ᵀ as a vector of size ndof. + static VectorMax12d + diag_distance_vector_outer(Eigen::ConstRef coeffs, const int d) + { + const int n = coeffs.size(); + VectorMax12d diag(n * d); + for (int i = 0; i < n; i++) { + const double ci2 = coeffs[i] * coeffs[i]; + diag.segment(i * d, d).setConstant(ci2); + } + return diag; + } + + /// @brief Compute diag((∂t/∂x · t)(∂t/∂x · t)ᵀ) efficiently (Eq. 12). + /// + /// Result is element-wise square of [c₀tᵀ, c₁tᵀ, ..., cₙtᵀ]. + /// This is used for computing diag(∂²b/∂x²) without the full Hessian. + /// + /// @param coeffs The coefficients cᵢ. + /// @param distance_vector The distance vector t. + /// @return The diagonal of (∂t/∂x·t)(∂t/∂x·t)ᵀ as a vector of size ndof. + static VectorMax12d diag_distance_vector_t_outer( + Eigen::ConstRef coeffs, + Eigen::ConstRef distance_vector) + { + const int n = coeffs.size(); + const int d = distance_vector.size(); + const VectorMax3d t2 = distance_vector.array().square(); + VectorMax12d diag(n * d); + for (int i = 0; i < n; i++) { + diag.segment(i * d, d).array() = (coeffs[i] * coeffs[i]) * t2; + } + return diag; + } + + /// @brief Compute pᵀ(∂t/∂x) efficiently as ∑ cᵢ pᵢ (Eqs. 13-14). + /// + /// Given p = [p₀, p₁, ..., pₙ]ᵀ where pᵢ ∈ ℝ^dim, this computes + /// pᵀ(∂t/∂x) = ∑ cᵢ pᵢ which is a dim-dimensional vector. + /// + /// @param coeffs The coefficients cᵢ. + /// @param p A vector of size ndof (the direction for the quadratic form). + /// @param d The spatial dimension (2 or 3). + /// @return pᵀ(∂t/∂x) as a dim-dimensional vector. + static VectorMax3d contract_distance_vector_jacobian( + Eigen::ConstRef coeffs, + Eigen::ConstRef p, + const int d) + { + const int n = coeffs.size(); + VectorMax3d result = VectorMax3d::Zero(d); + for (int i = 0; i < n; i++) { + result += coeffs[i] * p.segment(d * i, d); + } + return result; + } +}; + +/// @brief The stencil operations that are written once in terms of a few +/// primitives every stencil provides. +/// +/// A stencil supplies `num_vertices()`, `vertex_ids()`, the four +/// `positions`-based quantities (distance, its gradient and Hessian, and the +/// coefficients), `ccd()`, and the two unnormalized-normal quantities. This +/// mixin builds everything else on top of them. +/// +/// It is deliberately empty: no data members and no virtual functions. A +/// stencil that derives from it therefore pays nothing in size (empty base +/// optimization), and stays trivially copyable if its own members are, which +/// is what lets the broad phase move millions of candidates with a memcpy. +/// `CollisionStencil` derives from it too, and there the calls below land on +/// its virtual functions. +/// +/// @tparam Derived The stencil type deriving from this. +template class StencilMixin : public StencilMath { +protected: + constexpr const Derived& self() const + { + return static_cast(*this); + } + +public: + /// @brief A short name for the stencil type, for debug output. Stencils + /// that have one hide this. + static std::string name() { return "unknown"; } + + /// @brief Get the dimension of the collision stencil. + /// @param ndof Number of degrees of freedom in the stencil. + /// @return The dimension of the collision stencil. + int dim(const int ndof) const + { + assert(ndof % self().num_vertices() == 0); + return ndof / self().num_vertices(); + } + + /// @brief Get the vertex attributes of the collision stencil. + /// @param vertices Vertex attributes + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @return The vertex positions of the collision stencil. Elements i > num_vertices() are NaN. + std::array vertices( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + constexpr double NaN = std::numeric_limits::signaling_NaN(); + + const auto vertex_ids = self().vertex_ids(edges, faces); + + std::array stencil_vertices; + for (int i = 0; i < STENCIL_SIZE; i++) { + if (vertex_ids[i] >= 0) { + stencil_vertices[i] = vertices.row(vertex_ids[i]); + } else { + stencil_vertices[i].setConstant(vertices.cols(), NaN); + } + } + + return stencil_vertices; + } + + /// @brief Select this stencil's DOF from the full matrix of DOF. + /// @param X Full matrix of DOF (rowwise). + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @return This stencil's DOF. + VectorMax12d + dof(Eigen::ConstRef X, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + const int dim = X.cols(); + const int n = self().num_vertices(); + VectorMax12d x(n * dim); + const auto idx = self().vertex_ids(edges, faces); + for (int i = 0; i < n; i++) { + x.segment(i * dim, dim) = X.row(idx[i]); + } + return x; + } + + // -- Mesh-based overloads of the positions-based quantities -------------- + + /// @brief Compute the distance of the stencil. + /// @param vertices Collision mesh vertices + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @return Distance of the stencil. + double compute_distance( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return self().compute_distance(dof(vertices, edges, faces)); + } + + /// @brief Compute the distance gradient of the stencil w.r.t. the stencil's vertex positions. + /// @param vertices Collision mesh vertices + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @return Distance gradient of the stencil w.r.t. the stencil's vertex positions. + VectorMax12d compute_distance_gradient( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return self().compute_distance_gradient(dof(vertices, edges, faces)); + } + + /// @brief Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions. + /// @param vertices Collision mesh vertices + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @return Distance Hessian of the stencil w.r.t. the stencil's vertex positions. + MatrixMax12d compute_distance_hessian( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return self().compute_distance_hessian(dof(vertices, edges, faces)); + } + + /// @brief Compute the coefficients of the stencil s.t. \f$d(x) = \|\sum c_i \mathbf{x}_i\|^2\f$. + /// @param vertices Collision mesh vertices + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @return Coefficients of the stencil. + VectorMax4d compute_coefficients( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return self().compute_coefficients(dof(vertices, edges, faces)); + } + + /// @brief Compute the distance vector using the mesh vertices. + /// @param vertices Collision mesh vertices. + /// @param edges Collision mesh edges. + /// @param faces Collision mesh faces. + /// @return The distance vector (dim-dimensional). + VectorMax3d compute_distance_vector( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return compute_distance_vector(dof(vertices, edges, faces)); + } + + /// @brief Compute the normal of the stencil. + /// @param vertices Collision mesh vertices + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @param flip_if_negative If true, flip the normal if the point is on the negative side. + /// @param sign If not nullptr, set to the sign of the normal before any flipping. + /// @return Normal of the stencil. + VectorMax3d compute_normal( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + const bool flip_if_negative = true, + double* sign = nullptr) const + { + return compute_normal( + dof(vertices, edges, faces), flip_if_negative, sign); + } + + /// @brief Compute the Jacobian of the normal of the stencil. + /// @param vertices Collision mesh vertices + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + /// @param flip_if_negative If true, flip the normal if the point is on the negative side. + /// @return Jacobian of the normal of the stencil. + MatrixMax compute_normal_jacobian( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + const bool flip_if_negative = true) const + { + return compute_normal_jacobian( + dof(vertices, edges, faces), flip_if_negative); + } + + // ---------------------------------------------------------------------- + // NOTE: The following functions take stencil vertices as output by dof() + // ---------------------------------------------------------------------- + + /// @brief Compute the distance vector of the stencil: t = ∑ cᵢ xᵢ. + /// + /// The distance vector is the vector between the closest points on the + /// collision primitives. Its squared norm equals the squared distance. + /// + /// @param positions Stencil's vertex positions. + /// @return The distance vector (dim-dimensional, i.e., 2D or 3D). + VectorMax3d + compute_distance_vector(Eigen::ConstRef positions) const + { + VectorMax4d _; // Unused output + return compute_distance_vector(positions, _); + } + + /// @brief Compute the distance vector and the coefficients together. + /// @param positions Stencil's vertex positions. + /// @param[out] coeffs The computed coefficients cᵢ. + /// @return The distance vector. + VectorMax3d compute_distance_vector( + Eigen::ConstRef positions, VectorMax4d& coeffs) const + { + const int n = self().num_vertices(); + const int d = dim(positions.size()); + coeffs = self().compute_coefficients(positions); + VectorMax3d dv = VectorMax3d::Zero(d); + for (int i = 0; i < n; i++) { + dv += coeffs[i] * positions.segment(d * i, d); + } + return dv; + } + + /// @brief Compute the Jacobian of the distance vector w.r.t. positions: ∂t/∂x = [c₀I c₁I ... cₙI]ᵀ. + /// + /// Since ∂t/∂x has a very simple structure (block-diagonal with scalar + /// coefficients times identity), many operations can be done without + /// forming this matrix. This method is provided for completeness and + /// verification. + /// + /// @param positions Stencil's vertex positions. + /// @return The Jacobian ∂t/∂x ∈ ℝ^{ndof × dim}. + MatrixMax compute_distance_vector_jacobian( + Eigen::ConstRef positions) const + { + const int n = self().num_vertices(); + const int d = dim(positions.size()); + const int ndof = n * d; + const VectorMax4d c = self().compute_coefficients(positions); + // ∂t/∂x is ndof × dim + // Each block row i is cᵢ * I_{d×d} + MatrixMax J; + J.setZero(ndof, d); + for (int i = 0; i < n; i++) { + J.block(i * d, 0, d, d).diagonal().array() = c[i]; + } + return J; + } + + /// @brief Compute the normal of the stencil. + /// @param positions Stencil's vertex positions. + /// @param flip_if_negative If true, flip the normal if the point is on the negative side. + /// @param sign If not nullptr, set to the sign of the normal before any flipping. + /// @return Normal of the stencil. + VectorMax3d compute_normal( + Eigen::ConstRef positions, + bool flip_if_negative = true, + double* sign = nullptr) const + { + const int dim = this->dim(positions.size()); + + VectorMax3d n = + self().compute_unnormalized_normal(positions).normalized(); + + if (sign != nullptr) { + *sign = + (positions.head(dim) - positions.tail(dim)).dot(n) < 0 ? -1 : 1; + } + + // Flip the normal if the point is on the negative side. + // Any point on the second object will do, so we use the last point. + if (flip_if_negative + && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) { + n *= -1; + } + + return n; + } + + /// @brief Compute the Jacobian of the normal of the stencil. + /// @param positions Stencil's vertex positions. + /// @param flip_if_negative If true, flip the normal if the point is on the negative side. + /// @return Jacobian of the normal of the stencil. + MatrixMax compute_normal_jacobian( + Eigen::ConstRef positions, + bool flip_if_negative = true) const + { + const int dim = this->dim(positions.size()); + + const VectorMax3d n = self().compute_unnormalized_normal(positions); + + MatrixMax dn = normalization_jacobian(n) + * self().compute_unnormalized_normal_jacobian(positions); + + if (flip_if_negative + && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) { + dn *= -1; + } + + return dn; + } + + /// @brief Write the CCD query to a stream. + /// @param out Stream to write to. + /// @param vertices_t0 Stencil vertices at the start of the time step. + /// @param vertices_t1 Stencil vertices at the end of the time step. + /// @return The stream. + std::ostream& write_ccd_query( + std::ostream& out, + Eigen::ConstRef vertices_t0, + Eigen::ConstRef vertices_t1) const + { + assert(vertices_t0.size() == vertices_t1.size()); + + const int n = self().num_vertices(); + const int dim = vertices_t0.size() / n; + assert(vertices_t0.size() % n == 0); + + for (int i = 0; i < n; i++) { + out << vertices_t0.segment(dim * i, dim) + .transpose() + .format(OBJ_VERTEX_FORMAT); + } + + for (int i = 0; i < n; i++) { + out << vertices_t1.segment(dim * i, dim) + .transpose() + .format(OBJ_VERTEX_FORMAT); + } + + return out; + } +}; + +} // namespace ipc diff --git a/src/ipc/candidates/vertex_vertex.hpp b/src/ipc/candidates/vertex_vertex.hpp index ec1cb80bb..4dd87aeb6 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -1,6 +1,7 @@ #pragma once -#include +#include +#include #include #include @@ -10,16 +11,22 @@ namespace ipc { /// @brief A candidate for vertex-vertex collision detection. -class VertexVertexCandidate : virtual public CollisionStencil { +class VertexVertexCandidate : public StencilMixin { + friend class StencilMixin; + public: + /// @brief Construct a candidate with indeterminate edge IDs. + /// @note Keeping this trivial is what makes the type trivially copyable. + VertexVertexCandidate() = default; + VertexVertexCandidate(index_t vertex0_id, index_t vertex1_id); // ------------------------------------------------------------------------ - // CollisionStencil + // Stencil - int num_vertices() const override { return 2; } + constexpr static int num_vertices() { return 2; } - std::string name() const override { return "vv"; } + static std::string name() { return "vv"; } /// @brief Get the indices of the vertices /// @param edges edge matrix of mesh @@ -27,27 +34,26 @@ class VertexVertexCandidate : virtual public CollisionStencil { /// @return List of vertex indices std::array vertex_ids( Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + Eigen::ConstRef faces) const { return { { vertex0_id, vertex1_id, -1, -1 } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::compute_distance_hessian; + using StencilMixin::compute_coefficients; + using StencilMixin::compute_distance; + using StencilMixin::compute_distance_gradient; + using StencilMixin::compute_distance_hessian; - double - compute_distance(Eigen::ConstRef positions) const override; + double compute_distance(Eigen::ConstRef positions) const; - VectorMax12d compute_distance_gradient( - Eigen::ConstRef positions) const override; + VectorMax12d + compute_distance_gradient(Eigen::ConstRef positions) const; - MatrixMax12d compute_distance_hessian( - Eigen::ConstRef positions) const override; + MatrixMax12d + compute_distance_hessian(Eigen::ConstRef positions) const; - VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; + VectorMax4d + compute_coefficients(Eigen::ConstRef positions) const; // ------------------------------------------------------------------------ @@ -58,7 +64,7 @@ class VertexVertexCandidate : virtual public CollisionStencil { const double min_distance = 0.0, const double tmax = 1.0, const NarrowPhaseCCD& narrow_phase_ccd = - DEFAULT_NARROW_PHASE_CCD) const override; + DEFAULT_NARROW_PHASE_CCD) const; // ------------------------------------------------------------------------ @@ -81,11 +87,16 @@ class VertexVertexCandidate : virtual public CollisionStencil { index_t vertex1_id; protected: - VectorMax3d compute_unnormalized_normal( - Eigen::ConstRef positions) const override; + VectorMax3d + compute_unnormalized_normal(Eigen::ConstRef positions) const; MatrixMax compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const override; + Eigen::ConstRef positions) const; }; +/// @brief VertexVertexCandidate with the runtime-polymorphic stencil interface. +/// +/// The base of the corresponding collision types. +using VertexVertexStencil = StencilAdapter; + } // namespace ipc diff --git a/src/ipc/collisions/normal/edge_edge.cpp b/src/ipc/collisions/normal/edge_edge.cpp index 70fb517fb..c95669eca 100644 --- a/src/ipc/collisions/normal/edge_edge.cpp +++ b/src/ipc/collisions/normal/edge_edge.cpp @@ -11,7 +11,7 @@ EdgeEdgeNormalCollision::EdgeEdgeNormalCollision( const index_t _edge1_id, const double _eps_x, const EdgeEdgeDistanceType _dtype) - : EdgeEdgeCandidate(_edge0_id, _edge1_id) + : EdgeEdgeStencil(_edge0_id, _edge1_id) , eps_x(_eps_x) , dtype(_dtype) { @@ -21,7 +21,7 @@ EdgeEdgeNormalCollision::EdgeEdgeNormalCollision( const EdgeEdgeCandidate& candidate, const double _eps_x, const EdgeEdgeDistanceType _dtype) - : EdgeEdgeCandidate(candidate) + : EdgeEdgeStencil(candidate) , eps_x(_eps_x) , dtype(_dtype) { @@ -34,7 +34,7 @@ EdgeEdgeNormalCollision::EdgeEdgeNormalCollision( const double _weight, const Eigen::SparseVector& _weight_gradient, const EdgeEdgeDistanceType _dtype) - : EdgeEdgeCandidate(_edge0_id, _edge1_id) + : EdgeEdgeStencil(_edge0_id, _edge1_id) , NormalCollision(_weight, _weight_gradient) , eps_x(_eps_x) , dtype(_dtype) diff --git a/src/ipc/collisions/normal/edge_edge.hpp b/src/ipc/collisions/normal/edge_edge.hpp index c029dc533..a3455d575 100644 --- a/src/ipc/collisions/normal/edge_edge.hpp +++ b/src/ipc/collisions/normal/edge_edge.hpp @@ -6,8 +6,7 @@ namespace ipc { -class EdgeEdgeNormalCollision : public EdgeEdgeCandidate, - public NormalCollision { +class EdgeEdgeNormalCollision : public EdgeEdgeStencil, public NormalCollision { public: EdgeEdgeNormalCollision( const index_t edge0_id, diff --git a/src/ipc/collisions/normal/edge_vertex.hpp b/src/ipc/collisions/normal/edge_vertex.hpp index 1fe1209eb..fcbef964f 100644 --- a/src/ipc/collisions/normal/edge_vertex.hpp +++ b/src/ipc/collisions/normal/edge_vertex.hpp @@ -5,13 +5,13 @@ namespace ipc { -class EdgeVertexNormalCollision : public EdgeVertexCandidate, +class EdgeVertexNormalCollision : public EdgeVertexStencil, public NormalCollision { public: - using EdgeVertexCandidate::EdgeVertexCandidate; + using EdgeVertexStencil::EdgeVertexStencil; EdgeVertexNormalCollision(const EdgeVertexCandidate& candidate) - : EdgeVertexCandidate(candidate) + : EdgeVertexStencil(candidate) { } @@ -20,7 +20,7 @@ class EdgeVertexNormalCollision : public EdgeVertexCandidate, const index_t _vertex_id, const double _weight, const Eigen::SparseVector& _weight_gradient) - : EdgeVertexCandidate(_edge_id, _vertex_id) + : EdgeVertexStencil(_edge_id, _vertex_id) , NormalCollision(_weight, _weight_gradient) { } diff --git a/src/ipc/collisions/normal/face_vertex.hpp b/src/ipc/collisions/normal/face_vertex.hpp index d781aff22..8872da39c 100644 --- a/src/ipc/collisions/normal/face_vertex.hpp +++ b/src/ipc/collisions/normal/face_vertex.hpp @@ -5,13 +5,13 @@ namespace ipc { -class FaceVertexNormalCollision : public FaceVertexCandidate, +class FaceVertexNormalCollision : public FaceVertexStencil, public NormalCollision { public: - using FaceVertexCandidate::FaceVertexCandidate; + using FaceVertexStencil::FaceVertexStencil; FaceVertexNormalCollision(const FaceVertexCandidate& candidate) - : FaceVertexCandidate(candidate) + : FaceVertexStencil(candidate) { } @@ -20,7 +20,7 @@ class FaceVertexNormalCollision : public FaceVertexCandidate, const index_t _vertex_id, const double _weight, const Eigen::SparseVector& _weight_gradient) - : FaceVertexCandidate(_face_id, _vertex_id) + : FaceVertexStencil(_face_id, _vertex_id) , NormalCollision(_weight, _weight_gradient) { } diff --git a/src/ipc/collisions/normal/normal_collisions.hpp b/src/ipc/collisions/normal/normal_collisions.hpp index 7e82868a4..b9cbebe2e 100644 --- a/src/ipc/collisions/normal/normal_collisions.hpp +++ b/src/ipc/collisions/normal/normal_collisions.hpp @@ -95,6 +95,20 @@ class NormalCollisions { /// @return A const reference to the collision. const NormalCollision& operator[](size_t i) const; + /// @brief Apply a visitor to the collision at index i. + /// + /// Mirrors Candidates::visit so code can be written once for both. These + /// collisions are polymorphic, so the visitor sees a NormalCollision&. + /// + /// @param i The index of the collision. + /// @param f The visitor. + /// @return Whatever the visitor returns. + template + decltype(auto) visit(size_t i, Visitor&& f) const + { + return f((*this)[i]); + } + /// @brief Get if the collision at i is a vertex-vertex collision. /// @param i The index of the collision. /// @return If the collision at i is a vertex-vertex collision. diff --git a/src/ipc/collisions/normal/plane_vertex.hpp b/src/ipc/collisions/normal/plane_vertex.hpp index 1caed7e2b..896c4defc 100644 --- a/src/ipc/collisions/normal/plane_vertex.hpp +++ b/src/ipc/collisions/normal/plane_vertex.hpp @@ -6,13 +6,13 @@ namespace ipc { -class PlaneVertexNormalCollision : public PlaneVertexCandidate, +class PlaneVertexNormalCollision : public PlaneVertexStencil, public NormalCollision { public: - using PlaneVertexCandidate::PlaneVertexCandidate; + using PlaneVertexStencil::PlaneVertexStencil; PlaneVertexNormalCollision(const PlaneVertexCandidate& candidate) - : PlaneVertexCandidate(candidate) + : PlaneVertexStencil(candidate) { } @@ -21,7 +21,7 @@ class PlaneVertexNormalCollision : public PlaneVertexCandidate, const index_t _vertex_id, const double _weight, const Eigen::SparseVector& _weight_gradient) - : PlaneVertexCandidate(_plane, _vertex_id) + : PlaneVertexStencil(_plane, _vertex_id) , NormalCollision(_weight, _weight_gradient) { } diff --git a/src/ipc/collisions/normal/vertex_vertex.hpp b/src/ipc/collisions/normal/vertex_vertex.hpp index fa2c306ed..b5b74079b 100644 --- a/src/ipc/collisions/normal/vertex_vertex.hpp +++ b/src/ipc/collisions/normal/vertex_vertex.hpp @@ -6,13 +6,13 @@ namespace ipc { -class VertexVertexNormalCollision : public VertexVertexCandidate, +class VertexVertexNormalCollision : public VertexVertexStencil, public NormalCollision { public: - using VertexVertexCandidate::VertexVertexCandidate; + using VertexVertexStencil::VertexVertexStencil; VertexVertexNormalCollision(const VertexVertexCandidate& candidate) - : VertexVertexCandidate(candidate) + : VertexVertexStencil(candidate) { } @@ -21,7 +21,7 @@ class VertexVertexNormalCollision : public VertexVertexCandidate, const index_t _vertex1_id, const double _weight, const Eigen::SparseVector& _weight_gradient) - : VertexVertexCandidate(_vertex0_id, _vertex1_id) + : VertexVertexStencil(_vertex0_id, _vertex1_id) , NormalCollision(_weight, _weight_gradient) { } diff --git a/src/ipc/collisions/tangential/edge_edge.cpp b/src/ipc/collisions/tangential/edge_edge.cpp index 64b609360..dff2629ae 100644 --- a/src/ipc/collisions/tangential/edge_edge.cpp +++ b/src/ipc/collisions/tangential/edge_edge.cpp @@ -9,7 +9,7 @@ namespace ipc { EdgeEdgeTangentialCollision::EdgeEdgeTangentialCollision( const EdgeEdgeNormalCollision& collision) - : EdgeEdgeCandidate(collision.edge0_id, collision.edge1_id) + : EdgeEdgeStencil(collision.edge0_id, collision.edge1_id) { this->weight = collision.weight; this->weight_gradient = collision.weight_gradient; diff --git a/src/ipc/collisions/tangential/edge_edge.hpp b/src/ipc/collisions/tangential/edge_edge.hpp index bdee6e2bb..6fd35dfa8 100644 --- a/src/ipc/collisions/tangential/edge_edge.hpp +++ b/src/ipc/collisions/tangential/edge_edge.hpp @@ -6,10 +6,15 @@ namespace ipc { -class EdgeEdgeTangentialCollision : public EdgeEdgeCandidate, +class EdgeEdgeTangentialCollision : public EdgeEdgeStencil, public TangentialCollision { public: - using EdgeEdgeCandidate::EdgeEdgeCandidate; + // The adapter also names dim/ndof (the stencil's dof-count overloads); keep + // this collision's own no-argument accessors. + using TangentialCollision::dim; + using TangentialCollision::ndof; + + using EdgeEdgeStencil::EdgeEdgeStencil; EdgeEdgeTangentialCollision(const EdgeEdgeNormalCollision& collision); diff --git a/src/ipc/collisions/tangential/edge_vertex.cpp b/src/ipc/collisions/tangential/edge_vertex.cpp index 97ad3129a..f0f83c80f 100644 --- a/src/ipc/collisions/tangential/edge_vertex.cpp +++ b/src/ipc/collisions/tangential/edge_vertex.cpp @@ -9,7 +9,7 @@ namespace ipc { EdgeVertexTangentialCollision::EdgeVertexTangentialCollision( const EdgeVertexNormalCollision& collision) - : EdgeVertexCandidate(collision.edge_id, collision.vertex_id) + : EdgeVertexStencil(collision.edge_id, collision.vertex_id) { this->weight = collision.weight; this->weight_gradient = collision.weight_gradient; diff --git a/src/ipc/collisions/tangential/edge_vertex.hpp b/src/ipc/collisions/tangential/edge_vertex.hpp index 48d280339..0fd5f2d8e 100644 --- a/src/ipc/collisions/tangential/edge_vertex.hpp +++ b/src/ipc/collisions/tangential/edge_vertex.hpp @@ -6,10 +6,15 @@ namespace ipc { -class EdgeVertexTangentialCollision : public EdgeVertexCandidate, +class EdgeVertexTangentialCollision : public EdgeVertexStencil, public TangentialCollision { public: - using EdgeVertexCandidate::EdgeVertexCandidate; + // The adapter also names dim/ndof (the stencil's dof-count overloads); keep + // this collision's own no-argument accessors. + using TangentialCollision::dim; + using TangentialCollision::ndof; + + using EdgeVertexStencil::EdgeVertexStencil; EdgeVertexTangentialCollision(const EdgeVertexNormalCollision& collision); diff --git a/src/ipc/collisions/tangential/face_vertex.cpp b/src/ipc/collisions/tangential/face_vertex.cpp index 4fbd45d41..9ec2b6706 100644 --- a/src/ipc/collisions/tangential/face_vertex.cpp +++ b/src/ipc/collisions/tangential/face_vertex.cpp @@ -9,7 +9,7 @@ namespace ipc { FaceVertexTangentialCollision::FaceVertexTangentialCollision( const FaceVertexNormalCollision& collision) - : FaceVertexCandidate(collision.face_id, collision.vertex_id) + : FaceVertexStencil(collision.face_id, collision.vertex_id) { this->weight = collision.weight; this->weight_gradient = collision.weight_gradient; diff --git a/src/ipc/collisions/tangential/face_vertex.hpp b/src/ipc/collisions/tangential/face_vertex.hpp index 3f9e0b48a..10be5b63f 100644 --- a/src/ipc/collisions/tangential/face_vertex.hpp +++ b/src/ipc/collisions/tangential/face_vertex.hpp @@ -6,10 +6,15 @@ namespace ipc { -class FaceVertexTangentialCollision : public FaceVertexCandidate, +class FaceVertexTangentialCollision : public FaceVertexStencil, public TangentialCollision { public: - using FaceVertexCandidate::FaceVertexCandidate; + // The adapter also names dim/ndof (the stencil's dof-count overloads); keep + // this collision's own no-argument accessors. + using TangentialCollision::dim; + using TangentialCollision::ndof; + + using FaceVertexStencil::FaceVertexStencil; FaceVertexTangentialCollision(const FaceVertexNormalCollision& collision); diff --git a/src/ipc/collisions/tangential/plane_vertex.cpp b/src/ipc/collisions/tangential/plane_vertex.cpp index a7a0bc49a..5550a0c54 100644 --- a/src/ipc/collisions/tangential/plane_vertex.cpp +++ b/src/ipc/collisions/tangential/plane_vertex.cpp @@ -9,7 +9,7 @@ namespace ipc { PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( const PlaneVertexNormalCollision& collision) - : PlaneVertexCandidate(collision) + : PlaneVertexStencil(collision) { this->weight = collision.weight; this->weight_gradient = collision.weight_gradient; diff --git a/src/ipc/collisions/tangential/plane_vertex.hpp b/src/ipc/collisions/tangential/plane_vertex.hpp index c87c76fb1..a89155155 100644 --- a/src/ipc/collisions/tangential/plane_vertex.hpp +++ b/src/ipc/collisions/tangential/plane_vertex.hpp @@ -7,10 +7,15 @@ namespace ipc { -class PlaneVertexTangentialCollision : public PlaneVertexCandidate, +class PlaneVertexTangentialCollision : public PlaneVertexStencil, public TangentialCollision { public: - using PlaneVertexCandidate::PlaneVertexCandidate; + // The adapter also names dim/ndof (the stencil's dof-count overloads); keep + // this collision's own no-argument accessors. + using TangentialCollision::dim; + using TangentialCollision::ndof; + + using PlaneVertexStencil::PlaneVertexStencil; PlaneVertexTangentialCollision(const PlaneVertexNormalCollision& collision); diff --git a/src/ipc/collisions/tangential/vertex_vertex.cpp b/src/ipc/collisions/tangential/vertex_vertex.cpp index 1c9e9fd2e..4c2b6b139 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.cpp +++ b/src/ipc/collisions/tangential/vertex_vertex.cpp @@ -9,7 +9,7 @@ namespace ipc { VertexVertexTangentialCollision::VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision) - : VertexVertexCandidate(collision.vertex0_id, collision.vertex1_id) + : VertexVertexStencil(collision.vertex0_id, collision.vertex1_id) { this->weight = collision.weight; this->weight_gradient = collision.weight_gradient; diff --git a/src/ipc/collisions/tangential/vertex_vertex.hpp b/src/ipc/collisions/tangential/vertex_vertex.hpp index 3aebec36f..69caa1a3c 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.hpp +++ b/src/ipc/collisions/tangential/vertex_vertex.hpp @@ -6,10 +6,15 @@ namespace ipc { -class VertexVertexTangentialCollision : public VertexVertexCandidate, +class VertexVertexTangentialCollision : public VertexVertexStencil, public TangentialCollision { public: - using VertexVertexCandidate::VertexVertexCandidate; + // The adapter also names dim/ndof (the stencil's dof-count overloads); keep + // this collision's own no-argument accessors. + using TangentialCollision::dim; + using TangentialCollision::ndof; + + using VertexVertexStencil::VertexVertexStencil; VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision); diff --git a/src/ipc/esp/esp_collisions.cpp b/src/ipc/esp/esp_collisions.cpp index 4c8f7b947..66b446228 100644 --- a/src/ipc/esp/esp_collisions.cpp +++ b/src/ipc/esp/esp_collisions.cpp @@ -329,8 +329,11 @@ double ESPCollisions::compute_minimum_distance( double& local_min_dist = storage.local(); for (size_t i = r.begin(); i < r.end(); i++) { - const double dist = m_candidates[i].compute_distance( - m_candidates[i].dof(vertices, edges, faces)); + const double dist = + m_candidates.visit(i, [&](const auto& candidate) { + return candidate.compute_distance( + candidate.dof(vertices, edges, faces)); + }); local_min_dist = std::min(dist, local_min_dist); } diff --git a/src/ipc/gcp/gcp_collisions.cpp b/src/ipc/gcp/gcp_collisions.cpp index 5c21eaff3..d5d760a48 100644 --- a/src/ipc/gcp/gcp_collisions.cpp +++ b/src/ipc/gcp/gcp_collisions.cpp @@ -266,8 +266,9 @@ double GCPCollisions::compute_minimum_distance( tbb::parallel_for(size_t(0), m_candidates.size(), [&](size_t i) { double& local_min_dist = storage.local(); - const double dist = m_candidates[i].compute_distance( - m_candidates[i].dof(vertices, edges, faces)); + const double dist = m_candidates.visit(i, [&](const auto& c) { + return c.compute_distance(c.dof(vertices, edges, faces)); + }); local_min_dist = std::min(dist, local_min_dist); }); diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index 0c4be0638..3caeee7bf 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -243,14 +243,21 @@ void TrustRegion::planar_filter_step( } tbb::parallel_for(size_t(0), candidates.size(), [&](size_t i) { - const CollisionStencil& c = candidates[i]; - const auto ids = c.vertex_ids(mesh.edges(), mesh.faces()); - const int nv = c.num_vertices(); - const VectorMax12d pos = c.dof(vertices, mesh.edges(), mesh.faces()); - + // The candidate types are not polymorphic, so take what is needed + // from the visitor and work with values from here on. // dv = c_first − c_second; positive coeffs → first primitive + std::array ids {}; + int nv = 0; + VectorMax12d pos; VectorMax4d coeffs; - const VectorMax3d dv = c.compute_distance_vector(pos, coeffs); + VectorMax3d dv; + candidates.visit(i, [&](const auto& c) { + ids = c.vertex_ids(mesh.edges(), mesh.faces()); + nv = c.num_vertices(); + pos = c.dof(vertices, mesh.edges(), mesh.faces()); + dv = c.compute_distance_vector(pos, coeffs); + }); + const double dist = dv.norm(); if (dist < 1e-10) { return; diff --git a/tests/src/tests/candidates/test_candidates.cpp b/tests/src/tests/candidates/test_candidates.cpp index 32af0d84f..f753b0c88 100644 --- a/tests/src/tests/candidates/test_candidates.cpp +++ b/tests/src/tests/candidates/test_candidates.cpp @@ -51,65 +51,21 @@ TEST_CASE("Candidates", "[candidates]") CHECK(candidates.size() == 10); - CHECK( - dynamic_cast(candidates[0]) - == candidates.vv_candidates[0]); - CHECK( - dynamic_cast(candidates[2]) - == candidates.ev_candidates[0]); - CHECK( - dynamic_cast(candidates[4]) - == candidates.ee_candidates[0]); - CHECK( - dynamic_cast(candidates[6]) - == candidates.fv_candidates[0]); - CHECK(&(candidates[8]) == &(candidates.pv_candidates[0])); - - CHECK(&(candidates[0]) == &(candidates.vv_candidates[0])); - CHECK(&(candidates[2]) == &(candidates.ev_candidates[0])); - CHECK(&(candidates[4]) == &(candidates.ee_candidates[0])); - CHECK(&(candidates[6]) == &(candidates.fv_candidates[0])); - CHECK(&(candidates[8]) == &(candidates.pv_candidates[0])); - - try { - candidates[candidates.size()]; - FAIL("Should have thrown an exception"); - } catch (const std::out_of_range& e) { - SUCCEED("Exception thrown"); - CHECK( - e.what() - == fmt::format( - "Candidate index {} is out of range! Total candidates: {}", - candidates.size(), candidates.size())); - } catch (...) { - FAIL("Uknown exception thrown"); - } + auto candidates_match = [&](const int i, const auto& b) -> bool { + return candidates.visit(i, [&](const auto& a) { + return static_cast(&a) + == static_cast(&(b)); + }); + }; - const Candidates& const_candidates = candidates; - CHECK( - const_candidates[0].as() - == candidates.vv_candidates[0]); - CHECK( - const_candidates[2].as() - == candidates.ev_candidates[0]); - CHECK( - const_candidates[4].as() - == candidates.ee_candidates[0]); - CHECK( - const_candidates[6].as() - == candidates.fv_candidates[0]); - CHECK( - const_candidates[8].as() - == candidates.pv_candidates[0]); - - CHECK(&(const_candidates[0]) == &(candidates.vv_candidates[0])); - CHECK(&(const_candidates[2]) == &(candidates.ev_candidates[0])); - CHECK(&(const_candidates[4]) == &(candidates.ee_candidates[0])); - CHECK(&(const_candidates[6]) == &(candidates.fv_candidates[0])); - CHECK(&(const_candidates[8]) == &(candidates.pv_candidates[0])); + CHECK(candidates_match(0, candidates.vv_candidates[0])); + CHECK(candidates_match(2, candidates.ev_candidates[0])); + CHECK(candidates_match(4, candidates.ee_candidates[0])); + CHECK(candidates_match(6, candidates.fv_candidates[0])); + CHECK(candidates_match(8, candidates.pv_candidates[0])); try { - const_candidates[candidates.size()]; + candidates.visit(candidates.size(), [](const auto&) { }); FAIL("Should have thrown an exception"); } catch (const std::out_of_range& e) { SUCCEED("Exception thrown"); diff --git a/tests/src/tests/collisions/test_plane_vertex_collisions.cpp b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp index 258791319..6bdf9385a 100644 --- a/tests/src/tests/collisions/test_plane_vertex_collisions.cpp +++ b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp @@ -285,13 +285,10 @@ TEST_CASE( + candidates.ev_candidates.size() + candidates.ee_candidates.size() + candidates.fv_candidates.size(); - auto& pv_ref = candidates[pv_start]; - CHECK(&pv_ref == &candidates.pv_candidates[0]); - - // const version - const Candidates& const_cand = candidates; - auto& pv_const_ref = const_cand[pv_start]; - CHECK(&pv_const_ref == &candidates.pv_candidates[0]); + CHECK(candidates.visit(pv_start, [&](const auto& c) { + return static_cast(&c) + == static_cast(&(candidates.pv_candidates[0])); + })); } } diff --git a/tests/src/tests/potential/test_distance_vector_methods.cpp b/tests/src/tests/potential/test_distance_vector_methods.cpp index ca5cf7def..2292e500a 100644 --- a/tests/src/tests/potential/test_distance_vector_methods.cpp +++ b/tests/src/tests/potential/test_distance_vector_methods.cpp @@ -111,7 +111,7 @@ TEST_CASE( Eigen::MatrixXd V(2, 3); V << -1, 0, 0, /**/ 1, 0, 0; Eigen::MatrixXi E, F; - collision = std::make_unique(0, 1); + collision = std::make_unique(0, 1); positions = collision->dof(V, E, F); } SECTION("Edge-vertex") @@ -120,7 +120,7 @@ TEST_CASE( V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0; Eigen::MatrixXi E(1, 2), F; E << 0, 1; - collision = std::make_unique(0, 2); + collision = std::make_unique(0, 2); positions = collision->dof(V, E, F); } SECTION("Edge-edge") @@ -129,7 +129,7 @@ TEST_CASE( V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, -1, 1, /**/ 0, 1, 1; Eigen::MatrixXi E(2, 2), F; E << 0, 1, /**/ 2, 3; - collision = std::make_unique(0, 1); + collision = std::make_unique(0, 1); positions = collision->dof(V, E, F); } SECTION("Face-vertex") @@ -140,7 +140,7 @@ TEST_CASE( F << 0, 1, 2; Eigen::MatrixXi E; igl::edges(F, E); - collision = std::make_unique(0, 3); + collision = std::make_unique(0, 3); positions = collision->dof(V, E, F); } From e1a6fcb8478894115ae35d7dde994f925981efc4 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 16 Sep 2026 17:11:45 -0400 Subject: [PATCH 2/6] Merge the thread-local candidate buffers in parallel Each thread-local buffer lands in its own slice of the output, so once a prefix sum over their sizes gives the offsets the copies are independent. The trivially-copyable path now issues one memcpy per buffer through a tbb::parallel_for instead of walking them in order. Below 1 MiB of payload the copies are short enough that the parallel_for and the two allocations its bookkeeping needs cost more than they save, so that case keeps the in-order loop. Without the size check the Two-Cubes scene, whose whole detection is 0.06 ms, regressed 2x. Cloth-Ball, mean of 10 calls, profiling build: | Stage | Before | After | |----------|--------|-------| | traverse | 6.35 | 6.15 | | merge | 6.37 | 4.90 | | total | 12.73 | 11.06 | End-to-end edge-edge detection is 1.07x (Puffer-Ball) to 1.17x (Armadillo-Rollers) faster, and 1.93x to 3.66x against the polymorphic candidates this series started from. The merge gains less than the copies alone would suggest because out.resize() value-initializes the whole buffer before the memcpys overwrite it, and that pass is still serial. Co-Authored-By: Claude Opus 5 --- src/ipc/utils/merge_thread_local.hpp | 53 +++++++++++++++++++++++----- 1 file changed, 44 insertions(+), 9 deletions(-) diff --git a/src/ipc/utils/merge_thread_local.hpp b/src/ipc/utils/merge_thread_local.hpp index 3b5cd7225..866ac7cfa 100644 --- a/src/ipc/utils/merge_thread_local.hpp +++ b/src/ipc/utils/merge_thread_local.hpp @@ -9,13 +9,20 @@ #include #include +#include +#include // std::memcpy #include // std::make_move_iterator #include #include namespace ipc { +/// @brief Payload size below which merging in parallel costs more than it +/// saves: the copies are short enough that the parallel_for's own overhead +/// dominates. +inline constexpr size_t PARALLEL_MERGE_MIN_BYTES = 1 << 20; // 1 MiB + // Assumes `out` is empty at the start. The function may modify the provided // `vectors` (stealing and clearing per-thread buffers) for performance. template @@ -38,19 +45,47 @@ void merge_thread_local_vectors( } // Fast path for trivially-copyable types: allocate once and memcpy each - // thread-local buffer into the contiguous destination. + // thread-local buffer into the contiguous destination. Each buffer lands + // in its own slice of `out`, so once the offsets are known with a prefix + // sum the copies are independent and can run concurrently. if constexpr ( std::is_trivially_copyable_v && std::is_default_constructible_v) { out.resize(total); - char* dest = reinterpret_cast(out.data()); - for (auto& v : vectors) { - if (v.empty()) { - continue; + + if (total * sizeof(T) < PARALLEL_MERGE_MIN_BYTES) { + // Copy in order. The bookkeeping the parallel path needs costs + // two allocations, which is more than it saves at this size. + char* dest = reinterpret_cast(out.data()); + for (auto& v : vectors) { + if (v.empty()) { + continue; + } + std::memcpy(dest, v.data(), v.size() * sizeof(T)); + dest += v.size() * sizeof(T); + // release the local buffer to reduce memory usage + std::vector().swap(v); } - std::memcpy(dest, v.data(), v.size() * sizeof(T)); - dest += v.size() * sizeof(T); - // release the local buffer to reduce memory usage - std::vector().swap(v); + } else { + std::vector*> blocks; + blocks.reserve(vectors.size()); + for (auto& v : vectors) { + if (!v.empty()) { + blocks.push_back(&v); + } + } + + std::vector offsets(blocks.size() + 1, 0); + for (size_t i = 0; i < blocks.size(); i++) { + offsets[i + 1] = offsets[i] + blocks[i]->size(); + } + + tbb::parallel_for(size_t(0), blocks.size(), [&](const size_t i) { + std::memcpy( + out.data() + offsets[i], blocks[i]->data(), + blocks[i]->size() * sizeof(T)); + // release the local buffer to reduce memory usage + std::vector().swap(*blocks[i]); + }); } } else { // For non-trivial types, steal the largest thread-local buffer into From f8f4003b27db22a9aae83eaf2031f86dc20916bf Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 16 Sep 2026 17:30:06 -0400 Subject: [PATCH 3/6] Stop zeroing the candidate buffer before overwriting it The merge sizes the output with resize() and then memcpys every element into it, so the value-initialization resize() performs first is a wasted pass: zeroing Cloth-Ball's 5.2M edge-edge candidates costs 3.3 ms of a 3.8 ms merge. DefaultInitAllocator already exists for this, and LBVH::Nodes and AABBs already use it. `CandidateVector` (candidates/candidate_vector.hpp) is a std::vector with that allocator, and the broad phases now produce and consume it. merge_thread_local_vectors is templated on both allocators, so the thread-local buffers and the output no longer have to agree, and the steal in the non-trivial path is skipped when they differ. Note that clear() was never the cost: the candidates became trivially destructible when they lost their vptr, so clearing is already O(1). It is the resize that follows which was paying. Edge-edge detection, median of three runs of 20 samples: | Scene | Before | After | Speedup | |-------------------|-----------|-----------|---------| | Armadillo-Rollers | 9.22 ms | 7.03 ms | 1.31x | | Rod-Twist | 4.95 ms | 3.86 ms | 1.28x | | Cloth-Ball | 11.63 ms | 9.11 ms | 1.28x | | N-Body-Simulation | 36.31 ms | 24.86 ms | 1.46x | | Puffer-Ball | 307.75 ms | 211.28 ms | 1.46x | The merge stage itself, mean of 10 calls in a profiling build: Cloth-Ball 4.90 to 2.38 ms, Puffer-Ball 154.27 to 47.37 ms. Against the polymorphic candidates this series started from, the merge is 7.2x and 14.5x faster and detection overall is 3.29x and 5.35x. BREAKING: the broad phase detect_*_candidates() methods, Candidates' members and the candidate-taking helpers now use CandidateVector rather than std::vector. Elements left behind by a resize that grows the vector are uninitialized, so read only what has been written. Co-Authored-By: Claude Opus 5 --- python/src/broad_phase/broad_phase.cpp | 43 +++++++++++-------- python/src/candidates/candidates.cpp | 17 ++++---- src/ipc/broad_phase/broad_phase.hpp | 13 +++--- src/ipc/broad_phase/brute_force.cpp | 17 ++++---- src/ipc/broad_phase/brute_force.hpp | 15 ++++--- src/ipc/broad_phase/cuda/lbvh.cu | 18 ++++---- src/ipc/broad_phase/cuda/lbvh.hpp | 13 +++--- src/ipc/broad_phase/hash_grid.cpp | 21 ++++----- src/ipc/broad_phase/hash_grid.hpp | 17 ++++---- src/ipc/broad_phase/lbvh.cpp | 25 +++++------ src/ipc/broad_phase/lbvh.hpp | 17 ++++---- src/ipc/broad_phase/spatial_hash.cpp | 19 ++++---- src/ipc/broad_phase/spatial_hash.hpp | 13 +++--- src/ipc/broad_phase/sweep_and_prune.cpp | 14 +++--- src/ipc/broad_phase/sweep_and_prune.hpp | 13 +++--- .../broad_phase/sweep_and_tiniest_queue.cu | 14 +++--- .../broad_phase/sweep_and_tiniest_queue.hpp | 13 +++--- src/ipc/candidates/candidate_vector.hpp | 23 ++++++++++ src/ipc/candidates/candidates.cpp | 19 ++++---- src/ipc/candidates/candidates.hpp | 25 +++++------ src/ipc/candidates/edge_edge.hpp | 1 + src/ipc/candidates/edge_face.hpp | 1 + src/ipc/candidates/edge_vertex.hpp | 1 + src/ipc/candidates/face_face.hpp | 1 + src/ipc/candidates/face_vertex.hpp | 1 + src/ipc/candidates/plane_vertex.hpp | 1 + src/ipc/candidates/vertex_vertex.hpp | 1 + src/ipc/esp/esp_collisions_builder.cpp | 2 +- src/ipc/esp/esp_collisions_builder.hpp | 8 ++-- src/ipc/gcp/gcp_collisions_builder.cpp | 7 +-- src/ipc/gcp/gcp_collisions_builder.hpp | 7 +-- src/ipc/ipc.cpp | 5 ++- src/ipc/utils/merge_thread_local.hpp | 30 +++++++------ src/ipc/utils/save_obj.cpp | 11 ++--- src/ipc/utils/save_obj.hpp | 5 ++- .../broad_phase/benchmark_lbvh_stages.cu | 5 ++- .../broad_phase/brute_force_comparison.cpp | 17 ++++---- .../broad_phase/brute_force_comparison.hpp | 5 ++- .../tests/broad_phase/test_broad_phase.cpp | 7 +-- tests/src/tests/broad_phase/test_gpu_lbvh.cu | 39 +++++++++-------- tests/src/tests/broad_phase/test_lbvh.cpp | 42 +++++++++--------- tests/src/tests/broad_phase/test_stq.cpp | 9 ++-- 42 files changed, 326 insertions(+), 249 deletions(-) create mode 100644 src/ipc/candidates/candidate_vector.hpp diff --git a/python/src/broad_phase/broad_phase.cpp b/python/src/broad_phase/broad_phase.cpp index 812599b4b..adf7108b6 100644 --- a/python/src/broad_phase/broad_phase.cpp +++ b/python/src/broad_phase/broad_phase.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -39,13 +40,14 @@ class PyBroadPhase : public BroadPhase { void clear() override { PYBIND11_OVERRIDE(void, BroadPhase, clear); } void detect_vertex_vertex_candidates( - std::vector& candidates) const override + ipc::CandidateVector& candidates) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "detect_vertex_vertex_candidates"); if (override) { - candidates = override().cast>(); + candidates = + override().cast>(); return; } throw std::runtime_error( @@ -53,13 +55,14 @@ class PyBroadPhase : public BroadPhase { } void detect_edge_vertex_candidates( - std::vector& candidates) const override + ipc::CandidateVector& candidates) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "detect_edge_vertex_candidates"); if (override) { - candidates = override().cast>(); + candidates = + override().cast>(); return; } throw std::runtime_error( @@ -67,13 +70,14 @@ class PyBroadPhase : public BroadPhase { } void detect_edge_edge_candidates( - std::vector& candidates) const override + ipc::CandidateVector& candidates) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "detect_edge_edge_candidates"); if (override) { - candidates = override().cast>(); + candidates = + override().cast>(); return; } throw std::runtime_error( @@ -81,13 +85,14 @@ class PyBroadPhase : public BroadPhase { } void detect_face_vertex_candidates( - std::vector& candidates) const override + ipc::CandidateVector& candidates) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "detect_face_vertex_candidates"); if (override) { - candidates = override().cast>(); + candidates = + override().cast>(); return; } throw std::runtime_error( @@ -95,13 +100,14 @@ class PyBroadPhase : public BroadPhase { } void detect_edge_face_candidates( - std::vector& candidates) const override + ipc::CandidateVector& candidates) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "detect_edge_face_candidates"); if (override) { - candidates = override().cast>(); + candidates = + override().cast>(); return; } throw std::runtime_error( @@ -109,13 +115,14 @@ class PyBroadPhase : public BroadPhase { } void detect_face_face_candidates( - std::vector& candidates) const override + ipc::CandidateVector& candidates) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "detect_face_face_candidates"); if (override) { - candidates = override().cast>(); + candidates = + override().cast>(); return; } throw std::runtime_error( @@ -170,7 +177,7 @@ void define_broad_phase(py::module_& m) .def( "detect_vertex_vertex_candidates", [](const BroadPhase& self) { - std::vector candidates; + ipc::CandidateVector candidates; self.detect_vertex_vertex_candidates(candidates); return candidates; }, @@ -183,7 +190,7 @@ void define_broad_phase(py::module_& m) .def( "detect_edge_vertex_candidates", [](const BroadPhase& self) { - std::vector candidates; + ipc::CandidateVector candidates; self.detect_edge_vertex_candidates(candidates); return candidates; }, @@ -196,7 +203,7 @@ void define_broad_phase(py::module_& m) .def( "detect_edge_edge_candidates", [](const BroadPhase& self) { - std::vector candidates; + ipc::CandidateVector candidates; self.detect_edge_edge_candidates(candidates); return candidates; }, @@ -209,7 +216,7 @@ void define_broad_phase(py::module_& m) .def( "detect_face_vertex_candidates", [](const BroadPhase& self) { - std::vector candidates; + ipc::CandidateVector candidates; self.detect_face_vertex_candidates(candidates); return candidates; }, @@ -222,7 +229,7 @@ void define_broad_phase(py::module_& m) .def( "detect_edge_face_candidates", [](const BroadPhase& self) { - std::vector candidates; + ipc::CandidateVector candidates; self.detect_edge_face_candidates(candidates); return candidates; }, @@ -235,7 +242,7 @@ void define_broad_phase(py::module_& m) .def( "detect_face_face_candidates", [](const BroadPhase& self) { - std::vector candidates; + ipc::CandidateVector candidates; self.detect_face_face_candidates(candidates); return candidates; }, diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index 44c28cafe..d902a294b 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -162,8 +163,8 @@ void define_candidates(py::module_& m) .def( "edge_vertex_to_vertex_vertex", [](const Candidates& C, const CollisionMesh& mesh, - Eigen::ConstRef vertices, - py::object is_active) -> std::vector { + Eigen::ConstRef vertices, py::object is_active) + -> ipc::CandidateVector { std::function is_active_fn = [](double) { return true; }; @@ -190,8 +191,8 @@ void define_candidates(py::module_& m) .def( "face_vertex_to_vertex_vertex", [](const Candidates& C, const CollisionMesh& mesh, - Eigen::ConstRef vertices, - py::object is_active) -> std::vector { + Eigen::ConstRef vertices, py::object is_active) + -> ipc::CandidateVector { std::function is_active_fn = [](double) { return true; }; @@ -218,8 +219,8 @@ void define_candidates(py::module_& m) .def( "face_vertex_to_edge_vertex", [](const Candidates& C, const CollisionMesh& mesh, - Eigen::ConstRef vertices, - py::object is_active) -> std::vector { + Eigen::ConstRef vertices, py::object is_active) + -> ipc::CandidateVector { std::function is_active_fn = [](double) { return true; }; @@ -246,8 +247,8 @@ void define_candidates(py::module_& m) .def( "edge_edge_to_edge_vertex", [](const Candidates& C, const CollisionMesh& mesh, - Eigen::ConstRef vertices, - py::object is_active) -> std::vector { + Eigen::ConstRef vertices, py::object is_active) + -> ipc::CandidateVector { std::function is_active_fn = [](double) { return true; }; diff --git a/src/ipc/broad_phase/broad_phase.hpp b/src/ipc/broad_phase/broad_phase.hpp index 0168b8f83..e65ee74bf 100644 --- a/src/ipc/broad_phase/broad_phase.hpp +++ b/src/ipc/broad_phase/broad_phase.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -73,32 +74,32 @@ class BroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions (cleared first). virtual void detect_vertex_vertex_candidates( - std::vector& candidates) const = 0; + CandidateVector& candidates) const = 0; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions (cleared first). virtual void detect_edge_vertex_candidates( - std::vector& candidates) const = 0; + CandidateVector& candidates) const = 0; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions (cleared first). virtual void detect_edge_edge_candidates( - std::vector& candidates) const = 0; + CandidateVector& candidates) const = 0; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions (cleared first). virtual void detect_face_vertex_candidates( - std::vector& candidates) const = 0; + CandidateVector& candidates) const = 0; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections (cleared first). virtual void detect_edge_face_candidates( - std::vector& candidates) const = 0; + CandidateVector& candidates) const = 0; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions (cleared first). virtual void detect_face_face_candidates( - std::vector& candidates) const = 0; + CandidateVector& candidates) const = 0; /// @brief Filter for determining if two vertices can collide. CollisionFilter can_vertices_collide; diff --git a/src/ipc/broad_phase/brute_force.cpp b/src/ipc/broad_phase/brute_force.cpp index 9e9425254..4c87dfdd7 100644 --- a/src/ipc/broad_phase/brute_force.cpp +++ b/src/ipc/broad_phase/brute_force.cpp @@ -1,5 +1,6 @@ #include "brute_force.hpp" +#include #include #include @@ -17,9 +18,9 @@ void BruteForce::detect_candidates( const AABBs& boxes0, const AABBs& boxes1, const std::function& can_collide, - std::vector& candidates) const + CandidateVector& candidates) const { - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; tbb::parallel_for( tbb::blocked_range2d(0UL, boxes0.size(), 0UL, boxes1.size()), @@ -61,7 +62,7 @@ void BruteForce::detect_candidates( } void BruteForce::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -69,7 +70,7 @@ void BruteForce::detect_vertex_vertex_candidates( } void BruteForce::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -79,7 +80,7 @@ void BruteForce::detect_edge_vertex_candidates( } void BruteForce::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -88,7 +89,7 @@ void BruteForce::detect_edge_edge_candidates( } void BruteForce::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -98,7 +99,7 @@ void BruteForce::detect_face_vertex_candidates( } void BruteForce::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -108,7 +109,7 @@ void BruteForce::detect_edge_face_candidates( } void BruteForce::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( diff --git a/src/ipc/broad_phase/brute_force.hpp b/src/ipc/broad_phase/brute_force.hpp index 429fb10c0..006f4bd68 100644 --- a/src/ipc/broad_phase/brute_force.hpp +++ b/src/ipc/broad_phase/brute_force.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -16,32 +17,32 @@ class BruteForce : public BroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions. void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions. void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions. void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; private: /// @brief Detect candidates for collisions between two sets of boxes. @@ -56,7 +57,7 @@ class BruteForce : public BroadPhase { const AABBs& boxes0, const AABBs& boxes1, const std::function& can_collide, - std::vector& candidates) const; + CandidateVector& candidates) const; }; } // namespace ipc diff --git a/src/ipc/broad_phase/cuda/lbvh.cu b/src/ipc/broad_phase/cuda/lbvh.cu index c63136c09..230008325 100644 --- a/src/ipc/broad_phase/cuda/lbvh.cu +++ b/src/ipc/broad_phase/cuda/lbvh.cu @@ -1,5 +1,7 @@ #include "lbvh.hpp" +#include + #ifdef IPC_TOOLKIT_WITH_CUDA #include @@ -988,7 +990,7 @@ namespace { const LBVH::Impl::DeviceCandidates& buf, const CollisionFilter& filter, const CanCollide& can_collide, - std::vector& out) + CandidateVector& out) { out.clear(); const size_t count = buf.count; @@ -1032,7 +1034,7 @@ namespace { LBVH::Impl& impl, const CollisionFilter& filter, const CanCollide& can_collide, - std::vector& out) + CandidateVector& out) { // The detect_*() methods are const on the BroadPhase interface, and // ipc::LBVH's really are read-only, so a caller may legitimately run @@ -1218,7 +1220,7 @@ void LBVH::clear() // user vertex filter is applied on the host only when it is not accept-all. void LBVH::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { detect_host( impl(), can_vertices_collide, @@ -1227,7 +1229,7 @@ void LBVH::detect_vertex_vertex_candidates( } void LBVH::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { detect_host( impl(), can_vertices_collide, @@ -1236,7 +1238,7 @@ void LBVH::detect_edge_vertex_candidates( } void LBVH::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { detect_host( impl(), can_vertices_collide, @@ -1245,7 +1247,7 @@ void LBVH::detect_edge_edge_candidates( } void LBVH::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { detect_host( impl(), can_vertices_collide, @@ -1254,7 +1256,7 @@ void LBVH::detect_face_vertex_candidates( } void LBVH::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { detect_host( impl(), can_vertices_collide, @@ -1263,7 +1265,7 @@ void LBVH::detect_edge_face_candidates( } void LBVH::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { detect_host( impl(), can_vertices_collide, diff --git a/src/ipc/broad_phase/cuda/lbvh.hpp b/src/ipc/broad_phase/cuda/lbvh.hpp index af974d721..528c3b3f3 100644 --- a/src/ipc/broad_phase/cuda/lbvh.hpp +++ b/src/ipc/broad_phase/cuda/lbvh.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #ifdef IPC_TOOLKIT_WITH_CUDA @@ -114,17 +115,17 @@ class LBVH : public ipc::BroadPhase { // of the same type. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; // ------------------------------------------------------------------ // Device-resident candidate accessors (GPU-native pipeline). Each runs the diff --git a/src/ipc/broad_phase/hash_grid.cpp b/src/ipc/broad_phase/hash_grid.cpp index 58e1c41dd..d2c89dd14 100644 --- a/src/ipc/broad_phase/hash_grid.cpp +++ b/src/ipc/broad_phase/hash_grid.cpp @@ -1,6 +1,7 @@ #include "hash_grid.hpp" #include +#include #include #include @@ -110,7 +111,7 @@ void HashGrid::detect_candidates( const AABBs& boxes0, const AABBs& boxes1, const std::function& can_collide, - std::vector& candidates) const + CandidateVector& candidates) const { // Entries with the same key means they share a cell (that cell index // hashes to the same key) and should be flagged for low-level intersection @@ -145,7 +146,7 @@ void HashGrid::detect_candidates( // 2. Enumerate hash collisions #ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; #else tbb::enumerable_thread_specific> storage; #endif @@ -216,7 +217,7 @@ void HashGrid::detect_candidates( const std::vector& items, const AABBs& boxes, const std::function& can_collide, - std::vector& candidates) const + CandidateVector& candidates) const { // Entries with the same key means they share a cell (that cell index // hashes to the same key) and should be flagged for low-level @@ -224,7 +225,7 @@ void HashGrid::detect_candidates( // (key,value) pairs creating Candidate entries for pairs with the same key #ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; #else tbb::enumerable_thread_specific> storage; #endif @@ -284,7 +285,7 @@ void HashGrid::detect_candidates( } void HashGrid::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -292,7 +293,7 @@ void HashGrid::detect_vertex_vertex_candidates( } void HashGrid::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -302,7 +303,7 @@ void HashGrid::detect_edge_vertex_candidates( } void HashGrid::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -311,7 +312,7 @@ void HashGrid::detect_edge_edge_candidates( } void HashGrid::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -321,7 +322,7 @@ void HashGrid::detect_face_vertex_candidates( } void HashGrid::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( @@ -330,7 +331,7 @@ void HashGrid::detect_edge_face_candidates( } void HashGrid::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); detect_candidates( diff --git a/src/ipc/broad_phase/hash_grid.hpp b/src/ipc/broad_phase/hash_grid.hpp index 431ad94b7..09341cf35 100644 --- a/src/ipc/broad_phase/hash_grid.hpp +++ b/src/ipc/broad_phase/hash_grid.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -46,32 +47,32 @@ class HashGrid : public BroadPhase { /// @brief Find the candidate vertex-vertex collisions. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions. void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions. void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions. void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; double cell_size() const { return m_cell_size; } // TODO: Update this @@ -127,7 +128,7 @@ class HashGrid : public BroadPhase { const AABBs& boxes0, const AABBs& boxes1, const std::function& can_collide, - std::vector& candidates) const; + CandidateVector& candidates) const; /// @brief Find the candidate collisions among a set of items. /// @tparam Candidate The type of collision candidate. @@ -140,7 +141,7 @@ class HashGrid : public BroadPhase { const std::vector& items, const AABBs& boxes, const std::function& can_collide, - std::vector& candidates) const; + CandidateVector& candidates) const; protected: double m_cell_size = -1; diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp index 1b4e054b0..d5c7adb0c 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -202,7 +203,7 @@ namespace { const LBVH::Node& query, const LBVH::Node& node, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { int i = query.primitive_id, j = node.primitive_id; if constexpr (swap_order) { @@ -227,7 +228,7 @@ namespace { const LBVH::Nodes& lbvh, const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { details::traverse_lbvh( int(query_leaf_idx), lbvh.data(), int(lbvh.size()), @@ -254,7 +255,7 @@ namespace { const LBVH::Nodes& lbvh, const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { using batch_t = xs::batch; using mask_t = xs::batch_bool; @@ -337,7 +338,7 @@ namespace { const LBVH::Nodes& target, const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, - tbb::enumerable_thread_specific>& storage) + tbb::enumerable_thread_specific>& storage) { #ifdef IPC_TOOLKIT_WITH_SIMD // Enable SIMD acceleration when available constexpr size_t SIMD_SIZE = use_simd ? xs::batch::size : 1; @@ -393,13 +394,13 @@ void LBVH::detect_candidates( const Nodes& target, const RightmostLeaves& rightmost_leaves, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { if (source.empty() || target.empty()) { return; } - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; { IPC_TOOLKIT_PROFILE_BLOCK("traverse"); @@ -414,7 +415,7 @@ void LBVH::detect_candidates( } void LBVH::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (vertex_bvh.size() <= 1) { // Need at least 2 vertices for a collision @@ -428,7 +429,7 @@ void LBVH::detect_vertex_vertex_candidates( } void LBVH::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (!has_edges() || !has_vertices()) { @@ -445,7 +446,7 @@ void LBVH::detect_edge_vertex_candidates( } void LBVH::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (edge_bvh.size() <= 1) { // Need at least 2 edges for a collision @@ -460,7 +461,7 @@ void LBVH::detect_edge_edge_candidates( } void LBVH::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (!has_faces() || !has_vertices()) { @@ -476,7 +477,7 @@ void LBVH::detect_face_vertex_candidates( } void LBVH::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (!has_edges() || !has_faces()) { @@ -492,7 +493,7 @@ void LBVH::detect_edge_face_candidates( } void LBVH::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (face_bvh.size() <= 1) { // Need at least 2 faces for a collision diff --git a/src/ipc/broad_phase/lbvh.hpp b/src/ipc/broad_phase/lbvh.hpp index f8e0fc5b8..c46bc74f9 100644 --- a/src/ipc/broad_phase/lbvh.hpp +++ b/src/ipc/broad_phase/lbvh.hpp @@ -2,6 +2,7 @@ #include // for IPC_TOOLKIT_HOST_DEVICE #include +#include #include namespace ipc { @@ -128,32 +129,32 @@ class LBVH : public BroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions. void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions. void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions. void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the primitives whose AABB is within a radius of a point. /// @@ -215,7 +216,7 @@ class LBVH : public BroadPhase { const Nodes& target, const RightmostLeaves& rightmost_leaves, const std::function& can_collide, - std::vector& candidates); + CandidateVector& candidates); /// @brief Detect candidate collisions between a single LBVH and itself. /// @tparam Candidate Type of candidate collision. @@ -228,7 +229,7 @@ class LBVH : public BroadPhase { const Nodes& source_and_target, const RightmostLeaves& rightmost_leaves, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { detect_candidates( source_and_target, source_and_target, rightmost_leaves, can_collide, diff --git a/src/ipc/broad_phase/spatial_hash.cpp b/src/ipc/broad_phase/spatial_hash.cpp index 4d66c597a..08ad8373d 100644 --- a/src/ipc/broad_phase/spatial_hash.cpp +++ b/src/ipc/broad_phase/spatial_hash.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -185,9 +186,9 @@ namespace { const AABBs& boxesB, const std::function&)>& query_A_for_Bs, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; tbb::parallel_for(size_t(0), boxesA.size(), [&](size_t i) { auto& local_candidates = storage.local(); @@ -225,7 +226,7 @@ namespace { const AABBs& boxesA, const std::function&)>& query_A_for_As, const std::function& can_collide, - std::vector& candidates) + CandidateVector& candidates) { detect_candidates( boxesA, boxesA, query_A_for_As, can_collide, candidates); @@ -234,7 +235,7 @@ namespace { } // namespace void SpatialHash::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (vertex_boxes.empty()) { @@ -250,7 +251,7 @@ void SpatialHash::detect_vertex_vertex_candidates( } void SpatialHash::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (edge_boxes.empty() || vertex_boxes.empty()) { @@ -267,7 +268,7 @@ void SpatialHash::detect_edge_vertex_candidates( } void SpatialHash::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (edge_boxes.empty()) { @@ -283,7 +284,7 @@ void SpatialHash::detect_edge_edge_candidates( } void SpatialHash::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (face_boxes.empty() || vertex_boxes.empty()) { @@ -301,7 +302,7 @@ void SpatialHash::detect_face_vertex_candidates( } void SpatialHash::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (edge_boxes.empty() || face_boxes.empty()) { @@ -318,7 +319,7 @@ void SpatialHash::detect_edge_face_candidates( } void SpatialHash::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); if (face_boxes.empty()) { diff --git a/src/ipc/broad_phase/spatial_hash.hpp b/src/ipc/broad_phase/spatial_hash.hpp index ee561d4f6..252003c3f 100644 --- a/src/ipc/broad_phase/spatial_hash.hpp +++ b/src/ipc/broad_phase/spatial_hash.hpp @@ -3,6 +3,7 @@ #pragma once #include +#include #include #include @@ -124,32 +125,32 @@ class SpatialHash : public BroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions. void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions. void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions. void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; private: // helper functions void build( diff --git a/src/ipc/broad_phase/sweep_and_prune.cpp b/src/ipc/broad_phase/sweep_and_prune.cpp index 1088c1055..b8ec2fd3e 100644 --- a/src/ipc/broad_phase/sweep_and_prune.cpp +++ b/src/ipc/broad_phase/sweep_and_prune.cpp @@ -1,5 +1,7 @@ #include "sweep_and_prune.hpp" +#include + #include #include @@ -100,7 +102,7 @@ void SweepAndPrune::clear() } void SweepAndPrune::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); std::vector> overlaps; @@ -114,7 +116,7 @@ void SweepAndPrune::detect_vertex_vertex_candidates( } void SweepAndPrune::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); std::vector> overlaps; @@ -129,7 +131,7 @@ void SweepAndPrune::detect_edge_vertex_candidates( } void SweepAndPrune::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); std::vector> overlaps; @@ -143,7 +145,7 @@ void SweepAndPrune::detect_edge_edge_candidates( } void SweepAndPrune::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); std::vector> overlaps; @@ -158,7 +160,7 @@ void SweepAndPrune::detect_face_vertex_candidates( } void SweepAndPrune::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); std::vector> overlaps; @@ -173,7 +175,7 @@ void SweepAndPrune::detect_edge_face_candidates( } void SweepAndPrune::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); std::vector> overlaps; diff --git a/src/ipc/broad_phase/sweep_and_prune.hpp b/src/ipc/broad_phase/sweep_and_prune.hpp index 5db33ed23..4ec290f2b 100644 --- a/src/ipc/broad_phase/sweep_and_prune.hpp +++ b/src/ipc/broad_phase/sweep_and_prune.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -57,32 +58,32 @@ class SweepAndPrune : public BroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions. void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions. void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions. void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; protected: bool can_edge_vertex_collide(size_t ei, size_t vi) const override; diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.cu b/src/ipc/broad_phase/sweep_and_tiniest_queue.cu index 78b21d4b5..5d852c7d4 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.cu +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.cu @@ -1,5 +1,7 @@ #include "sweep_and_tiniest_queue.hpp" +#include + #ifdef IPC_TOOLKIT_WITH_CUDA #include @@ -121,7 +123,7 @@ void SweepAndTiniestQueue::clear() } void SweepAndTiniestQueue::detect_vertex_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); scalable_ccd::cuda::BroadPhase broad_phase; @@ -137,7 +139,7 @@ void SweepAndTiniestQueue::detect_vertex_vertex_candidates( } void SweepAndTiniestQueue::detect_edge_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); scalable_ccd::cuda::BroadPhase broad_phase; @@ -154,7 +156,7 @@ void SweepAndTiniestQueue::detect_edge_vertex_candidates( } void SweepAndTiniestQueue::detect_edge_edge_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); scalable_ccd::cuda::BroadPhase broad_phase; @@ -170,7 +172,7 @@ void SweepAndTiniestQueue::detect_edge_edge_candidates( } void SweepAndTiniestQueue::detect_face_vertex_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); scalable_ccd::cuda::BroadPhase broad_phase; @@ -187,7 +189,7 @@ void SweepAndTiniestQueue::detect_face_vertex_candidates( } void SweepAndTiniestQueue::detect_edge_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); scalable_ccd::cuda::BroadPhase broad_phase; @@ -204,7 +206,7 @@ void SweepAndTiniestQueue::detect_edge_face_candidates( } void SweepAndTiniestQueue::detect_face_face_candidates( - std::vector& candidates) const + CandidateVector& candidates) const { candidates.clear(); scalable_ccd::cuda::BroadPhase broad_phase; diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp index b137d2609..577f1f53d 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #ifdef IPC_TOOLKIT_WITH_CUDA @@ -61,32 +62,32 @@ class SweepAndTiniestQueue : public BroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-vertex collisions. /// @param[out] candidates The candidate edge-vertex collisions. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisions. void detect_edge_edge_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-vertex collisions. /// @param[out] candidates The candidate face-vertex collisions. void detect_face_vertex_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; /// @brief Find the candidate face-face collisions. /// @param[out] candidates The candidate face-face collisions. void detect_face_face_candidates( - std::vector& candidates) const override; + CandidateVector& candidates) const override; private: bool can_edge_vertex_collide(size_t ei, size_t vi) const override; diff --git a/src/ipc/candidates/candidate_vector.hpp b/src/ipc/candidates/candidate_vector.hpp new file mode 100644 index 000000000..55f9b85c9 --- /dev/null +++ b/src/ipc/candidates/candidate_vector.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include + +namespace ipc { + +/// @brief Storage for broad-phase candidates. +/// +/// A std::vector that does not value-initialize on resize. The broad phase +/// sizes the output and then immediately overwrites every element, so zeroing +/// it first is a wasted pass over tens of megabytes: on Cloth-Ball's 5.2M +/// edge-edge candidates it costs 3.3 ms of a 3.8 ms merge. +/// +/// The elements are left uninitialized by a resize that grows the vector, so +/// read only what has been written. +/// +/// @tparam T The candidate type. +template +using CandidateVector = std::vector>; + +} // namespace ipc diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 5277e641c..5dbeb5211 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -571,14 +572,14 @@ namespace { /// @param is_active Function to determine if a candidate is active /// @return Vertex-vertex candidates template - std::vector + CandidateVector element_vertex_to_vertex_vertex_candidates( Eigen::ConstRef elements, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const std::function& is_active) { - std::vector vv_candidates; + CandidateVector vv_candidates; for (const auto& [ei, vi] : candidates) { for (int j = 0; j < elements.cols(); j++) { const int vj = elements(ei, j); @@ -600,7 +601,7 @@ namespace { } // namespace -std::vector Candidates::edge_vertex_to_vertex_vertex( +CandidateVector Candidates::edge_vertex_to_vertex_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active) const @@ -609,7 +610,7 @@ std::vector Candidates::edge_vertex_to_vertex_vertex( mesh.edges(), vertices, ev_candidates, is_active); } -std::vector Candidates::face_vertex_to_vertex_vertex( +CandidateVector Candidates::face_vertex_to_vertex_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active) const @@ -618,12 +619,12 @@ std::vector Candidates::face_vertex_to_vertex_vertex( mesh.faces(), vertices, fv_candidates, is_active); } -std::vector Candidates::face_vertex_to_edge_vertex( +CandidateVector Candidates::face_vertex_to_edge_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active) const { - std::vector C_ev; + CandidateVector C_ev; for (const auto& [fi, vi] : fv_candidates) { for (int j = 0; j < 3; j++) { const int ei = mesh.faces_to_edges()(fi, j); @@ -643,12 +644,12 @@ std::vector Candidates::face_vertex_to_edge_vertex( return C_ev; } -std::vector Candidates::edge_edge_to_edge_vertex( +CandidateVector Candidates::edge_edge_to_edge_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active) const { - std::vector C_ev; + CandidateVector C_ev; for (const EdgeEdgeCandidate& ee : ee_candidates) { for (int i = 0; i < 2; i++) { const int ei = i == 0 ? ee.edge0_id : ee.edge1_id; diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index bfbacc3b7..f12854068 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -252,7 +253,7 @@ class Candidates { /// @param vertices Collision mesh vertex positions (rowwise). /// @param is_active (Optional) Function to determine if a candidate is active. /// @return Vertex-vertex candidates derived from edge-vertex candidates. - std::vector edge_vertex_to_vertex_vertex( + CandidateVector edge_vertex_to_vertex_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active = default_is_active) const; @@ -262,7 +263,7 @@ class Candidates { /// @param vertices Collision mesh vertex positions (rowwise). /// @param is_active (Optional) Function to determine if a candidate is active. /// @return Vertex-vertex candidates derived from face-vertex candidates. - std::vector face_vertex_to_vertex_vertex( + CandidateVector face_vertex_to_vertex_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active = default_is_active) const; @@ -272,7 +273,7 @@ class Candidates { /// @param vertices Collision mesh vertex positions (rowwise). /// @param is_active (Optional) Function to determine if a candidate is active. /// @return Edge-vertex candidates derived from face-vertex candidates. - std::vector face_vertex_to_edge_vertex( + CandidateVector face_vertex_to_edge_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active = default_is_active) const; @@ -282,7 +283,7 @@ class Candidates { /// @param vertices Collision mesh vertex positions (rowwise). /// @param is_active (Optional) Function to determine if a candidate is active. /// @return Edge-vertex candidates derived from edge-edge candidates. - std::vector edge_edge_to_edge_vertex( + CandidateVector edge_edge_to_edge_vertex( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::function& is_active = default_is_active) const; @@ -330,14 +331,14 @@ class Candidates { AdjacencyList ff_set(const CollisionMesh& mesh, index_t id) const; public: - std::vector vv_candidates; - std::vector ev_candidates; - std::vector ee_candidates; - std::vector fv_candidates; - std::vector pv_candidates; - - std::vector ef_candidates; - std::vector ff_candidates; + CandidateVector vv_candidates; + CandidateVector ev_candidates; + CandidateVector ee_candidates; + CandidateVector fv_candidates; + CandidateVector pv_candidates; + + CandidateVector ef_candidates; + CandidateVector ff_candidates; /// @brief Whether these candidates include the types only ESP needs. /// diff --git a/src/ipc/candidates/edge_edge.hpp b/src/ipc/candidates/edge_edge.hpp index d1feeacad..4ad12e13c 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include diff --git a/src/ipc/candidates/edge_face.hpp b/src/ipc/candidates/edge_face.hpp index f1c2deb4d..9ccc5d34e 100644 --- a/src/ipc/candidates/edge_face.hpp +++ b/src/ipc/candidates/edge_face.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include diff --git a/src/ipc/candidates/edge_vertex.hpp b/src/ipc/candidates/edge_vertex.hpp index 4b9ab8558..74c147c42 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include diff --git a/src/ipc/candidates/face_face.hpp b/src/ipc/candidates/face_face.hpp index 4263dfd8d..9f6d41e4f 100644 --- a/src/ipc/candidates/face_face.hpp +++ b/src/ipc/candidates/face_face.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include diff --git a/src/ipc/candidates/face_vertex.hpp b/src/ipc/candidates/face_vertex.hpp index 9220a5ef5..cb7b5a8d9 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include diff --git a/src/ipc/candidates/plane_vertex.hpp b/src/ipc/candidates/plane_vertex.hpp index 6e0dfd76f..c26d915e2 100644 --- a/src/ipc/candidates/plane_vertex.hpp +++ b/src/ipc/candidates/plane_vertex.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include diff --git a/src/ipc/candidates/vertex_vertex.hpp b/src/ipc/candidates/vertex_vertex.hpp index 4dd87aeb6..70ec2b53d 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include diff --git a/src/ipc/esp/esp_collisions_builder.cpp b/src/ipc/esp/esp_collisions_builder.cpp index 2b307fc38..fc4490b75 100644 --- a/src/ipc/esp/esp_collisions_builder.cpp +++ b/src/ipc/esp/esp_collisions_builder.cpp @@ -340,7 +340,7 @@ void QuadratureCollisionsBuilder::build_face_collisions( void QuadratureCollisionsBuilder::build_edge_edge_collisions( Eigen::ConstRef vertices, - const std::vector& ee_candidates, + const CandidateVector& ee_candidates, const size_t start_i, const size_t end_i) { diff --git a/src/ipc/esp/esp_collisions_builder.hpp b/src/ipc/esp/esp_collisions_builder.hpp index 4f2c54721..cba40eb43 100644 --- a/src/ipc/esp/esp_collisions_builder.hpp +++ b/src/ipc/esp/esp_collisions_builder.hpp @@ -78,7 +78,7 @@ template <> class ESPCollisionsBuilder<3> { void add_face_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const ESPParameters& params, const size_t start_i, const size_t end_i); @@ -86,7 +86,7 @@ template <> class ESPCollisionsBuilder<3> { void add_face_vertex_negative_edge_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const ESPParameters& params, const size_t start_i, const size_t end_i); @@ -94,7 +94,7 @@ template <> class ESPCollisionsBuilder<3> { void add_face_vertex_positive_vertex_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const ESPParameters& params, const size_t start_i, const size_t end_i); @@ -149,7 +149,7 @@ class QuadratureCollisionsBuilder { void build_edge_edge_collisions( Eigen::ConstRef vertices, - const std::vector& ee_candidates, + const CandidateVector& ee_candidates, const size_t start_i, const size_t end_i); diff --git a/src/ipc/gcp/gcp_collisions_builder.cpp b/src/ipc/gcp/gcp_collisions_builder.cpp index 31f6d9447..368823e1b 100644 --- a/src/ipc/gcp/gcp_collisions_builder.cpp +++ b/src/ipc/gcp/gcp_collisions_builder.cpp @@ -1,5 +1,6 @@ #include "gcp_collisions_builder.hpp" +#include #include #include #include @@ -41,7 +42,7 @@ namespace { void GCPCollisionsBuilder<2>::add_edge_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, @@ -83,7 +84,7 @@ void GCPCollisionsBuilder<2>::add_edge_vertex_collisions( void GCPCollisionsBuilder<3>::add_edge_edge_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, @@ -118,7 +119,7 @@ void GCPCollisionsBuilder<3>::add_edge_edge_collisions( void GCPCollisionsBuilder<3>::add_face_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, diff --git a/src/ipc/gcp/gcp_collisions_builder.hpp b/src/ipc/gcp/gcp_collisions_builder.hpp index 64271f666..d0e63623d 100644 --- a/src/ipc/gcp/gcp_collisions_builder.hpp +++ b/src/ipc/gcp/gcp_collisions_builder.hpp @@ -6,6 +6,7 @@ #pragma once #include +#include #include #include @@ -23,7 +24,7 @@ template <> class GCPCollisionsBuilder<2> { void add_edge_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, @@ -60,7 +61,7 @@ template <> class GCPCollisionsBuilder<3> { void add_edge_edge_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, @@ -70,7 +71,7 @@ template <> class GCPCollisionsBuilder<3> { void add_face_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, + const CandidateVector& candidates, const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, diff --git a/src/ipc/ipc.cpp b/src/ipc/ipc.cpp index ee4010f6e..cf22f1685 100644 --- a/src/ipc/ipc.cpp +++ b/src/ipc/ipc.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -127,7 +128,7 @@ bool has_intersections( if (vertices.cols() == 2) { // Need to check segment-segment intersections in 2D - std::vector ee_candidates; + CandidateVector ee_candidates; broad_phase->detect_edge_edge_candidates(ee_candidates); broad_phase->clear(); @@ -147,7 +148,7 @@ bool has_intersections( // Need to check segment-triangle intersections in 3D assert(vertices.cols() == 3); - std::vector ef_candidates; + CandidateVector ef_candidates; broad_phase->detect_edge_face_candidates(ef_candidates); broad_phase->clear(); diff --git a/src/ipc/utils/merge_thread_local.hpp b/src/ipc/utils/merge_thread_local.hpp index 866ac7cfa..8916f00f5 100644 --- a/src/ipc/utils/merge_thread_local.hpp +++ b/src/ipc/utils/merge_thread_local.hpp @@ -25,10 +25,10 @@ inline constexpr size_t PARALLEL_MERGE_MIN_BYTES = 1 << 20; // 1 MiB // Assumes `out` is empty at the start. The function may modify the provided // `vectors` (stealing and clearing per-thread buffers) for performance. -template +template void merge_thread_local_vectors( - tbb::enumerable_thread_specific>& vectors, - std::vector& out) + tbb::enumerable_thread_specific>& vectors, + std::vector& out) { IPC_TOOLKIT_PROFILE_BLOCK("merge_thread_local_vectors"); @@ -63,10 +63,10 @@ void merge_thread_local_vectors( std::memcpy(dest, v.data(), v.size() * sizeof(T)); dest += v.size() * sizeof(T); // release the local buffer to reduce memory usage - std::vector().swap(v); + std::vector().swap(v); } } else { - std::vector*> blocks; + std::vector*> blocks; blocks.reserve(vectors.size()); for (auto& v : vectors) { if (!v.empty()) { @@ -84,23 +84,29 @@ void merge_thread_local_vectors( out.data() + offsets[i], blocks[i]->data(), blocks[i]->size() * sizeof(T)); // release the local buffer to reduce memory usage - std::vector().swap(*blocks[i]); + std::vector().swap(*blocks[i]); }); } } else { // For non-trivial types, steal the largest thread-local buffer into // `out` (cheap swap) and move from the remaining buffers. - std::vector* largest = nullptr; + std::vector* largest = nullptr; for (auto& v : vectors) { if (!largest || v.size() > largest->size()) { largest = &v; } } - if (largest && !largest->empty()) { - // out is empty, so swapping moves the largest contents into out and - // leaves the thread-local buffer empty (former out). - out.swap(*largest); + if constexpr (std::is_same_v) { + if (largest && !largest->empty()) { + // out is empty, so swapping moves the largest contents into + // out and leaves the thread-local buffer empty (former out). + out.swap(*largest); + } + } else { + // The buffers do not share an allocator, so the largest cannot be + // stolen; it is moved from with the rest below. + largest = nullptr; } out.reserve(total); @@ -113,7 +119,7 @@ void merge_thread_local_vectors( std::make_move_iterator(v.end())); } // Ensure capacity is released. - std::vector().swap(v); + std::vector().swap(v); } } } diff --git a/src/ipc/utils/save_obj.cpp b/src/ipc/utils/save_obj.cpp index f8d82795d..bf6cdf7b4 100644 --- a/src/ipc/utils/save_obj.cpp +++ b/src/ipc/utils/save_obj.cpp @@ -1,5 +1,6 @@ #include "save_obj.hpp" +#include #include #include #include @@ -15,7 +16,7 @@ void save_obj( Eigen::ConstRef V, Eigen::ConstRef /*unused*/, Eigen::ConstRef /*unused*/, - const std::vector& vv_candidates, + const CandidateVector& vv_candidates, const int /*unused*/) { out << "o VV\n"; @@ -31,7 +32,7 @@ void save_obj( Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, - const std::vector& ev_candidates, + const CandidateVector& ev_candidates, const int v_offset) { out << "o EV\n"; @@ -51,7 +52,7 @@ void save_obj( Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, - const std::vector& ee_candidates, + const CandidateVector& ee_candidates, const int v_offset) { out << "o EE\n"; @@ -73,7 +74,7 @@ void save_obj( Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, - const std::vector& fv_candidates, + const CandidateVector& fv_candidates, const int v_offset) { out << "o FV\n"; @@ -94,7 +95,7 @@ void save_obj( Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, - const std::vector& ef_candidates, + const CandidateVector& ef_candidates, const int v_offset) { out << "o EF\n"; diff --git a/src/ipc/utils/save_obj.hpp b/src/ipc/utils/save_obj.hpp index 819b323f2..1a3fc8a35 100644 --- a/src/ipc/utils/save_obj.hpp +++ b/src/ipc/utils/save_obj.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -14,7 +15,7 @@ void save_obj( Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, - const std::vector& candidates, + const CandidateVector& candidates, const int v_offset = 0); template @@ -23,7 +24,7 @@ bool save_obj( Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, - const std::vector& candidates) + const CandidateVector& candidates) { std::ofstream obj(filename); if (!obj.is_open()) { diff --git a/tests/src/tests/broad_phase/benchmark_lbvh_stages.cu b/tests/src/tests/broad_phase/benchmark_lbvh_stages.cu index 5c16087da..da8ab21a8 100644 --- a/tests/src/tests/broad_phase/benchmark_lbvh_stages.cu +++ b/tests/src/tests/broad_phase/benchmark_lbvh_stages.cu @@ -16,6 +16,7 @@ // IPC_TOOLKIT_BENCH_SAMPLES timed calls per scene and phase (default 10) // IPC_TOOLKIT_BENCH_OUTPUT write the results as JSON to this path +#include #include #if defined(IPC_TOOLKIT_WITH_CUDA) && defined(IPC_TOOLKIT_WITH_PROFILER) @@ -115,12 +116,12 @@ TEST_CASE("Benchmark LBVH stages", "[!benchmark][broad_phase][lbvh_stages]") // calls -- on Cloth-Ball that is 42 MB per call. size_t n_cpu = 0, n_gpu = 0; scene["cpu_detect"] = profile_calls(samples, [&]() { - std::vector candidates; + ipc::CandidateVector candidates; cpu.detect_edge_edge_candidates(candidates); n_cpu = candidates.size(); }); scene["gpu_detect"] = profile_calls(samples, [&]() { - std::vector candidates; + ipc::CandidateVector candidates; gpu.detect_edge_edge_candidates(candidates); n_gpu = candidates.size(); }); diff --git a/tests/src/tests/broad_phase/brute_force_comparison.cpp b/tests/src/tests/broad_phase/brute_force_comparison.cpp index 42d93cf63..d6b35ac09 100644 --- a/tests/src/tests/broad_phase/brute_force_comparison.cpp +++ b/tests/src/tests/broad_phase/brute_force_comparison.cpp @@ -1,3 +1,4 @@ +#include #include "brute_force_comparison.hpp" #include @@ -122,8 +123,8 @@ void brute_force_comparison( const ipc::CollisionMesh& mesh, const Eigen::MatrixXd& V0, const Eigen::MatrixXd& V1, - std::vector& candidates, - std::vector& bf_candidates) + ipc::CandidateVector& candidates, + ipc::CandidateVector& bf_candidates) { CHECK(candidates.size() <= bf_candidates.size()); @@ -152,17 +153,17 @@ template void brute_force_comparison( const ipc::CollisionMesh& mesh, const Eigen::MatrixXd& V0, const Eigen::MatrixXd& V1, - std::vector& candidates, - std::vector& bf_candidates); + ipc::CandidateVector& candidates, + ipc::CandidateVector& bf_candidates); template void brute_force_comparison( const ipc::CollisionMesh& mesh, const Eigen::MatrixXd& V0, const Eigen::MatrixXd& V1, - std::vector& candidates, - std::vector& bf_candidates); + ipc::CandidateVector& candidates, + ipc::CandidateVector& bf_candidates); template void brute_force_comparison( const ipc::CollisionMesh& mesh, const Eigen::MatrixXd& V0, const Eigen::MatrixXd& V1, - std::vector& candidates, - std::vector& bf_candidates); + ipc::CandidateVector& candidates, + ipc::CandidateVector& bf_candidates); diff --git a/tests/src/tests/broad_phase/brute_force_comparison.hpp b/tests/src/tests/broad_phase/brute_force_comparison.hpp index 4a6108a83..c3ddf03b6 100644 --- a/tests/src/tests/broad_phase/brute_force_comparison.hpp +++ b/tests/src/tests/broad_phase/brute_force_comparison.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -16,5 +17,5 @@ void brute_force_comparison( const ipc::CollisionMesh& mesh, const Eigen::MatrixXd& V0, const Eigen::MatrixXd& V1, - std::vector& candidates, - std::vector& bf_candidates); + ipc::CandidateVector& candidates, + ipc::CandidateVector& bf_candidates); diff --git a/tests/src/tests/broad_phase/test_broad_phase.cpp b/tests/src/tests/broad_phase/test_broad_phase.cpp index e30ea47e3..b0c22bfdf 100644 --- a/tests/src/tests/broad_phase/test_broad_phase.cpp +++ b/tests/src/tests/broad_phase/test_broad_phase.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -37,7 +38,7 @@ void test_face_face_broad_phase( } else { broad_phase->build(V0, mesh.edges(), mesh.faces(), inflation_radius); } - std::vector ff_candidates; + ipc::CandidateVector ff_candidates; broad_phase->detect_face_face_candidates(ff_candidates); BruteForce bf; @@ -47,7 +48,7 @@ void test_face_face_broad_phase( } else { bf.build(V0, mesh.edges(), mesh.faces(), inflation_radius); } - std::vector bf_ff_candidates; + ipc::CandidateVector bf_ff_candidates; bf.detect_face_face_candidates(bf_ff_candidates); CHECK(!ff_candidates.empty()); @@ -286,7 +287,7 @@ TEST_CASE("Broad phase build from boxes", "[broad_phase]") const auto broad_phase = GENERATE(tests::BroadPhaseGenerator::create()); broad_phase->build(boxes, Eigen::MatrixXi(), Eigen::MatrixXi(), /*dim=*/3); - std::vector candidates; + ipc::CandidateVector candidates; broad_phase->detect_vertex_vertex_candidates(candidates); CAPTURE(broad_phase->name()); diff --git a/tests/src/tests/broad_phase/test_gpu_lbvh.cu b/tests/src/tests/broad_phase/test_gpu_lbvh.cu index a3ba945fa..a35a0a339 100644 --- a/tests/src/tests/broad_phase/test_gpu_lbvh.cu +++ b/tests/src/tests/broad_phase/test_gpu_lbvh.cu @@ -6,6 +6,7 @@ // AABB (an order-independent union of identically-inflated boxes). The // detected candidate sets must be exactly equal. +#include #include #ifdef IPC_TOOLKIT_WITH_CUDA @@ -39,7 +40,7 @@ namespace { // so they must be exactly equal as sets. template void compare_candidates_exact( - std::vector gpu, std::vector cpu) + ipc::CandidateVector gpu, ipc::CandidateVector cpu) { std::sort(gpu.begin(), gpu.end()); std::sort(cpu.begin(), cpu.end()); @@ -126,19 +127,19 @@ TEST_CASE("GPU LBVH detect candidates", "[broad_phase][lbvh][cuda][gpu]") cpu_lbvh.build(vertices_t0, vertices_t1, edges, faces, inflation_radius); { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_vertex_vertex_candidates(gpu_c); cpu_lbvh.detect_vertex_vertex_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_vertex_candidates(gpu_c); cpu_lbvh.detect_edge_vertex_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_edge_candidates(gpu_c); cpu_lbvh.detect_edge_edge_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); @@ -156,7 +157,7 @@ TEST_CASE("GPU LBVH detect candidates", "[broad_phase][lbvh][cuda][gpu]") REQUIRE_CUDA(cudaMemcpy( b.data(), view.b, view.size * sizeof(int32_t), cudaMemcpyDeviceToHost)); - std::vector view_c; + ipc::CandidateVector view_c; for (size_t k = 0; k < view.size; ++k) { view_c.emplace_back(a[k], b[k]); } @@ -168,19 +169,19 @@ TEST_CASE("GPU LBVH detect candidates", "[broad_phase][lbvh][cuda][gpu]") CHECK(gpu_c.size() == cpu_c.size()); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_face_vertex_candidates(gpu_c); cpu_lbvh.detect_face_vertex_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_face_candidates(gpu_c); cpu_lbvh.detect_edge_face_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_face_face_candidates(gpu_c); cpu_lbvh.detect_face_face_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); @@ -216,7 +217,7 @@ TEST_CASE( cpu_lbvh.build(vertices_t0, vertices_t1, edges, faces, 0); { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_edge_candidates(gpu_c); cpu_lbvh.detect_edge_edge_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); @@ -227,13 +228,13 @@ TEST_CASE( gpu_lbvh.detect_edge_edge_candidates_device().size >= cpu_c.size()); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_face_vertex_candidates(gpu_c); cpu_lbvh.detect_face_vertex_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_face_candidates(gpu_c); cpu_lbvh.detect_edge_face_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); @@ -276,7 +277,7 @@ TEST_CASE("GPU LBVH 2D build and detect", "[broad_phase][lbvh][cuda][gpu]") // -- Detection parity (only edge-vertex is meaningful in 2D; mirrors // BroadPhase::detect_collision_candidates's dim == 2 branch). -- - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_vertex_candidates(gpu_c); cpu_lbvh.detect_edge_vertex_candidates(cpu_c); compare_candidates_exact(gpu_c, cpu_c); @@ -329,7 +330,7 @@ TEST_CASE("GPU LBVH single-primitive trees", "[broad_phase][lbvh][cuda][gpu]") tests::check_lbvh_nodes_match(nodes, cpu_lbvh.edge_nodes()); { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_face_vertex_candidates(gpu_c); cpu_lbvh.detect_face_vertex_candidates(cpu_c); // Without this the checks would pass on an empty set, which is @@ -338,14 +339,14 @@ TEST_CASE("GPU LBVH single-primitive trees", "[broad_phase][lbvh][cuda][gpu]") compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_face_candidates(gpu_c); cpu_lbvh.detect_edge_face_candidates(cpu_c); REQUIRE(!cpu_c.empty()); compare_candidates_exact(gpu_c, cpu_c); } { - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_vertex_candidates(gpu_c); cpu_lbvh.detect_edge_vertex_candidates(cpu_c); REQUIRE(!cpu_c.empty()); @@ -393,7 +394,7 @@ TEST_CASE("GPU LBVH degenerate domain", "[broad_phase][lbvh][cuda][gpu]") gpu_lbvh.face_nodes_to_host(nodes, rightmost); tests::check_lbvh_nodes_match(nodes, cpu_lbvh.face_nodes()); - std::vector gpu_c, cpu_c; + ipc::CandidateVector gpu_c, cpu_c; gpu_lbvh.detect_edge_edge_candidates(gpu_c); cpu_lbvh.detect_edge_edge_candidates(cpu_c); REQUIRE(!cpu_c.empty()); @@ -419,7 +420,7 @@ TEST_CASE("GPU LBVH move", "[broad_phase][lbvh][cuda][gpu]") CHECK(b.num_face_nodes() == num_nodes); CHECK(a.num_face_nodes() == 0); // NOLINT(bugprone-use-after-move) - std::vector candidates; + ipc::CandidateVector candidates; a.detect_face_face_candidates(candidates); // cleared, not null CHECK(candidates.empty()); @@ -493,13 +494,13 @@ TEST_CASE( // Warm up the CUDA context so the first sample is not skewed by lazy // context/allocation initialization. { - std::vector warmup; + ipc::CandidateVector warmup; gpu_lbvh.detect_edge_edge_candidates(warmup); } BENCHMARK("cuda::LBVH::detect_edge_edge_candidates") { - std::vector ee_candidates; + ipc::CandidateVector ee_candidates; gpu_lbvh.detect_edge_edge_candidates(ee_candidates); return ee_candidates.size(); }; diff --git a/tests/src/tests/broad_phase/test_lbvh.cpp b/tests/src/tests/broad_phase/test_lbvh.cpp index f1a630823..74b3b28d4 100644 --- a/tests/src/tests/broad_phase/test_lbvh.cpp +++ b/tests/src/tests/broad_phase/test_lbvh.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -80,7 +81,8 @@ namespace { /// @return false Otherwise. template bool contains_all_candidates( - std::vector actual, std::vector expected) + ipc::CandidateVector actual, + ipc::CandidateVector expected) { // 1. Sort the actual candidates to prepare for set operations tbb::parallel_sort(actual.begin(), actual.end()); @@ -160,10 +162,10 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") // detect_vertex_vertex_candidates { - std::vector vv_candidates; + ipc::CandidateVector vv_candidates; lbvh->detect_vertex_vertex_candidates(vv_candidates); - std::vector expected_vv_candidates; + ipc::CandidateVector expected_vv_candidates; spatial_hash->detect_vertex_vertex_candidates(expected_vv_candidates); CHECK(vv_candidates.size() >= expected_vv_candidates.size()); @@ -171,10 +173,10 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") } { - std::vector ev_candidates; + ipc::CandidateVector ev_candidates; lbvh->detect_edge_vertex_candidates(ev_candidates); - std::vector expected_ev_candidates; + ipc::CandidateVector expected_ev_candidates; spatial_hash->detect_edge_vertex_candidates(expected_ev_candidates); CHECK(ev_candidates.size() >= expected_ev_candidates.size()); @@ -182,10 +184,10 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") } { - std::vector ee_candidates; + ipc::CandidateVector ee_candidates; lbvh->detect_edge_edge_candidates(ee_candidates); - std::vector expected_ee_candidates; + ipc::CandidateVector expected_ee_candidates; spatial_hash->detect_edge_edge_candidates(expected_ee_candidates); CHECK(ee_candidates.size() >= expected_ee_candidates.size()); @@ -193,10 +195,10 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") } { - std::vector fv_candidates; + ipc::CandidateVector fv_candidates; lbvh->detect_face_vertex_candidates(fv_candidates); - std::vector expected_fv_candidates; + ipc::CandidateVector expected_fv_candidates; spatial_hash->detect_face_vertex_candidates(expected_fv_candidates); CHECK(fv_candidates.size() >= expected_fv_candidates.size()); @@ -204,10 +206,10 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") } { - std::vector ef_candidates; + ipc::CandidateVector ef_candidates; lbvh->detect_edge_face_candidates(ef_candidates); - std::vector expected_ef_candidates; + ipc::CandidateVector expected_ef_candidates; spatial_hash->detect_edge_face_candidates(expected_ef_candidates); CHECK(ef_candidates.size() >= expected_ef_candidates.size()); @@ -215,10 +217,10 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") } { - std::vector ff_candidates; + ipc::CandidateVector ff_candidates; lbvh->detect_face_face_candidates(ff_candidates); - std::vector expected_ff_candidates; + ipc::CandidateVector expected_ff_candidates; spatial_hash->detect_face_face_candidates(expected_ff_candidates); CHECK(ff_candidates.size() >= expected_ff_candidates.size()); @@ -270,7 +272,7 @@ TEST_CASE("LBVH single-primitive trees", "[broad_phase][lbvh]") // The LBVH rounds its AABBs outward to floats, so it may report a superset // of the exact (double-precision) brute-force set, never a subset. { - std::vector fv_candidates, expected; + ipc::CandidateVector fv_candidates, expected; lbvh.detect_face_vertex_candidates(fv_candidates); brute_force.detect_face_vertex_candidates(expected); @@ -282,7 +284,7 @@ TEST_CASE("LBVH single-primitive trees", "[broad_phase][lbvh]") } { - std::vector ef_candidates, expected; + ipc::CandidateVector ef_candidates, expected; lbvh.detect_edge_face_candidates(ef_candidates); brute_force.detect_edge_face_candidates(expected); @@ -294,14 +296,14 @@ TEST_CASE("LBVH single-primitive trees", "[broad_phase][lbvh]") // The remaining types traverse multi-node targets here, but are cheap to // check on a mesh this small. { - std::vector vv_candidates, expected; + ipc::CandidateVector vv_candidates, expected; lbvh.detect_vertex_vertex_candidates(vv_candidates); brute_force.detect_vertex_vertex_candidates(expected); CHECK(contains_all_candidates(vv_candidates, expected)); } { - std::vector ev_candidates, expected; + ipc::CandidateVector ev_candidates, expected; lbvh.detect_edge_vertex_candidates(ev_candidates); brute_force.detect_edge_vertex_candidates(expected); REQUIRE(!expected.empty()); @@ -350,14 +352,14 @@ TEST_CASE("LBVH degenerate domain", "[broad_phase][lbvh]") BruteForce brute_force; brute_force.build(vertices, edges, faces, 0); { - std::vector candidates, expected; + ipc::CandidateVector candidates, expected; lbvh.detect_edge_edge_candidates(candidates); brute_force.detect_edge_edge_candidates(expected); REQUIRE(!expected.empty()); // coplanar neighbors do overlap CHECK(contains_all_candidates(candidates, expected)); } { - std::vector candidates, expected; + ipc::CandidateVector candidates, expected; lbvh.detect_face_vertex_candidates(candidates); brute_force.detect_face_vertex_candidates(expected); REQUIRE(!expected.empty()); @@ -420,7 +422,7 @@ TEST_CASE( BENCHMARK("LBVH::detect_edge_edge_candidates") { - std::vector ee_candidates; + ipc::CandidateVector ee_candidates; lbvh->detect_edge_edge_candidates(ee_candidates); return ee_candidates.size(); }; diff --git a/tests/src/tests/broad_phase/test_stq.cpp b/tests/src/tests/broad_phase/test_stq.cpp index f893dbebd..ae3fea69d 100644 --- a/tests/src/tests/broad_phase/test_stq.cpp +++ b/tests/src/tests/broad_phase/test_stq.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -43,19 +44,19 @@ TEST_CASE("STQ All Cases", "[broad_phase][stq][cuda]") CHECK(candidates.ee_candidates.size() == 5'197'332); CHECK(candidates.fv_candidates.size() == 1'655'541); - std::vector vv_candidates; + ipc::CandidateVector vv_candidates; broad_phase->detect_vertex_vertex_candidates(vv_candidates); CHECK(vv_candidates.size() == 84'912); - std::vector ev_candidates; + ipc::CandidateVector ev_candidates; broad_phase->detect_edge_vertex_candidates(ev_candidates); CHECK(ev_candidates.size() == 1'666'926); - std::vector ef_candidates; + ipc::CandidateVector ef_candidates; broad_phase->detect_edge_face_candidates(ef_candidates); CHECK(ef_candidates.size() == 9'248'220); - std::vector ff_candidates; + ipc::CandidateVector ff_candidates; broad_phase->detect_face_face_candidates(ff_candidates); CHECK(ff_candidates.size() == 3'975'589); From 552f4293df4d0c72dcca7078b850c933fd16a7a8 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 16 Sep 2026 17:47:26 -0400 Subject: [PATCH 4/6] Copy the GPU candidates to the host in one memcpy The device wrote the candidate ids into two parallel int32 arrays, so materializing them meant staging both on the host, copying each, and then building every candidate one at a time: 12.4 ms of Cloth-Ball's 14.7 ms detection. The candidates are trivially copyable and are two index_t in a fixed order, so with 32-bit indices a device pair and a host candidate have the same layout. The device now writes LBVH::CandidatePair, one 8-byte store per candidate rather than two into separate streams, and materialize() sizes the output and copies the set straight into it. The direct copy is used only when the filter accepts all -- otherwise the pairs still have to be trimmed on the host -- and when the layouts really do match. That is checked at compile time and confirmed once at runtime against a known pair, so reordering a candidate's members falls back to building them one by one rather than silently producing garbage. GPU detect stages, Cloth-Ball, mean of 10 calls in a profiling build: | Stage | Before | After | |-----------------------|--------|-------| | traverse | 2.15 | 2.14 | | allocate_host_buffers | 3.38 | - | | download_pairs | 2.25 | - | | construct_candidates | 6.75 | - | | download_candidates | - | 5.55 | | total | 14.65 | 7.69 | Edge-edge detection, median of three runs of 20 samples: | Scene | Before | After | Speedup | |-------------------|-----------|-----------|---------| | Cloth-Funnel | 0.20 ms | 0.17 ms | 1.20x | | Armadillo-Rollers | 12.75 ms | 8.11 ms | 1.57x | | Rod-Twist | 4.20 ms | 2.03 ms | 2.07x | | Cloth-Ball | 15.22 ms | 7.69 ms | 1.98x | | N-Body-Simulation | 59.48 ms | 25.36 ms | 2.35x | | Puffer-Ball | 472.21 ms | 205.08 ms | 2.30x | BREAKING: LBVH::DeviceCandidateView is AoS. Its `a` and `b` pointers are replaced by a single `pairs` pointer to LBVH::CandidatePair. EdgeFaceCandidate and FaceFaceCandidate gain the defaulted default constructor the other candidates already have, so they can be sized without being written. Co-Authored-By: Claude Opus 5 --- src/ipc/broad_phase/cuda/lbvh.cu | 83 +++++++++++++------- src/ipc/broad_phase/cuda/lbvh.hpp | 23 ++++-- src/ipc/broad_phase/cuda/lbvh_impl.cuh | 8 +- src/ipc/candidates/edge_face.hpp | 7 ++ src/ipc/candidates/face_face.hpp | 7 ++ tests/src/tests/broad_phase/test_gpu_lbvh.cu | 10 +-- 6 files changed, 92 insertions(+), 46 deletions(-) diff --git a/src/ipc/broad_phase/cuda/lbvh.cu b/src/ipc/broad_phase/cuda/lbvh.cu index 230008325..abf7b5ee1 100644 --- a/src/ipc/broad_phase/cuda/lbvh.cu +++ b/src/ipc/broad_phase/cuda/lbvh.cu @@ -642,8 +642,7 @@ namespace { __device__ inline void emit_pair( const int32_t query_prim, const int32_t node_prim, - int32_t* __restrict__ out_a, - int32_t* __restrict__ out_b, + LBVH::CandidatePair* __restrict__ out_pairs, unsigned long long* __restrict__ counter, const unsigned long long capacity) { @@ -655,8 +654,7 @@ namespace { } const unsigned long long slot = atomicAdd(counter, 1ULL); if (slot < capacity) { - out_a[slot] = a; - out_b[slot] = b; + out_pairs[slot] = LBVH::CandidatePair { a, b }; } } @@ -678,8 +676,7 @@ namespace { /// @param target_rightmost The target's per-node rightmost-leaf indices. /// @param source_conn The source connectivity (null for vertices). /// @param target_conn The target connectivity (null for vertices). - /// @param[out] out_a The first ids of the emitted pairs. - /// @param[out] out_b The second ids of the emitted pairs. + /// @param[out] out_pairs The emitted pairs. /// @param[in,out] counter The emitted-pair counter. /// @param capacity The output arrays' capacity. template < @@ -696,8 +693,7 @@ namespace { const int32_t* __restrict__ target_rightmost, const int32_t* __restrict__ source_conn, const int32_t* __restrict__ target_conn, - int32_t* __restrict__ out_a, - int32_t* __restrict__ out_b, + LBVH::CandidatePair* __restrict__ out_pairs, unsigned long long* __restrict__ counter, const unsigned long long capacity) { @@ -721,7 +717,7 @@ namespace { leaf.primitive_id, target_conn, leaf_ids); if (!ipc::details::share_vertex(query_ids, leaf_ids)) { emit_pair( - query.primitive_id, leaf.primitive_id, out_a, out_b, + query.primitive_id, leaf.primitive_id, out_pairs, counter, capacity); } }); @@ -929,14 +925,13 @@ namespace { const int source_leaf_offset = n_source_leaves - 1; size_t capacity = std::max( - { buf.a.capacity(), size_t(1024), + { buf.pairs.capacity(), size_t(1024), size_t(8) * static_cast(n_source_leaves) }); impl.counter.resize(1); unsigned long long count = 0; for (int pass = 0; pass < 2; ++pass) { - buf.a.reserve(capacity); - buf.b.reserve(capacity); + buf.pairs.reserve(capacity); impl.counter.zero(); traverse_kernel< @@ -945,8 +940,7 @@ namespace { source.nodes.data(), n_source_leaves, source_leaf_offset, target.nodes.data(), target_size, target.rightmost_leaves.data(), T::source_conn(impl), - T::target_conn(impl), buf.a.data(), buf.b.data(), - impl.counter.data(), + T::target_conn(impl), buf.pairs.data(), impl.counter.data(), static_cast(capacity)); IPC_TOOLKIT_CUDA_CHECK(cudaGetLastError()); @@ -972,8 +966,7 @@ namespace { } buf.count = static_cast(count); - buf.a.resize(buf.count); - buf.b.resize(buf.count); + buf.pairs.resize(buf.count); return buf.count; } @@ -985,6 +978,32 @@ namespace { /// @param filter The user vertex filter. /// @param can_collide The full predicate applied when the filter is not accept-all. /// @param[out] out The materialized candidates (cleared first). + /// @brief Whether a candidate set can be copied straight from the device + /// into the host output. + /// + /// The device writes two int32 ids per candidate and the host candidate + /// types are two index_t in the same order, so when index_t is 32 bits the + /// two have identical layout and the whole set is one memcpy. The layout + /// is verified once at runtime as well, so reordering a candidate's + /// members falls back to building them one by one rather than silently + /// producing garbage. + template bool can_copy_pairs_directly() + { + if constexpr ( + std::is_trivially_copyable_v + && sizeof(Candidate) == sizeof(LBVH::CandidatePair) + && alignof(Candidate) >= alignof(LBVH::CandidatePair)) { + static const bool matches = []() { + const Candidate candidate(1, 2); + const LBVH::CandidatePair pair { 1, 2 }; + return std::memcmp(&candidate, &pair, sizeof(pair)) == 0; + }(); + return matches; + } else { + return false; + } + } + template void materialize( const LBVH::Impl::DeviceCandidates& buf, @@ -997,18 +1016,27 @@ namespace { if (count == 0) { return; } - std::vector h_a, h_b; + + // The device set is already exact when the filter accepts all, so + // with a matching layout the candidates never have to be built: size + // the output and copy the pairs into it. + if (filter.accepts_all() && can_copy_pairs_directly()) { + IPC_TOOLKIT_PROFILE_BLOCK("download_candidates"); + out.resize(count); + buf.pairs.download( + reinterpret_cast(out.data())); + return; + } + + std::vector h_pairs; { - // Staging for the pairs: two int32 arrays the size of the - // candidate set (42 MB on Cloth-Ball), allocated and zeroed. + // Staging for the pairs, the size of the candidate set. IPC_TOOLKIT_PROFILE_BLOCK("allocate_host_buffers"); - h_a.resize(count); - h_b.resize(count); + h_pairs.resize(count); } { IPC_TOOLKIT_PROFILE_BLOCK("download_pairs"); - buf.a.download(h_a.data()); - buf.b.download(h_b.data()); + buf.pairs.download(h_pairs.data()); } { @@ -1016,12 +1044,12 @@ namespace { out.reserve(count); if (filter.accepts_all()) { for (size_t k = 0; k < count; ++k) { - out.emplace_back(h_a[k], h_b[k]); + out.emplace_back(h_pairs[k].a, h_pairs[k].b); } } else { for (size_t k = 0; k < count; ++k) { - if (can_collide(h_a[k], h_b[k])) { - out.emplace_back(h_a[k], h_b[k]); + if (can_collide(h_pairs[k].a, h_pairs[k].b)) { + out.emplace_back(h_pairs[k].a, h_pairs[k].b); } } } @@ -1063,8 +1091,7 @@ namespace { const size_t count = run_traversal(impl); const LBVH::Impl::DeviceCandidates& buf = Traversal::buffer(impl); - return LBVH::DeviceCandidateView { count ? buf.a.data() : nullptr, - count ? buf.b.data() : nullptr, + return LBVH::DeviceCandidateView { count ? buf.pairs.data() : nullptr, count }; } diff --git a/src/ipc/broad_phase/cuda/lbvh.hpp b/src/ipc/broad_phase/cuda/lbvh.hpp index 528c3b3f3..fd2db5b3c 100644 --- a/src/ipc/broad_phase/cuda/lbvh.hpp +++ b/src/ipc/broad_phase/cuda/lbvh.hpp @@ -54,17 +54,26 @@ class LBVH : public ipc::BroadPhase { LBVH(const LBVH&) = delete; LBVH& operator=(const LBVH&) = delete; - /// @brief Non-owning view of device-resident candidate pairs (SoA). + /// @brief One device-resident candidate: the two primitive ids. /// - /// The pointers address device memory owned by this LBVH and stay valid + /// Laid out to match the host candidate types, which are two index_t in + /// this order, so a candidate set can be copied to the host in one + /// memcpy when index_t is 32 bits. The ids are int32_t, not index_t: see + /// the class comment. + struct CandidatePair { + int32_t a; ///< The first primitive's id. + int32_t b; ///< The second primitive's id. + }; + + /// @brief Non-owning view of device-resident candidate pairs (AoS). + /// + /// The pointer addresses device memory owned by this LBVH and stays valid /// until the next detect_*() or detect_*_device() call of the SAME /// candidate type (both variants share one buffer per type), or until - /// clear(), build(), or destruction. The ids are int32_t, not index_t: - /// see the class comment. + /// clear(), build(), or destruction. struct DeviceCandidateView { - const int32_t* a = nullptr; ///< Device pointer to the first ids. - const int32_t* b = nullptr; ///< Device pointer to the second ids. - size_t size = 0; ///< Number of candidate pairs. + const CandidatePair* pairs = nullptr; ///< Device pointer to the pairs. + size_t size = 0; ///< Number of candidate pairs. }; /// @brief Get the name of the broad phase method. diff --git a/src/ipc/broad_phase/cuda/lbvh_impl.cuh b/src/ipc/broad_phase/cuda/lbvh_impl.cuh index 4beda3c1b..53c84d06f 100644 --- a/src/ipc/broad_phase/cuda/lbvh_impl.cuh +++ b/src/ipc/broad_phase/cuda/lbvh_impl.cuh @@ -56,15 +56,13 @@ struct LBVH::Impl { /// Nothing is initialized: the kernel writes exactly the slots it fills. /// The memory is released only by the destructor. struct DeviceCandidates { - DeviceBuffer a; - DeviceBuffer b; - size_t count = 0; ///< The number of valid pairs in a/b. + DeviceBuffer pairs; + size_t count = 0; ///< The number of valid pairs. void clear() { count = 0; - a.clear(); - b.clear(); + pairs.clear(); } }; diff --git a/src/ipc/candidates/edge_face.hpp b/src/ipc/candidates/edge_face.hpp index 9ccc5d34e..d06a15330 100644 --- a/src/ipc/candidates/edge_face.hpp +++ b/src/ipc/candidates/edge_face.hpp @@ -12,6 +12,13 @@ namespace ipc { /// Not included in Candidates because it is not a collision candidate. class EdgeFaceCandidate { public: + /// @brief Construct a candidate with indeterminate IDs. + /// + /// Leaves the members uninitialized so a container can size itself + /// without writing them; keeping this trivial is what makes the type + /// trivially copyable. + EdgeFaceCandidate() = default; + EdgeFaceCandidate(index_t edge_id, index_t face_id); bool operator==(const EdgeFaceCandidate& other) const; diff --git a/src/ipc/candidates/face_face.hpp b/src/ipc/candidates/face_face.hpp index 9f6d41e4f..6a9e08567 100644 --- a/src/ipc/candidates/face_face.hpp +++ b/src/ipc/candidates/face_face.hpp @@ -12,6 +12,13 @@ namespace ipc { /// @note This may be useful for nonlinear triangles in the future. class FaceFaceCandidate { public: + /// @brief Construct a candidate with indeterminate IDs. + /// + /// Leaves the members uninitialized so a container can size itself + /// without writing them; keeping this trivial is what makes the type + /// trivially copyable. + FaceFaceCandidate() = default; + FaceFaceCandidate(index_t face0_id, index_t face1_id); bool operator==(const FaceFaceCandidate& other) const; diff --git a/tests/src/tests/broad_phase/test_gpu_lbvh.cu b/tests/src/tests/broad_phase/test_gpu_lbvh.cu index a35a0a339..fc7a23c39 100644 --- a/tests/src/tests/broad_phase/test_gpu_lbvh.cu +++ b/tests/src/tests/broad_phase/test_gpu_lbvh.cu @@ -150,16 +150,14 @@ TEST_CASE("GPU LBVH detect candidates", "[broad_phase][lbvh][cuda][gpu]") const cuda::LBVH::DeviceCandidateView view = gpu_lbvh.detect_edge_edge_candidates_device(); REQUIRE(view.size == cpu_c.size()); - std::vector a(view.size), b(view.size); + std::vector pairs(view.size); REQUIRE_CUDA(cudaMemcpy( - a.data(), view.a, view.size * sizeof(int32_t), - cudaMemcpyDeviceToHost)); - REQUIRE_CUDA(cudaMemcpy( - b.data(), view.b, view.size * sizeof(int32_t), + pairs.data(), view.pairs, + view.size * sizeof(cuda::LBVH::CandidatePair), cudaMemcpyDeviceToHost)); ipc::CandidateVector view_c; for (size_t k = 0; k < view.size; ++k) { - view_c.emplace_back(a[k], b[k]); + view_c.emplace_back(pairs[k].a, pairs[k].b); } compare_candidates_exact(view_c, cpu_c); From a89ce7fe442a2b31c087df1c796975546aebb376 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 16 Sep 2026 18:05:24 -0400 Subject: [PATCH 5/6] Rename the GPU candidate copy profiler scope download_candidates undersold what the stage is: with the AoS layout there is nothing left to construct, so the whole of materialization is one device-to-host copy. copy_candidates_to_host says that, and matches how the stage benchmark's figure buckets it. --- src/ipc/broad_phase/cuda/lbvh.cu | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ipc/broad_phase/cuda/lbvh.cu b/src/ipc/broad_phase/cuda/lbvh.cu index abf7b5ee1..2477af0df 100644 --- a/src/ipc/broad_phase/cuda/lbvh.cu +++ b/src/ipc/broad_phase/cuda/lbvh.cu @@ -1021,7 +1021,7 @@ namespace { // with a matching layout the candidates never have to be built: size // the output and copy the pairs into it. if (filter.accepts_all() && can_copy_pairs_directly()) { - IPC_TOOLKIT_PROFILE_BLOCK("download_candidates"); + IPC_TOOLKIT_PROFILE_BLOCK("copy_candidates_to_host"); out.resize(count); buf.pairs.download( reinterpret_cast(out.data())); From 2979a1a322d840b84bda621217691f50f7954119 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 2 Nov 2026 21:16:53 -0500 Subject: [PATCH 6/6] Fix MSVC overload_cast of mixin stencil overloads MSVC can't take the address of an overload set that merges the derived class's virtuals with StencilMixin's members through using-declarations, so py::overload_cast fails with C3889. Bind the mesh overloads with lambdas instead. --- python/src/candidates/collision_stencil.cpp | 44 +++++++++++---------- 1 file changed, 24 insertions(+), 20 deletions(-) diff --git a/python/src/candidates/collision_stencil.cpp b/python/src/candidates/collision_stencil.cpp index 33612e86c..b3fcf86b1 100644 --- a/python/src/candidates/collision_stencil.cpp +++ b/python/src/candidates/collision_stencil.cpp @@ -69,11 +69,12 @@ void define_collision_stencil(py::module_& m) "X"_a, "edges"_a, "faces"_a) .def( "compute_distance", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &CollisionStencil::compute_distance, py::const_), + [](const CollisionStencil& self, + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) { + return self.compute_distance(vertices, edges, faces); + }, R"ipc_Qu8mg5v7( Compute the distance of the stencil. @@ -88,11 +89,12 @@ void define_collision_stencil(py::module_& m) "vertices"_a, "edges"_a, "faces"_a) .def( "compute_distance_gradient", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &CollisionStencil::compute_distance_gradient, py::const_), + [](const CollisionStencil& self, + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) { + return self.compute_distance_gradient(vertices, edges, faces); + }, R"ipc_Qu8mg5v7( Compute the distance gradient of the stencil w.r.t. the stencil's vertex positions. @@ -107,11 +109,12 @@ void define_collision_stencil(py::module_& m) "vertices"_a, "edges"_a, "faces"_a) .def( "compute_distance_hessian", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &CollisionStencil::compute_distance_hessian, py::const_), + [](const CollisionStencil& self, + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) { + return self.compute_distance_hessian(vertices, edges, faces); + }, R"ipc_Qu8mg5v7( Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions. @@ -126,11 +129,12 @@ void define_collision_stencil(py::module_& m) "vertices"_a, "edges"_a, "faces"_a) .def( "compute_coefficients", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &CollisionStencil::compute_coefficients, py::const_), + [](const CollisionStencil& self, + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) { + return self.compute_coefficients(vertices, edges, faces); + }, R"ipc_Qu8mg5v7( Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions.