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 872665c2e..d902a294b 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -51,10 +52,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( @@ -154,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; }; @@ -182,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; }; @@ -210,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; }; @@ -238,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/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. 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/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..2477af0df 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 @@ -640,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) { @@ -653,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 }; } } @@ -676,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 < @@ -694,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) { @@ -719,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); } }); @@ -927,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< @@ -943,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()); @@ -970,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; } @@ -983,30 +978,65 @@ 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, const CollisionFilter& filter, const CanCollide& can_collide, - std::vector& out) + CandidateVector& out) { out.clear(); const size_t count = buf.count; 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("copy_candidates_to_host"); + 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()); } { @@ -1014,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); } } } @@ -1032,7 +1062,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 @@ -1061,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 }; } @@ -1218,7 +1247,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 +1256,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 +1265,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 +1274,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 +1283,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 +1292,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..fd2db5b3c 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 @@ -53,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. @@ -114,17 +124,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/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/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/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/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 b9da17d8e..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 @@ -248,13 +249,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 +285,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 +327,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 +397,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 +520,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 @@ -619,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); @@ -648,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 @@ -657,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 @@ -666,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); @@ -691,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 839c3f7c5..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 @@ -12,6 +13,7 @@ #include #include #include +#include #include #include @@ -67,15 +69,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. @@ -200,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; @@ -210,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; @@ -220,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; @@ -230,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; @@ -278,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/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..4ad12e13c 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -1,6 +1,8 @@ #pragma once -#include +#include +#include +#include #include #include @@ -10,16 +12,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 +35,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 +73,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 +102,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_face.hpp b/src/ipc/candidates/edge_face.hpp index f1c2deb4d..d06a15330 100644 --- a/src/ipc/candidates/edge_face.hpp +++ b/src/ipc/candidates/edge_face.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include @@ -11,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/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..74c147c42 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -1,6 +1,8 @@ #pragma once -#include +#include +#include +#include #include #include @@ -10,16 +12,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 +35,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 +70,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 +96,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_face.hpp b/src/ipc/candidates/face_face.hpp index 4263dfd8d..6a9e08567 100644 --- a/src/ipc/candidates/face_face.hpp +++ b/src/ipc/candidates/face_face.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include @@ -11,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/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..cb7b5a8d9 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -1,6 +1,8 @@ #pragma once -#include +#include +#include +#include #include #include @@ -10,16 +12,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 +35,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 +75,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 +103,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..c26d915e2 100644 --- a/src/ipc/candidates/plane_vertex.hpp +++ b/src/ipc/candidates/plane_vertex.hpp @@ -1,51 +1,60 @@ #pragma once +#include +#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 +71,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 +92,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..70ec2b53d 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -1,6 +1,8 @@ #pragma once -#include +#include +#include +#include #include #include @@ -10,16 +12,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 +35,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 +65,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 +88,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/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.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/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/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/src/ipc/utils/merge_thread_local.hpp b/src/ipc/utils/merge_thread_local.hpp index 3b5cd7225..8916f00f5 100644 --- a/src/ipc/utils/merge_thread_local.hpp +++ b/src/ipc/utils/merge_thread_local.hpp @@ -9,19 +9,26 @@ #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 +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"); @@ -38,34 +45,68 @@ 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); + } + } else { + std::vector*> blocks; + blocks.reserve(vectors.size()); + for (auto& v : vectors) { + if (!v.empty()) { + blocks.push_back(&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); + + 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 // `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); @@ -78,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..fc7a23c39 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); @@ -149,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), + pairs.data(), view.pairs, + view.size * sizeof(cuda::LBVH::CandidatePair), cudaMemcpyDeviceToHost)); - 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]); + view_c.emplace_back(pairs[k].a, pairs[k].b); } compare_candidates_exact(view_c, cpu_c); @@ -168,19 +167,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 +215,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 +226,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 +275,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 +328,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 +337,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 +392,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 +418,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 +492,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); 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); }