diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index 3b50f2abc..fb32358e1 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -42,6 +42,7 @@ else() -Werror=nonnull -Werror=init-self -Werror=main + -Werror=extra-semi -Werror=missing-braces -Werror=sequence-point -Werror=return-type diff --git a/cmake/recipes/eigen.cmake b/cmake/recipes/eigen.cmake index 9a43f2808..884746b2a 100644 --- a/cmake/recipes/eigen.cmake +++ b/cmake/recipes/eigen.cmake @@ -26,6 +26,10 @@ target_include_directories(Eigen3_Eigen SYSTEM INTERFACE $ ) +set_target_properties(Eigen3_Eigen PROPERTIES + INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_SOURCE_DIR}" +) + if(EIGEN_DONT_VECTORIZE) target_compile_definitions(Eigen3_Eigen INTERFACE EIGEN_DONT_VECTORIZE=1) endif() diff --git a/python/src/CMakeLists.txt b/python/src/CMakeLists.txt index 04f24a120..b48bd0ffe 100644 --- a/python/src/CMakeLists.txt +++ b/python/src/CMakeLists.txt @@ -19,7 +19,6 @@ add_subdirectory(collisions) add_subdirectory(distance) add_subdirectory(friction) add_subdirectory(geometry) -add_subdirectory(implicits) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potentials) diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 0a31510ae..d20970c2e 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -40,6 +40,7 @@ PYBIND11_MODULE(ipctk, m) define_edge_vertex_candidate(m); define_face_face_candidate(m); define_face_vertex_candidate(m); + define_plane_vertex_candidate(m); define_vertex_vertex_candidate(m); // ccd @@ -72,6 +73,7 @@ PYBIND11_MODULE(ipctk, m) define_edge_edge_tangential_collision(m); define_edge_vertex_tangential_collision(m); define_face_vertex_tangential_collision(m); + define_plane_vertex_tangential_collision(m); define_vertex_vertex_tangential_collision(m); // distance @@ -97,9 +99,6 @@ PYBIND11_MODULE(ipctk, m) define_intersection(m); define_normal(m); - // implicits - define_plane_implicit(m); - // math define_interval(m); diff --git a/python/src/bindings.hpp b/python/src/bindings.hpp index bc2d389a7..fcd7775fa 100644 --- a/python/src/bindings.hpp +++ b/python/src/bindings.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/candidates/CMakeLists.txt b/python/src/candidates/CMakeLists.txt index 7c821f354..01a0aba40 100644 --- a/python/src/candidates/CMakeLists.txt +++ b/python/src/candidates/CMakeLists.txt @@ -5,6 +5,7 @@ set(SOURCES edge_vertex.cpp face_face.cpp face_vertex.cpp + plane_vertex.cpp vertex_vertex.cpp candidates.cpp ) diff --git a/python/src/candidates/bindings.hpp b/python/src/candidates/bindings.hpp index 2d7bd09f3..dac007648 100644 --- a/python/src/candidates/bindings.hpp +++ b/python/src/candidates/bindings.hpp @@ -10,4 +10,5 @@ void define_edge_face_candidate(py::module_& m); void define_edge_vertex_candidate(py::module_& m); void define_face_face_candidate(py::module_& m); void define_face_vertex_candidate(py::module_& m); +void define_plane_vertex_candidate(py::module_& m); void define_vertex_vertex_candidate(py::module_& m); \ No newline at end of file diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index 92ab19f0c..be82f62cc 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -267,5 +267,6 @@ void define_candidates(py::module_& m) .def_readwrite("vv_candidates", &Candidates::vv_candidates) .def_readwrite("ev_candidates", &Candidates::ev_candidates) .def_readwrite("ee_candidates", &Candidates::ee_candidates) - .def_readwrite("fv_candidates", &Candidates::fv_candidates); + .def_readwrite("fv_candidates", &Candidates::fv_candidates) + .def_readwrite("pv_candidates", &Candidates::pv_candidates); } diff --git a/python/src/candidates/plane_vertex.cpp b/python/src/candidates/plane_vertex.cpp new file mode 100644 index 000000000..07f6eb30f --- /dev/null +++ b/python/src/candidates/plane_vertex.cpp @@ -0,0 +1,18 @@ +#include + +#include + +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) + .def_readwrite( + "plane", &PlaneVertexCandidate::plane, "Plane of the candidate") + .def_readwrite( + "vertex_id", &PlaneVertexCandidate::vertex_id, "ID of the vertex"); +} \ No newline at end of file diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp index e8df18395..ae88b1981 100644 --- a/python/src/collision_mesh.cpp +++ b/python/src/collision_mesh.cpp @@ -135,7 +135,25 @@ void define_collision_mesh(py::module_& m) )ipc_Qu8mg5v7", "i"_a, "j"_a); - py::class_(m, "CollisionMesh") + py::class_>(m, "Hyperplane") + .def(py::init<>()) + .def(py::init()) + .def( + "normal", + [](const Eigen::Hyperplane& self) { + return self.normal(); + }) + .def( + "offset", + [](const Eigen::Hyperplane& self) { + return self.offset(); + }) + .def("origin", [](const Eigen::Hyperplane& self) { + return -self.offset() * self.normal(); + }); + + py::class_>( + m, "CollisionMesh") .def( py::init< Eigen::ConstRef, @@ -489,5 +507,16 @@ void define_collision_mesh(py::module_& m) A function that takes two vertex IDs and returns true if the vertices (and faces or edges containing the vertices) can collide. By default all primitives can collide with all other primitives. + )ipc_Qu8mg5v7") + .def_readwrite( + "planes", &CollisionMesh::planes, + R"ipc_Qu8mg5v7( + A vector of planes in the collision mesh. + + Each plane is represented as a `Hyperplane` object (wrapping `Eigen::Hyperplane`). + In Python, a `Hyperplane` can be constructed from either: + * a normal and a point on the plane: `Hyperplane(normal, point)`, or + * a normal and an offset: `Hyperplane(normal, offset)`, + where `normal` and `point` are 3D vectors and `offset` is a scalar. )ipc_Qu8mg5v7"); } diff --git a/python/src/collisions/normal/plane_vertex.cpp b/python/src/collisions/normal/plane_vertex.cpp index 07f727c32..abfe60ed0 100644 --- a/python/src/collisions/normal/plane_vertex.cpp +++ b/python/src/collisions/normal/plane_vertex.cpp @@ -6,20 +6,10 @@ using namespace ipc; void define_plane_vertex_normal_collision(py::module_& m) { - py::class_( + py::class_< + PlaneVertexNormalCollision, PlaneVertexCandidate, NormalCollision>( m, "PlaneVertexNormalCollision") .def( - py::init< - Eigen::ConstRef, Eigen::ConstRef, - const index_t>(), - "plane_origin"_a, "plane_normal"_a, "vertex_id"_a) - .def_readwrite( - "plane_origin", &PlaneVertexNormalCollision::plane_origin, - "The plane's origin.") - .def_readwrite( - "plane_normal", &PlaneVertexNormalCollision::plane_normal, - "The plane's normal.") - .def_readwrite( - "vertex_id", &PlaneVertexNormalCollision::vertex_id, - "The vertex's id."); + py::init, index_t>(), "plane"_a, + "vertex_id"_a); } diff --git a/python/src/collisions/tangential/CMakeLists.txt b/python/src/collisions/tangential/CMakeLists.txt index 652d6fe65..9c20f7ca8 100644 --- a/python/src/collisions/tangential/CMakeLists.txt +++ b/python/src/collisions/tangential/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES edge_edge.cpp edge_vertex.cpp face_vertex.cpp + plane_vertex.cpp tangential_collision.cpp tangential_collisions.cpp vertex_vertex.cpp diff --git a/python/src/collisions/tangential/bindings.hpp b/python/src/collisions/tangential/bindings.hpp index 491d4966f..1283fb75c 100644 --- a/python/src/collisions/tangential/bindings.hpp +++ b/python/src/collisions/tangential/bindings.hpp @@ -7,4 +7,5 @@ void define_tangential_collisions(py::module_& m); void define_edge_edge_tangential_collision(py::module_& m); void define_edge_vertex_tangential_collision(py::module_& m); void define_face_vertex_tangential_collision(py::module_& m); +void define_plane_vertex_tangential_collision(py::module_& m); void define_vertex_vertex_tangential_collision(py::module_& m); \ No newline at end of file diff --git a/python/src/collisions/tangential/plane_vertex.cpp b/python/src/collisions/tangential/plane_vertex.cpp new file mode 100644 index 000000000..1a6ffe363 --- /dev/null +++ b/python/src/collisions/tangential/plane_vertex.cpp @@ -0,0 +1,18 @@ +#include + +#include + +using namespace ipc; + +void define_plane_vertex_tangential_collision(py::module_& m) +{ + py::class_< + PlaneVertexTangentialCollision, PlaneVertexCandidate, + TangentialCollision>(m, "PlaneVertexTangentialCollision") + .def(py::init(), "collision"_a) + .def( + py::init< + const PlaneVertexNormalCollision&, + Eigen::ConstRef, const NormalPotential&>(), + "collision"_a, "positions"_a, "normal_potential"_a); +} \ No newline at end of file diff --git a/python/src/implicits/CMakeLists.txt b/python/src/implicits/CMakeLists.txt deleted file mode 100644 index fb4526ff5..000000000 --- a/python/src/implicits/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -set(SOURCES - plane.cpp -) - -target_sources(ipctk PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/python/src/implicits/bindings.hpp b/python/src/implicits/bindings.hpp deleted file mode 100644 index df6ab2e47..000000000 --- a/python/src/implicits/bindings.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include - -void define_plane_implicit(py::module_& m); \ No newline at end of file diff --git a/python/src/implicits/plane.cpp b/python/src/implicits/plane.cpp deleted file mode 100644 index ef3c02839..000000000 --- a/python/src/implicits/plane.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include - -#include - -using namespace ipc; - -void define_plane_implicit(py::module_& m) -{ - m.def( - "construct_point_plane_collisions", - [](Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, const double dhat, - const double dmin = 0) { - std::vector pv_collisions; - construct_point_plane_collisions( - points, plane_origins, plane_normals, dhat, pv_collisions, - dmin); - return pv_collisions; - }, - R"ipc_Qu8mg5v7( - Construct a set of point-plane distance collisions used to compute - - Note: - The given pv_collisions will be cleared. - - the barrier potential. - - Parameters: - points: Points as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - dhat: The activation distance of the barrier. - dmin: Minimum distance. - - Returns: - The constructed set of collisions. - )ipc_Qu8mg5v7", - "points"_a, "plane_origins"_a, "plane_normals"_a, "dhat"_a, - "dmin"_a = 0); - - m.def( - "construct_point_plane_collisions", - [](Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, const double dhat, - const double dmin, - const std::function& can_collide) { - std::vector pv_collisions; - construct_point_plane_collisions( - points, plane_origins, plane_normals, dhat, pv_collisions, dmin, - can_collide); - return pv_collisions; - }, - R"ipc_Qu8mg5v7( - Construct a set of point-plane distance collisions used to compute - - Note: - The given pv_collisions will be cleared. - - the barrier potential. - - Parameters: - points: Points as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - dhat: The activation distance of the barrier. - dmin: Minimum distance. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - The constructed set of collisions. - )ipc_Qu8mg5v7", - "points"_a, "plane_origins"_a, "plane_normals"_a, "dhat"_a, "dmin"_a, - "can_collide"_a); - - m.def( - "is_step_point_plane_collision_free", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals) { - return is_step_point_plane_collision_free( - points_t0, points_t1, plane_origins, plane_normals); - }, - R"ipc_Qu8mg5v7( - Determine if the step is collision free. - - Note: - Assumes the trajectory is linear. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - - Returns: - True if any collisions occur. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a); - - m.def( - "is_step_point_plane_collision_free", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) { - return is_step_point_plane_collision_free( - points_t0, points_t1, plane_origins, plane_normals, - can_collide); - }, - R"ipc_Qu8mg5v7( - Determine if the step is collision free. - - Note: - Assumes the trajectory is linear. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - True if any collisions occur. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a, - "can_collide"_a); - - m.def( - "compute_point_plane_collision_free_stepsize", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals) { - return compute_point_plane_collision_free_stepsize( - points_t0, points_t1, plane_origins, plane_normals); - }, - R"ipc_Qu8mg5v7( - Computes a maximal step size that is collision free. - - Notes: - Assumes points_t0 is intersection free. - Assumes the trajectory is linear. - A value of 1.0 if a full step and 0.0 is no step. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - A step-size $\in [0, 1]$ that is collision free. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a); - - m.def( - "compute_point_plane_collision_free_stepsize", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) { - return compute_point_plane_collision_free_stepsize( - points_t0, points_t1, plane_origins, plane_normals, - can_collide); - }, - R"ipc_Qu8mg5v7( - Computes a maximal step size that is collision free. - - Notes: - Assumes points_t0 is intersection free. - Assumes the trajectory is linear. - A value of 1.0 if a full step and 0.0 is no step. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - A step-size $\in [0, 1]$ that is collision free. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a, - "can_collide"_a); -} diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index a4dc25987..aa1ae83de 100644 --- a/src/ipc/CMakeLists.txt +++ b/src/ipc/CMakeLists.txt @@ -20,7 +20,6 @@ add_subdirectory(collisions) add_subdirectory(distance) add_subdirectory(friction) add_subdirectory(geometry) -add_subdirectory(implicits) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potentials) diff --git a/src/ipc/candidates/CMakeLists.txt b/src/ipc/candidates/CMakeLists.txt index f7efceb30..be0fc3139 100644 --- a/src/ipc/candidates/CMakeLists.txt +++ b/src/ipc/candidates/CMakeLists.txt @@ -13,8 +13,8 @@ set(SOURCES face_face.hpp face_vertex.cpp face_vertex.hpp - # plane_vertex.cpp - # plane_vertex.hpp + plane_vertex.cpp + plane_vertex.hpp vertex_vertex.cpp vertex_vertex.hpp ) diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index f12028d84..8f23e102a 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -111,6 +112,15 @@ void Candidates::build( vi = mesh.codim_vertices()[vi]; // Map back to vertices } } + + // Planes to vertices: + for (const auto& plane : mesh.planes) { + for (index_t vi = 0; vi < mesh.num_vertices(); ++vi) { + if (plane.signedDistance(vertices.row(vi)) < inflation_radius) { + pv_candidates.emplace_back(plane, vi); + } + } + } } void Candidates::build( @@ -195,6 +205,17 @@ void Candidates::build( vi = mesh.codim_vertices()[vi]; // Map back to vertices } } + + // Planes to vertices: + for (const auto& plane : mesh.planes) { + for (index_t vi = 0; vi < mesh.num_vertices(); ++vi) { + const double d_t0 = plane.signedDistance(vertices_t0.row(vi)); + const double d_t1 = plane.signedDistance(vertices_t1.row(vi)); + if (d_t0 < inflation_radius || d_t1 < inflation_radius) { + pv_candidates.emplace_back(plane, vi); + } + } + } } bool Candidates::is_step_collision_free( @@ -467,13 +488,14 @@ Eigen::VectorXd Candidates::compute_per_vertex_safe_distances( size_t Candidates::size() const { return vv_candidates.size() + ev_candidates.size() + ee_candidates.size() - + fv_candidates.size(); + + fv_candidates.size() + pv_candidates.size(); } bool Candidates::empty() const { return vv_candidates.empty() && ev_candidates.empty() - && ee_candidates.empty() && fv_candidates.empty(); + && ee_candidates.empty() && fv_candidates.empty() + && pv_candidates.empty(); } void Candidates::clear() @@ -482,6 +504,7 @@ void Candidates::clear() ev_candidates.clear(); ee_candidates.clear(); fv_candidates.clear(); + pv_candidates.clear(); } CollisionStencil& Candidates::operator[](size_t i) @@ -501,6 +524,10 @@ CollisionStencil& Candidates::operator[](size_t i) if (i < fv_candidates.size()) { return fv_candidates[i]; } + i -= fv_candidates.size(); + if (i < pv_candidates.size()) { + return pv_candidates[i]; + } throw std::out_of_range("Candidate index is out of range!"); } @@ -521,6 +548,10 @@ const CollisionStencil& Candidates::operator[](size_t i) const if (i < fv_candidates.size()) { return fv_candidates[i]; } + i -= fv_candidates.size(); + if (i < pv_candidates.size()) { + return pv_candidates[i]; + } throw std::out_of_range("Candidate index is out of range!"); } diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index c9ce8afcd..5ac2ca12e 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -215,6 +216,7 @@ class Candidates { std::vector ev_candidates; std::vector ee_candidates; std::vector fv_candidates; + std::vector pv_candidates; private: static bool default_is_active(double candidate) { return true; } diff --git a/src/ipc/candidates/edge_edge.hpp b/src/ipc/candidates/edge_edge.hpp index c1837f163..0e174fb14 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -17,7 +17,7 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 4; }; + int num_vertices() const override { return 4; } /// @brief Get the vertex IDs for the edge-edge pair /// @param edges The edge connectivity matrix diff --git a/src/ipc/candidates/edge_vertex.hpp b/src/ipc/candidates/edge_vertex.hpp index 866d6637a..b489fbe49 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -17,7 +17,7 @@ class EdgeVertexCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 3; }; + int num_vertices() const override { return 3; } /// @brief Get the vertex IDs for the edge-vertex pair /// @param edges The edge connectivity matrix diff --git a/src/ipc/candidates/face_vertex.hpp b/src/ipc/candidates/face_vertex.hpp index 9f9fc8bf9..ef8cd755f 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -17,7 +17,7 @@ class FaceVertexCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 4; }; + int num_vertices() const override { return 4; } /// @brief Get the vertex IDs for the face-vertex pair /// @param edges The edge connectivity matrix diff --git a/src/ipc/candidates/plane_vertex.cpp b/src/ipc/candidates/plane_vertex.cpp new file mode 100644 index 000000000..be4c8205e --- /dev/null +++ b/src/ipc/candidates/plane_vertex.cpp @@ -0,0 +1,95 @@ +#include "plane_vertex.hpp" + +#include +#include + +namespace ipc { + +PlaneVertexCandidate::PlaneVertexCandidate( + const Eigen::Hyperplane& _plane, const index_t _vertex_id) + : plane(_plane) + , vertex_id(_vertex_id) +{ +} + +double PlaneVertexCandidate::compute_distance( + Eigen::ConstRef point) const +{ + assert(point.size() == 3); + return point_plane_distance( + point, -plane.offset() * plane.normal(), plane.normal()); +} + +VectorMax12d PlaneVertexCandidate::compute_distance_gradient( + Eigen::ConstRef point) const +{ + assert(point.size() == 3); + return point_plane_distance_gradient( + point, -plane.offset() * plane.normal(), plane.normal()); +} + +MatrixMax12d PlaneVertexCandidate::compute_distance_hessian( + Eigen::ConstRef point) const +{ + assert(point.size() == 3); + return point_plane_distance_hessian( + point, -plane.offset() * plane.normal(), plane.normal()); +} + +VectorMax4d PlaneVertexCandidate::compute_coefficients( + Eigen::ConstRef positions) const +{ + VectorMax4d coeffs(1); + coeffs << 1.0; + return coeffs; +} + +VectorMax3d PlaneVertexCandidate::compute_unnormalized_normal( + Eigen::ConstRef positions) const +{ + return plane.normal(); +} + +MatrixMax +PlaneVertexCandidate::compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const +{ + return MatrixMax::Zero(positions.size(), positions.size()); +} + +bool PlaneVertexCandidate::ccd( + Eigen::ConstRef vertices_t0, + Eigen::ConstRef vertices_t1, + double& toi, + const double min_distance, + const double tmax, + const NarrowPhaseCCD& narrow_phase_ccd) const +{ + assert(min_distance == 0 && "Not implemented"); + assert(vertices_t0.size() == 3 && vertices_t1.size() == 3); + return point_static_plane_ccd( + vertices_t0, vertices_t1, -plane.offset() * plane.normal(), + plane.normal(), toi, narrow_phase_ccd.conservative_rescaling); +} + +bool PlaneVertexCandidate::operator==(const PlaneVertexCandidate& other) const +{ + return this->vertex_id == other.vertex_id + && this->plane.normal().isApprox(other.plane.normal()) + && this->plane.offset() == other.plane.offset(); +} + +bool PlaneVertexCandidate::operator!=(const PlaneVertexCandidate& other) const +{ + return !(*this == other); +} + +bool PlaneVertexCandidate::operator<(const PlaneVertexCandidate& other) const +{ + if (this->vertex_id == other.vertex_id) { + return this->plane.offset() < other.plane.offset(); + } + return this->vertex_id < other.vertex_id; +} + +} // namespace ipc diff --git a/src/ipc/candidates/plane_vertex.hpp b/src/ipc/candidates/plane_vertex.hpp new file mode 100644 index 000000000..349186134 --- /dev/null +++ b/src/ipc/candidates/plane_vertex.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include +#include + +namespace ipc { + +class PlaneVertexCandidate : virtual public CollisionStencil { +public: + PlaneVertexCandidate( + const Eigen::Hyperplane& plane, const index_t vertex_id); + + int num_vertices() const override { return 1; } + + std::array vertex_ids( + Eigen::ConstRef edges, + Eigen::ConstRef faces) const override + { + return { { vertex_id, -1, -1, -1 } }; + } + + using CollisionStencil::compute_coefficients; + using CollisionStencil::compute_distance; + using CollisionStencil::compute_distance_gradient; + using CollisionStencil::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; + + /// @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; + + /// @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; + + /// @brief Compute the coefficients of the stencil. + /// @param positions Vertex positions. + /// @return Coefficients of the stencil. + VectorMax4d compute_coefficients( + Eigen::ConstRef positions) const override; + + /// @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. + /// @param[out] toi Computed time of impact (normalized). + /// @param[in] min_distance Minimum separation distance between primitives. + /// @param[in] tmax Maximum time (normalized) to look for collisions. + /// @param[in] narrow_phase_ccd The narrow phase CCD algorithm to use. + /// @return If the candidate had a collision over the time interval. + 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; + + /// @brief The plane of the candidate. + Eigen::Hyperplane plane; + + /// @brief The vertex's id. + index_t vertex_id; + + /// @brief Compare two PlaneVertexCandidates for equality. + bool operator==(const PlaneVertexCandidate& other) const; + + /// @brief Compare two PlaneVertexCandidates for inequality. + bool operator!=(const PlaneVertexCandidate& other) const; + + /// @brief Compare two PlaneVertexCandidates for less than. + bool operator<(const PlaneVertexCandidate& other) const; + +protected: + /// @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; + + /// @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; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/candidates/vertex_vertex.hpp b/src/ipc/candidates/vertex_vertex.hpp index 943a87ef5..a2a440b6f 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -17,7 +17,7 @@ class VertexVertexCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 2; }; + int num_vertices() const override { return 2; } /// @brief Get the indices of the vertices /// @param edges edge matrix of mesh diff --git a/src/ipc/ccd/additive_ccd.cpp b/src/ipc/ccd/additive_ccd.cpp index 645298209..b0e536ccb 100644 --- a/src/ipc/ccd/additive_ccd.cpp +++ b/src/ipc/ccd/additive_ccd.cpp @@ -64,8 +64,8 @@ namespace { AdditiveCCD::AdditiveCCD( const long _max_iterations, const double _conservative_rescaling) : max_iterations(_max_iterations) - , conservative_rescaling(_conservative_rescaling) { + conservative_rescaling = _conservative_rescaling; } bool AdditiveCCD::additive_ccd( diff --git a/src/ipc/ccd/additive_ccd.hpp b/src/ipc/ccd/additive_ccd.hpp index 123477c16..875544c22 100644 --- a/src/ipc/ccd/additive_ccd.hpp +++ b/src/ipc/ccd/additive_ccd.hpp @@ -27,7 +27,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @brief Construct a new AdditiveCCD object. /// @param max_iterations The maximum number of iterations to use for the CCD algorithm. If set to UNLIMITTED_ITERATIONS, the algorithm will run until it converges. - /// @param conservative_rescaling The conservative rescaling of the time of impact. + /// @param conservative_rescaling The conservative rescaling value used to avoid taking steps exactly to impact. explicit AdditiveCCD( const long max_iterations = UNLIMITTED_ITERATIONS, const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); @@ -127,9 +127,6 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @brief Maximum number of iterations. long max_iterations; - /// @brief The conservative rescaling value used to avoid taking steps exactly to impact. - double conservative_rescaling; - private: /// @brief Computes the time of impact between two objects using additive continuous collision detection. /// @param x Initial positions diff --git a/src/ipc/ccd/inexact_ccd.cpp b/src/ipc/ccd/inexact_ccd.cpp index 233664836..09df70b82 100644 --- a/src/ipc/ccd/inexact_ccd.cpp +++ b/src/ipc/ccd/inexact_ccd.cpp @@ -12,9 +12,9 @@ namespace ipc { -InexactCCD::InexactCCD(const double conservative_rescaling) - : conservative_rescaling(conservative_rescaling) +InexactCCD::InexactCCD(const double _conservative_rescaling) { + conservative_rescaling = _conservative_rescaling; } bool InexactCCD::ccd_strategy( diff --git a/src/ipc/ccd/inexact_ccd.hpp b/src/ipc/ccd/inexact_ccd.hpp index 6e0f085d3..7f36964b3 100644 --- a/src/ipc/ccd/inexact_ccd.hpp +++ b/src/ipc/ccd/inexact_ccd.hpp @@ -19,7 +19,7 @@ class InexactCCD : public NarrowPhaseCCD { static constexpr double SMALL_TOI = 1e-6; /// @brief Construct a new AdditiveCCD object. - /// @param conservative_rescaling The conservative rescaling of the time of impact. + /// @param conservative_rescaling The conservative rescaling value used to avoid taking steps exactly to impact. InexactCCD( const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); @@ -115,9 +115,6 @@ class InexactCCD : public NarrowPhaseCCD { const double min_distance = 0.0, const double tmax = 1.0) const override; - /// @brief Conservative rescaling of the time of impact. - double conservative_rescaling; - private: /// @brief Computes the time of impact between two points in 3D using continuous collision detection. /// @param[in] p0_t0 The initial position of the first point. diff --git a/src/ipc/ccd/narrow_phase_ccd.hpp b/src/ipc/ccd/narrow_phase_ccd.hpp index 3911f9954..cc6d67625 100644 --- a/src/ipc/ccd/narrow_phase_ccd.hpp +++ b/src/ipc/ccd/narrow_phase_ccd.hpp @@ -102,6 +102,21 @@ class NarrowPhaseCCD { double& toi, const double min_distance = 0.0, const double tmax = 1.0) const = 0; + + /// @brief The conservative rescaling value used to avoid taking steps exactly to impact. + /// + /// This is used to creat a effective minimum distance between objects, + /// which can help prevent numerical issues when objects are very close to + /// each other. + /// + /// I.e., min_effective_distance = (1.0 - conservative_rescaling) * + /// (initial_distance - min_distance) + /// + /// If the time of impact is small (e.g. 1e-6), then the CCD will be rerun + /// without a effective minimum distance to try to get a more accurate time + /// of impact. In this case, the conservative rescaling will be used to + /// scale the time of impact to avoid taking steps exactly to impact. + double conservative_rescaling; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/ccd/point_static_plane.cpp b/src/ipc/ccd/point_static_plane.cpp index 0158fc659..9418c2b1b 100644 --- a/src/ipc/ccd/point_static_plane.cpp +++ b/src/ipc/ccd/point_static_plane.cpp @@ -6,7 +6,7 @@ namespace ipc { -inline bool is_in_01(double x) { return 0 <= x && x <= 1; }; +inline bool is_in_01(double x) { return 0 <= x && x <= 1; } bool point_static_plane_ccd( Eigen::ConstRef p_t0, diff --git a/src/ipc/ccd/tight_inclusion_ccd.cpp b/src/ipc/ccd/tight_inclusion_ccd.cpp index e7a5441a3..e13563d2e 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.cpp +++ b/src/ipc/ccd/tight_inclusion_ccd.cpp @@ -26,8 +26,8 @@ TightInclusionCCD::TightInclusionCCD( const double _conservative_rescaling) : tolerance(_tolerance) , max_iterations(_max_iterations) - , conservative_rescaling(_conservative_rescaling) { + conservative_rescaling = _conservative_rescaling; } bool TightInclusionCCD::ccd_strategy( diff --git a/src/ipc/ccd/tight_inclusion_ccd.hpp b/src/ipc/ccd/tight_inclusion_ccd.hpp index 5f9637701..08b434a0d 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.hpp +++ b/src/ipc/ccd/tight_inclusion_ccd.hpp @@ -21,7 +21,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @brief Construct a new AdditiveCCD object. /// @param tolerance The tolerance used for the CCD algorithm. /// @param max_iterations The maximum number of iterations for the CCD algorithm. - /// @param conservative_rescaling The conservative rescaling of the time of impact. + /// @param conservative_rescaling The conservative rescaling value used to avoid taking steps exactly to impact. explicit TightInclusionCCD( const double tolerance = DEFAULT_TOLERANCE, const long max_iterations = DEFAULT_MAX_ITERATIONS, @@ -125,9 +125,6 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @brief Maximum number of iterations. long max_iterations; - /// @brief Conservative rescaling of the time of impact. - double conservative_rescaling; - private: /// @brief Computes the time of impact between two points in 3D using continuous collision detection. /// @param[in] p0_t0 The initial position of the first point. diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index c267bcc8c..ae1a215a6 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -327,6 +327,11 @@ class CollisionMesh { /// primitives can collide with all other primitives. std::function can_collide = default_can_collide; + /// @brief Analytic planes in the scene that can be collided with. + /// This is useful for representing infinite planes (e.g., the ground plane) + /// or planes that are not part of the collision mesh. + std::vector> planes; + protected: // ----------------------------------------------------------------------- // Helper initialization functions diff --git a/src/ipc/collisions/normal/CMakeLists.txt b/src/ipc/collisions/normal/CMakeLists.txt index 98ea4c0fe..28266c59e 100644 --- a/src/ipc/collisions/normal/CMakeLists.txt +++ b/src/ipc/collisions/normal/CMakeLists.txt @@ -9,7 +9,6 @@ set(SOURCES normal_collisions_builder.hpp normal_collisions.cpp normal_collisions.hpp - plane_vertex.cpp plane_vertex.hpp vertex_vertex.hpp ) diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 6b5611f48..92fdc2cfe 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -146,6 +146,24 @@ void NormalCollisions::build( NormalCollisionsBuilder::merge(storage, *this); + for (const auto& [plane, vertex_id] : candidates.pv_candidates) { + const double d = plane.signedDistance(vertices.row(vertex_id)); + if (d < dhat + dmin) { + const double weight = + use_area_weighting() ? mesh.vertex_area(vertex_id) : 1; + + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives()) { + weight_gradient = use_area_weighting() + ? mesh.vertex_area_gradient(vertex_id) + : Eigen::SparseVector(vertices.size()); + } + + pv_collisions.emplace_back( + plane, vertex_id, weight, weight_gradient); + } + } + // logger().debug(to_string(mesh, vertices)); for (size_t ci = 0; ci < size(); ci++) { diff --git a/src/ipc/collisions/normal/plane_vertex.cpp b/src/ipc/collisions/normal/plane_vertex.cpp deleted file mode 100644 index 974b74205..000000000 --- a/src/ipc/collisions/normal/plane_vertex.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "plane_vertex.hpp" - -#include -#include - -namespace ipc { - -PlaneVertexNormalCollision::PlaneVertexNormalCollision( - Eigen::ConstRef _plane_origin, - Eigen::ConstRef _plane_normal, - const index_t _vertex_id) - : plane_origin(_plane_origin) - , plane_normal(_plane_normal) - , vertex_id(_vertex_id) -{ -} - -double PlaneVertexNormalCollision::compute_distance( - Eigen::ConstRef point) const -{ - assert(point.size() == plane_origin.size()); - return point_plane_distance(point, plane_origin, plane_normal); -} - -VectorMax12d PlaneVertexNormalCollision::compute_distance_gradient( - Eigen::ConstRef point) const -{ - assert(point.size() == plane_origin.size()); - return point_plane_distance_gradient(point, plane_origin, plane_normal); -} - -MatrixMax12d PlaneVertexNormalCollision::compute_distance_hessian( - Eigen::ConstRef point) const -{ - assert(point.size() == plane_origin.size()); - return point_plane_distance_hessian(point, plane_origin, plane_normal); -} - -VectorMax4d PlaneVertexNormalCollision::compute_coefficients( - Eigen::ConstRef positions) const -{ - VectorMax4d coeffs(1); - coeffs << 1.0; - return coeffs; -} - -VectorMax3d PlaneVertexNormalCollision::compute_unnormalized_normal( - Eigen::ConstRef positions) const -{ - return plane_normal; -} - -MatrixMax -PlaneVertexNormalCollision::compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const -{ - return MatrixMax::Zero(positions.size(), positions.size()); -} - -bool PlaneVertexNormalCollision::ccd( - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - double& toi, - const double min_distance, - const double tmax, - const NarrowPhaseCCD& narrow_phase_ccd) const -{ - assert(min_distance == 0 && "Not implemented"); - return point_static_plane_ccd( - vertices_t0, vertices_t1, plane_origin, plane_normal, toi); -} - -} // namespace ipc diff --git a/src/ipc/collisions/normal/plane_vertex.hpp b/src/ipc/collisions/normal/plane_vertex.hpp index 34901afb2..1caed7e2b 100644 --- a/src/ipc/collisions/normal/plane_vertex.hpp +++ b/src/ipc/collisions/normal/plane_vertex.hpp @@ -1,92 +1,30 @@ #pragma once +#include #include #include namespace ipc { -class PlaneVertexNormalCollision : public NormalCollision { +class PlaneVertexNormalCollision : public PlaneVertexCandidate, + public NormalCollision { public: - PlaneVertexNormalCollision( - Eigen::ConstRef plane_origin, - Eigen::ConstRef plane_normal, - const index_t vertex_id); - - int num_vertices() const override { return 1; } + using PlaneVertexCandidate::PlaneVertexCandidate; - std::array vertex_ids( - Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + PlaneVertexNormalCollision(const PlaneVertexCandidate& candidate) + : PlaneVertexCandidate(candidate) { - return { { vertex_id, -1, -1, -1 } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::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; - - /// @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; - - /// @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; - - /// @brief Compute the coefficients of the stencil. - /// @param positions Vertex positions. - /// @return Coefficients of the stencil. - VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; - - /// @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. - /// @param[out] toi Computed time of impact (normalized). - /// @param[in] min_distance Minimum separation distance between primitives. - /// @param[in] tmax Maximum time (normalized) to look for collisions. - /// @param[in] narrow_phase_ccd The narrow phase CCD algorithm to use. - /// @return If the candidate had a collision over the time interval. - 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; - - /// @brief The plane's origin. - VectorMax3d plane_origin; - - /// @brief The plane's normal. - VectorMax3d plane_normal; - - /// @brief The vertex's id. - index_t vertex_id; - -protected: - /// @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; - - /// @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; + PlaneVertexNormalCollision( + const Eigen::Hyperplane& _plane, + const index_t _vertex_id, + const double _weight, + const Eigen::SparseVector& _weight_gradient) + : PlaneVertexCandidate(_plane, _vertex_id) + , NormalCollision(_weight, _weight_gradient) + { + } }; } // namespace ipc diff --git a/src/ipc/collisions/tangential/CMakeLists.txt b/src/ipc/collisions/tangential/CMakeLists.txt index 3f5ac3b88..251503f3a 100644 --- a/src/ipc/collisions/tangential/CMakeLists.txt +++ b/src/ipc/collisions/tangential/CMakeLists.txt @@ -5,6 +5,8 @@ set(SOURCES edge_vertex.hpp face_vertex.cpp face_vertex.hpp + plane_vertex.cpp + plane_vertex.hpp tangential_collision.cpp tangential_collision.hpp tangential_collisions.cpp diff --git a/src/ipc/collisions/tangential/plane_vertex.cpp b/src/ipc/collisions/tangential/plane_vertex.cpp new file mode 100644 index 000000000..09f08ba91 --- /dev/null +++ b/src/ipc/collisions/tangential/plane_vertex.cpp @@ -0,0 +1,96 @@ +#include "plane_vertex.hpp" + +#include +#include +#include +#include + +namespace ipc { + +PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision) + : PlaneVertexCandidate(collision) +{ + this->weight = collision.weight; + this->weight_gradient = collision.weight_gradient; +} + +PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) + : PlaneVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_force); +} + +PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const NormalPotential& normal_potential) + : PlaneVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_potential); +} + +// ============================================================================ + +MatrixMax PlaneVertexTangentialCollision::compute_tangent_basis( + Eigen::ConstRef positions) const +{ + assert(positions.size() == ndof()); + Eigen::Vector3d p0 = -plane.offset() * plane.normal(); + Eigen::Vector3d p1 = p0 + plane.normal(); + return point_point_tangent_basis( + p0.head(positions.size()), p1.head(positions.size())); +} + +MatrixMax +PlaneVertexTangentialCollision::compute_tangent_basis_jacobian( + Eigen::ConstRef positions) const +{ + assert(positions.size() == ndof()); + return MatrixMax::Zero( + ndof() * ndof(), positions.size() == 2 ? 1 : 2); +} + +// ============================================================================ + +VectorMax2d PlaneVertexTangentialCollision::compute_closest_point( + Eigen::ConstRef positions) const +{ + return VectorMax2d(); +} + +MatrixMax +PlaneVertexTangentialCollision::compute_closest_point_jacobian( + Eigen::ConstRef positions) const +{ + return MatrixMax(); +} + +// ============================================================================ + +VectorMax3d PlaneVertexTangentialCollision::relative_velocity( + Eigen::ConstRef velocities) const +{ + assert(velocities.size() <= 3); + return velocities; +} + +MatrixMax +PlaneVertexTangentialCollision::relative_velocity_matrix( + Eigen::ConstRef _closest_point) const +{ + return MatrixMax::Identity(ndof(), ndof()); +} + +MatrixMax +PlaneVertexTangentialCollision::relative_velocity_matrix_jacobian( + Eigen::ConstRef _closest_point) const +{ + return MatrixMax::Zero( + _closest_point.size() * ndof(), ndof()); +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/tangential/plane_vertex.hpp b/src/ipc/collisions/tangential/plane_vertex.hpp new file mode 100644 index 000000000..20f2a6ef3 --- /dev/null +++ b/src/ipc/collisions/tangential/plane_vertex.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include + +namespace ipc { + +class PlaneVertexTangentialCollision : public PlaneVertexCandidate, + public TangentialCollision { +public: + using PlaneVertexCandidate::PlaneVertexCandidate; + + PlaneVertexTangentialCollision(const PlaneVertexNormalCollision& collision); + + PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + + PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const NormalPotential& normal_potential); + +protected: + MatrixMax compute_tangent_basis( + Eigen::ConstRef positions) const override; + + MatrixMax compute_tangent_basis_jacobian( + Eigen::ConstRef positions) const override; + + VectorMax2d compute_closest_point( + Eigen::ConstRef positions) const override; + + MatrixMax compute_closest_point_jacobian( + Eigen::ConstRef positions) const override; + + VectorMax3d + relative_velocity(Eigen::ConstRef velocities) const override; + + using TangentialCollision::relative_velocity_matrix; + + MatrixMax relative_velocity_matrix( + Eigen::ConstRef closest_point) const override; + + MatrixMax relative_velocity_matrix_jacobian( + Eigen::ConstRef closest_point) const override; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/tangential/tangential_collisions.cpp b/src/ipc/collisions/tangential/tangential_collisions.cpp index 0c44cdeb6..9424ab2b7 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.cpp +++ b/src/ipc/collisions/tangential/tangential_collisions.cpp @@ -32,7 +32,8 @@ void TangentialCollisions::build( const auto& C_ev = collisions.ev_collisions; const auto& C_ee = collisions.ee_collisions; const auto& C_fv = collisions.fv_collisions; - auto& [FC_vv, FC_ev, FC_ee, FC_fv] = *this; + const auto& C_pv = collisions.pv_collisions; + auto& [FC_vv, FC_ev, FC_ee, FC_fv, FC_pv] = *this; FC_vv.reserve(C_vv.size()); for (const auto& c_vv : C_vv) { @@ -107,6 +108,15 @@ void TangentialCollisions::build( + FC_fv.back().closest_point[1] * (mu_k(f2i) - mu_k(f0i)); FC_fv.back().mu_k = blend_mu(face_mu_k, mu_k(vi)); } + + FC_pv.reserve(C_pv.size()); + for (const auto& c_pv : C_pv) { + FC_pv.emplace_back( + c_pv, c_pv.dof(vertices, edges, faces), normal_potential); + const auto& [vi, _0, _1, _2] = FC_pv.back().vertex_ids(edges, faces); + FC_pv.back().mu_s = mu_s(vi); + FC_pv.back().mu_k = mu_k(vi); + } } void TangentialCollisions::build( @@ -127,7 +137,7 @@ void TangentialCollisions::build( clear(); - auto& [FC_vv, FC_ev, FC_ee, FC_fv] = *this; + auto& [FC_vv, FC_ev, FC_ee, FC_fv, FC_pv] = *this; // FC_vv.reserve(C_vv.size()); for (size_t i = 0; i < collisions.size(); i++) { @@ -301,13 +311,14 @@ void TangentialCollisions::build( size_t TangentialCollisions::size() const { return vv_collisions.size() + ev_collisions.size() + ee_collisions.size() - + fv_collisions.size(); + + fv_collisions.size() + pv_collisions.size(); } bool TangentialCollisions::empty() const { return vv_collisions.empty() && ev_collisions.empty() - && ee_collisions.empty() && fv_collisions.empty(); + && ee_collisions.empty() && fv_collisions.empty() + && pv_collisions.empty(); } void TangentialCollisions::clear() @@ -316,6 +327,7 @@ void TangentialCollisions::clear() ev_collisions.clear(); ee_collisions.clear(); fv_collisions.clear(); + pv_collisions.clear(); } TangentialCollision& TangentialCollisions::operator[](size_t i) @@ -335,6 +347,10 @@ TangentialCollision& TangentialCollisions::operator[](size_t i) if (i < fv_collisions.size()) { return fv_collisions[i]; } + i -= fv_collisions.size(); + if (i < pv_collisions.size()) { + return pv_collisions[i]; + } throw std::out_of_range("Friction collision index is out of range!"); } @@ -355,6 +371,10 @@ const TangentialCollision& TangentialCollisions::operator[](size_t i) const if (i < fv_collisions.size()) { return fv_collisions[i]; } + i -= fv_collisions.size(); + if (i < pv_collisions.size()) { + return pv_collisions[i]; + } throw std::out_of_range("Friction collision index is out of range!"); } diff --git a/src/ipc/collisions/tangential/tangential_collisions.hpp b/src/ipc/collisions/tangential/tangential_collisions.hpp index 5ba55125b..71c202aca 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.hpp +++ b/src/ipc/collisions/tangential/tangential_collisions.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -136,6 +137,8 @@ class TangentialCollisions { std::vector ee_collisions; /// @brief Face-vertex tangential collisions. std::vector fv_collisions; + /// @brief Plane-vertex tangential collisions. + std::vector pv_collisions; }; } // namespace ipc diff --git a/src/ipc/implicits/CMakeLists.txt b/src/ipc/implicits/CMakeLists.txt deleted file mode 100644 index c210df385..000000000 --- a/src/ipc/implicits/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -set(SOURCES - plane.cpp - plane.hpp -) - -target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/implicits/plane.cpp b/src/ipc/implicits/plane.cpp deleted file mode 100644 index 5dbe8451d..000000000 --- a/src/ipc/implicits/plane.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "plane.hpp" - -#include -#include - -#include -#include - -namespace ipc { - -void construct_point_plane_collisions( - Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const double dhat, - std::vector& pv_collisions, - const double dmin, - const std::function& can_collide) -{ - pv_collisions.clear(); - - double dhat_squared = dhat * dhat; - double dmin_squared = dmin * dmin; - - // Cull the candidates by measuring the distance and dropping those that are - // greater than dhat. - - size_t n_planes = plane_origins.rows(); - assert(plane_normals.rows() == n_planes); - - for (size_t vi = 0; vi < points.rows(); vi++) { - for (size_t pi = 0; pi < n_planes; pi++) { - if (!can_collide(vi, pi)) { - continue; - } - - const auto& plane_origin = plane_origins.row(pi); - const auto& plane_normal = plane_normals.row(pi); - - double distance_sqr = point_plane_distance( - points.row(vi), plane_origin, plane_normal); - - if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) { - pv_collisions.emplace_back(plane_origin, plane_normal, vi); - pv_collisions.back().dmin = dmin; - } - } - } -} - -// ============================================================================ - -bool is_step_point_plane_collision_free( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) -{ - size_t n_planes = plane_origins.rows(); - assert(plane_normals.rows() == n_planes); - assert(points_t0.rows() == points_t1.rows()); - - for (size_t vi = 0; vi < points_t0.rows(); vi++) { - for (size_t pi = 0; pi < n_planes; pi++) { - if (!can_collide(vi, pi)) { - continue; - } - - const auto& plane_origin = plane_origins.row(pi); - const auto& plane_normal = plane_normals.row(pi); - - double toi; - bool is_collision = point_static_plane_ccd( - points_t0.row(vi), points_t1.row(vi), plane_origin, - plane_normal, toi); - - if (is_collision) { - return false; - } - } - } - - return true; -} - -// ============================================================================ - -double compute_point_plane_collision_free_stepsize( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) -{ - size_t n_planes = plane_origins.rows(); - assert(plane_normals.rows() == n_planes); - assert(points_t0.rows() == points_t1.rows()); - - const double earliest_toi = tbb::parallel_reduce( - tbb::blocked_range(0, points_t0.rows()), - /*inital_step_size=*/1.0, - [&](tbb::blocked_range r, double current_toi) { - for (size_t vi = r.begin(); vi < r.end(); vi++) { - for (size_t pi = 0; pi < n_planes; pi++) { - if (!can_collide(vi, pi)) { - continue; - } - - const auto& plane_origin = plane_origins.row(pi); - const auto& plane_normal = plane_normals.row(pi); - - double toi; - bool are_colliding = point_static_plane_ccd( - points_t0.row(vi), points_t1.row(vi), plane_origin, - plane_normal, toi); - - if (are_colliding) { - current_toi = std::min(current_toi, toi); - } - } - } - return current_toi; - }, - [&](double a, double b) { return std::min(a, b); }); - - assert(earliest_toi >= 0 && earliest_toi <= 1.0); - return earliest_toi; -} - -} // namespace ipc diff --git a/src/ipc/implicits/plane.hpp b/src/ipc/implicits/plane.hpp deleted file mode 100644 index 7b3bed163..000000000 --- a/src/ipc/implicits/plane.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include - -#include - -#include - -namespace ipc { - -inline bool -default_can_point_plane_collide(size_t /*unused*/, size_t /*unused*/) -{ - return true; -} - -/// @brief Construct a set of point-plane distance collisions used to compute -/// the barrier potential. -/// -/// @note The given pv_collisions will be cleared. -/// -/// @param[in] points Points as rows of a matrix. -/// @param[in] plane_origins Plane origins as rows of a matrix. -/// @param[in] plane_normals Plane normals as rows of a matrix. -/// @param[in] dhat The activation distance of the barrier. -/// @param[out] pv_collisions The constructed set of collisions. -/// @param[in] dmin Minimum distance. -/// @param[in] can_collide A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. -void construct_point_plane_collisions( - Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const double dhat, - std::vector& pv_collisions, - const double dmin = 0, - const std::function& can_collide = - default_can_point_plane_collide); - -// ============================================================================ -// Collision detection - -/// @brief Determine if the step is collision free. -/// -/// @note Assumes the trajectory is linear. -/// -/// @param[in] points_t0 Points at start as rows of a matrix. -/// @param[in] points_t1 Points at end as rows of a matrix. -/// @param[in] plane_origins Plane origins as rows of a matrix. -/// @param[in] plane_normals Plane normals as rows of a matrix. -/// @param[in] can_collide A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. -/// @returns True if any collisions occur. -bool is_step_point_plane_collision_free( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide = - default_can_point_plane_collide); - -/// @brief Computes a maximal step size that is collision free. -/// -/// @note Assumes points_t0 is intersection free. -/// @note Assumes the trajectory is linear. -/// @note A value of 1.0 if a full step and 0.0 is no step. -/// -/// @param points_t0 Points at start as rows of a matrix. -/// @param points_t1 Points at end as rows of a matrix. -/// @param plane_origins Plane origins as rows of a matrix. -/// @param plane_normals Plane normals as rows of a matrix. -/// @param can_collide A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. -/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. -double compute_point_plane_collision_free_stepsize( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide = - default_can_point_plane_collide); - -} // namespace ipc diff --git a/src/ipc/potentials/potential.cpp b/src/ipc/potentials/potential.cpp index 2eb1b2062..c4da5d2b2 100644 --- a/src/ipc/potentials/potential.cpp +++ b/src/ipc/potentials/potential.cpp @@ -51,7 +51,7 @@ double Potential::operator()( } return partial_sum; }, - [](double a, double b) { return a + b; }); + std::plus()); } template diff --git a/src/ipc/smooth_contact/primitives/edge.cpp b/src/ipc/smooth_contact/primitives/edge.cpp index 15d50d354..24a0db29a 100644 --- a/src/ipc/smooth_contact/primitives/edge.cpp +++ b/src/ipc/smooth_contact/primitives/edge.cpp @@ -9,7 +9,7 @@ Edge::Edge( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/edge.hpp b/src/ipc/smooth_contact/primitives/edge.hpp index e0711c7ed..d1016a948 100644 --- a/src/ipc/smooth_contact/primitives/edge.hpp +++ b/src/ipc/smooth_contact/primitives/edge.hpp @@ -20,7 +20,7 @@ template class Edge : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/edge2.cpp b/src/ipc/smooth_contact/primitives/edge2.cpp index c009fdd56..fd3870e02 100644 --- a/src/ipc/smooth_contact/primitives/edge2.cpp +++ b/src/ipc/smooth_contact/primitives/edge2.cpp @@ -7,7 +7,7 @@ Edge2::Edge2( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/edge2.hpp b/src/ipc/smooth_contact/primitives/edge2.hpp index b52c13b43..d9bfe5efa 100644 --- a/src/ipc/smooth_contact/primitives/edge2.hpp +++ b/src/ipc/smooth_contact/primitives/edge2.hpp @@ -15,7 +15,7 @@ class Edge2 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/edge3.cpp b/src/ipc/smooth_contact/primitives/edge3.cpp index 67ec74c7a..4390fafab 100644 --- a/src/ipc/smooth_contact/primitives/edge3.cpp +++ b/src/ipc/smooth_contact/primitives/edge3.cpp @@ -384,7 +384,7 @@ Edge3::Edge3( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/edge3.hpp b/src/ipc/smooth_contact/primitives/edge3.hpp index 32ce6f307..eb1193480 100644 --- a/src/ipc/smooth_contact/primitives/edge3.hpp +++ b/src/ipc/smooth_contact/primitives/edge3.hpp @@ -15,7 +15,7 @@ class Edge3 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/face.cpp b/src/ipc/smooth_contact/primitives/face.cpp index 2d1254e85..9621b8f7e 100644 --- a/src/ipc/smooth_contact/primitives/face.cpp +++ b/src/ipc/smooth_contact/primitives/face.cpp @@ -24,7 +24,7 @@ Face::Face( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/face.hpp b/src/ipc/smooth_contact/primitives/face.hpp index 962457acd..db061b3f6 100644 --- a/src/ipc/smooth_contact/primitives/face.hpp +++ b/src/ipc/smooth_contact/primitives/face.hpp @@ -15,7 +15,7 @@ class Face : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/point2.cpp b/src/ipc/smooth_contact/primitives/point2.cpp index 52f415dfa..9d7e0ba79 100644 --- a/src/ipc/smooth_contact/primitives/point2.cpp +++ b/src/ipc/smooth_contact/primitives/point2.cpp @@ -107,7 +107,7 @@ Point2::Point2( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/point2.hpp b/src/ipc/smooth_contact/primitives/point2.hpp index 120468a73..a72ac2ea2 100644 --- a/src/ipc/smooth_contact/primitives/point2.hpp +++ b/src/ipc/smooth_contact/primitives/point2.hpp @@ -15,7 +15,7 @@ class Point2 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); Point2( diff --git a/src/ipc/smooth_contact/primitives/point3.cpp b/src/ipc/smooth_contact/primitives/point3.cpp index e82fd3a65..b2455acdf 100644 --- a/src/ipc/smooth_contact/primitives/point3.cpp +++ b/src/ipc/smooth_contact/primitives/point3.cpp @@ -10,7 +10,7 @@ Point3::Point3( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/point3.hpp b/src/ipc/smooth_contact/primitives/point3.hpp index 32407d7cd..c55ec437d 100644 --- a/src/ipc/smooth_contact/primitives/point3.hpp +++ b/src/ipc/smooth_contact/primitives/point3.hpp @@ -15,7 +15,7 @@ class Point3 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); Point3( diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index e81869efd..f0086228e 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -6,6 +6,8 @@ set(SOURCES local_to_global.hpp logger.cpp logger.hpp + matrix_cache.cpp + matrix_cache.hpp merge_thread_local.hpp profiler.cpp profiler.hpp @@ -16,9 +18,6 @@ set(SOURCES vertex_to_min_edge.cpp vertex_to_min_edge.hpp world_bbox_diagonal_length.hpp - autodiff_types.hpp - matrix_cache.hpp - matrix_cache.cpp ) target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 140d5a071..2dd00f79b 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -9,7 +9,7 @@ // NOTE: Avoid error about abs casting double to int. Eigen does this // internally but seemingly only if EIGEN_DONT_VECTORIZE is not defined. // TODO: We should always use std::abs to avoid this issue. -EIGEN_USING_STD(abs); // using std::abs; +EIGEN_USING_STD(abs) // using std::abs; #endif namespace Eigen { diff --git a/tests/src/tests/candidates/test_candidates.cpp b/tests/src/tests/candidates/test_candidates.cpp index 350d1f67f..aa1b5e55b 100644 --- a/tests/src/tests/candidates/test_candidates.cpp +++ b/tests/src/tests/candidates/test_candidates.cpp @@ -3,6 +3,7 @@ #include #include #include +#include using namespace ipc; @@ -13,8 +14,12 @@ TEST_CASE("Candidates", "[candidates]") candidates.ev_candidates = { { 3, 4 }, { 5, 6 } }; candidates.ee_candidates = { { 6, 7 }, { 8, 9 } }; candidates.fv_candidates = { { 10, 11 }, { 12, 13 } }; + candidates.pv_candidates.emplace_back( + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 14); + candidates.pv_candidates.emplace_back( + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 15); - CHECK(candidates.size() == 8); + CHECK(candidates.size() == 10); CHECK( dynamic_cast(candidates[0]) @@ -28,11 +33,13 @@ TEST_CASE("Candidates", "[candidates]") 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()]; @@ -46,22 +53,26 @@ TEST_CASE("Candidates", "[candidates]") const Candidates& const_candidates = candidates; CHECK( - dynamic_cast(const_candidates[0]) + const_candidates[0].as() == candidates.vv_candidates[0]); CHECK( - dynamic_cast(const_candidates[2]) + const_candidates[2].as() == candidates.ev_candidates[0]); CHECK( - dynamic_cast(const_candidates[4]) + const_candidates[4].as() == candidates.ee_candidates[0]); CHECK( - dynamic_cast(const_candidates[6]) + 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])); try { const_candidates[candidates.size()]; @@ -139,3 +150,12 @@ TEST_CASE("Face-Face Candidate", "[candidates][face-face]") CHECK(FaceFaceCandidate(0, 1) < FaceFaceCandidate(2, 0)); CHECK(!(FaceFaceCandidate(1, 1) < FaceFaceCandidate(0, 2))); } + +TEST_CASE("Plane-Vertex Candidate", "[candidates][plane-vertex]") +{ + Eigen::Hyperplane plane(Eigen::Vector3d::UnitY(), 0); + CHECK(PlaneVertexCandidate(plane, 1) == PlaneVertexCandidate(plane, 1)); + CHECK(PlaneVertexCandidate(plane, 1) != PlaneVertexCandidate(plane, 2)); + CHECK(PlaneVertexCandidate(plane, 1) < PlaneVertexCandidate(plane, 2)); + CHECK(!(PlaneVertexCandidate(plane, 2) < PlaneVertexCandidate(plane, 1))); +} \ No newline at end of file diff --git a/tests/src/tests/candidates/test_coefficients.cpp b/tests/src/tests/candidates/test_coefficients.cpp index 4e8dd9125..f401eefe5 100644 --- a/tests/src/tests/candidates/test_coefficients.cpp +++ b/tests/src/tests/candidates/test_coefficients.cpp @@ -145,7 +145,7 @@ TEST_CASE("Plane-vertex collision stencil coeffs.", "[pv][stencil][coeffs]") Eigen::MatrixXi E, F; - PlaneVertexNormalCollision pv(o, n, 0); + PlaneVertexNormalCollision pv(Eigen::Hyperplane(n, o), 0); VectorMax4d coeffs = pv.compute_coefficients(V, E, F); diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index 9fca19f64..b47b340bd 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -363,7 +363,7 @@ TEST_CASE("Plane-vertex collision normal", "[pv][normal]") Eigen::MatrixXi E, F; - PlaneVertexNormalCollision pv(o, n, 0); + PlaneVertexNormalCollision pv(Eigen::Hyperplane(n, o), 0); Eigen::Vector3d normal = pv.compute_normal(V, E, F); Eigen::MatrixXd jacobian = pv.compute_normal_jacobian(V, E, F); diff --git a/tests/src/tests/collisions/CMakeLists.txt b/tests/src/tests/collisions/CMakeLists.txt index 56b054e6f..8fa263aa4 100644 --- a/tests/src/tests/collisions/CMakeLists.txt +++ b/tests/src/tests/collisions/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES # Tests test_normal_collisions.cpp + test_plane_vertex_collisions.cpp # Benchmarks diff --git a/tests/src/tests/collisions/test_normal_collisions.cpp b/tests/src/tests/collisions/test_normal_collisions.cpp index 3e30daa81..5f62a924e 100644 --- a/tests/src/tests/collisions/test_normal_collisions.cpp +++ b/tests/src/tests/collisions/test_normal_collisions.cpp @@ -281,13 +281,13 @@ TEST_CASE("Plane-Vertex NormalCollision", "[collision][plane-vertex]") { Eigen::MatrixXi edges, faces; const Eigen::Vector3d n(0, 1, 0), o(0, 0, 0); - const PlaneVertexNormalCollision c(o, n, 0); + const PlaneVertexNormalCollision c(Eigen::Hyperplane(n, o), 0); CHECK(c.num_vertices() == 1); CHECK( c.vertex_ids(edges, faces) == std::array { { 0, -1, -1, -1 } }); - CHECK(c.plane_origin == o); - CHECK(c.plane_normal == n); + CHECK(-c.plane.offset() * c.plane.normal() == o); + CHECK(c.plane.normal() == n); CHECK(c.vertex_id == 0); CHECK(c.compute_distance(Eigen::Vector3d(0, -2, 0)) == 4.0); @@ -308,7 +308,7 @@ TEST_CASE("NormalCollisions::is_*", "[collisions]") collisions.ee_collisions.emplace_back(0, 1, 0.0); collisions.fv_collisions.emplace_back(0, 1); collisions.pv_collisions.emplace_back( - Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 1, 0), 0); + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 0); for (int i = 0; i < collisions.size(); i++) { CHECK(collisions.is_vertex_vertex(i) == (i == 0)); diff --git a/tests/src/tests/collisions/test_plane_vertex_collisions.cpp b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp new file mode 100644 index 000000000..3cd826b1d --- /dev/null +++ b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp @@ -0,0 +1,580 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace ipc; + +// ============================================================================= +// PlaneVertexCandidate tests +// ============================================================================= + +TEST_CASE("PlaneVertexCandidate", "[candidates][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Hyperplane plane(n, o); + + PlaneVertexCandidate pvc(plane, 0); + + CHECK(pvc.num_vertices() == 1); + + Eigen::MatrixXi edges, faces; + auto ids = pvc.vertex_ids(edges, faces); + CHECK(ids == std::array { { 0, -1, -1, -1 } }); + + SECTION("Distance computation") + { + Eigen::Vector3d point(1.0, 2.0, 3.0); + double dist = pvc.compute_distance(point); + CHECK(dist == Catch::Approx(4.0)); // squared distance = (2)^2 + + Eigen::VectorXd grad = pvc.compute_distance_gradient(point); + CHECK(grad.size() == 3); + CHECK(grad.isApprox(Eigen::Vector3d(0, 4, 0))); + + Eigen::MatrixXd hess = pvc.compute_distance_hessian(point); + CHECK(hess.isApprox(2 * n * n.transpose())); + } + + SECTION("Coefficients") + { + Eigen::Vector3d point(0.5, 1.0, 0.5); + VectorMax4d coeffs = pvc.compute_coefficients(point); + CHECK(coeffs.size() == 1); + CHECK(coeffs(0) == Catch::Approx(1.0)); + } + + SECTION("Normal (exercises unnormalized normal internally)") + { + Eigen::Vector3d point(0, 1, 0); + // compute_normal is public and calls compute_unnormalized_normal + VectorMax3d normal = pvc.compute_normal(point); + CHECK(normal.isApprox(n)); + } + + SECTION("CCD") + { + AdditiveCCD ccd; + ccd.conservative_rescaling = 1.0; // No rescaling for testing + + // Vertex starts above the plane and moves below + Eigen::Vector3d v_t0(0, 1, 0); + Eigen::Vector3d v_t1(0, -1, 0); + double toi; + bool hit = + pvc.ccd(v_t0, v_t1, toi, /*min_distance=*/0, /*tmax=*/1.0, ccd); + CHECK(hit); + CHECK(toi == Catch::Approx(0.5)); + + // Vertex starts and stays above the plane + Eigen::Vector3d v_t0_above(0, 2, 0); + Eigen::Vector3d v_t1_above(0, 1, 0); + bool no_hit = pvc.ccd( + v_t0_above, v_t1_above, toi, /*min_distance=*/0, /*tmax=*/1.0, ccd); + CHECK_FALSE(no_hit); + + // Vertex starts below and moves further below — already past plane + Eigen::Vector3d v_t0_below(0, -0.5, 0); + Eigen::Vector3d v_t1_below(0, -2, 0); + bool below_hit = pvc.ccd( + v_t0_below, v_t1_below, toi, /*min_distance=*/0, /*tmax=*/1.0, ccd); + CHECK_FALSE(below_hit); + } +} + +// ============================================================================= +// PlaneVertexNormalCollision constructor tests +// ============================================================================= + +TEST_CASE( + "PlaneVertexNormalCollision constructors", "[collision][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Hyperplane plane(n, o); + + SECTION("From PlaneVertexCandidate") + { + PlaneVertexCandidate candidate(plane, 5); + PlaneVertexNormalCollision collision(candidate); + CHECK(collision.vertex_id == 5); + CHECK(collision.plane.normal().isApprox(n)); + CHECK(collision.num_vertices() == 1); + } + + SECTION("From plane, vertex_id, weight, weight_gradient") + { + const double weight = 2.5; + Eigen::SparseVector weight_gradient(9); + weight_gradient.insert(3) = 1.0; + + PlaneVertexNormalCollision collision(plane, 3, weight, weight_gradient); + CHECK(collision.vertex_id == 3); + CHECK(collision.plane.normal().isApprox(n)); + CHECK(collision.weight == Catch::Approx(weight)); + CHECK(collision.weight_gradient.nonZeros() == 1); + } +} + +// ============================================================================= +// PlaneVertexTangentialCollision tests +// ============================================================================= + +TEST_CASE( + "PlaneVertexTangentialCollision", "[collision][tangential][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Hyperplane plane(n, o); + + PlaneVertexNormalCollision normal_collision( + plane, 0, 1.0, Eigen::SparseVector(9)); + + SECTION("Constructor from NormalCollision") + { + PlaneVertexTangentialCollision tc(normal_collision); + CHECK(tc.vertex_id == 0); + CHECK(tc.weight == Catch::Approx(1.0)); + } + + SECTION("Constructor with positions and normal_force") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + CHECK(tc.vertex_id == 0); + CHECK(tc.normal_force_magnitude == Catch::Approx(normal_force)); + } + + SECTION("Constructor with positions and NormalPotential") + { + Eigen::Vector3d pos(0, 0.5, 0); + BarrierPotential bp(1.0, 1.0); + PlaneVertexTangentialCollision tc(normal_collision, pos, bp); + CHECK(tc.vertex_id == 0); + } + + SECTION("Tangent basis") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + // Call through base class reference (public in TangentialCollision) + TangentialCollision& tc_base = tc; + auto basis = tc_base.compute_tangent_basis(pos); + // For a Y-normal plane, tangent basis should be in XZ plane + CHECK(basis.rows() == 3); + CHECK(basis.cols() == 2); + // The tangent basis vectors should be orthogonal to the normal + CHECK(std::abs(n.dot(basis.col(0))) < 1e-10); + CHECK(std::abs(n.dot(basis.col(1))) < 1e-10); + } + + SECTION("Tangent basis jacobian") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto jac = tc_base.compute_tangent_basis_jacobian(pos); + CHECK(jac.isZero()); + } + + SECTION("Closest point") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto cp = tc_base.compute_closest_point(pos); + CHECK(cp.size() == 0); + } + + SECTION("Closest point jacobian") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto jac = tc_base.compute_closest_point_jacobian(pos); + CHECK(jac.rows() == 0); + } + + SECTION("Relative velocity") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + Eigen::Vector3d velocity(1.0, 2.0, 3.0); + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto rv = tc_base.relative_velocity(velocity); + CHECK(rv.isApprox(velocity)); + } + + SECTION("Relative velocity matrix") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + VectorMax2d cp; // empty closest point for pv + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto mat = tc_base.relative_velocity_matrix(cp); + CHECK(mat.isApprox(MatrixMax::Identity(3, 3))); + } + + SECTION("Relative velocity matrix jacobian") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + VectorMax2d cp; // empty closest point for pv + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto jac = tc_base.relative_velocity_matrix_jacobian(cp); + CHECK(jac.isZero()); + } +} + +// ============================================================================= +// Integration: Candidates::build with planes +// ============================================================================= + +TEST_CASE( + "Candidates build with ground plane", "[candidates][plane-vertex][build]") +{ + // 4 vertices: two above plane, two below + Eigen::MatrixXd vertices(4, 3); + vertices << 0, 0.5, 0, // above, close + 1, 2.0, 0, // above, far + -1, 0.1, 0, // above, very close + 0, -0.5, 0; // below + + CollisionMesh mesh(vertices); + mesh.planes.emplace_back(Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero()); + + const double inflation_radius = 1.0; + + SECTION("Static build") + { + Candidates candidates; + candidates.build(mesh, vertices, inflation_radius); + + // Vertices 0 (d=0.5), 2 (d=0.1), 3 (d=-0.5) are within + // inflation_radius of the plane + CHECK(candidates.pv_candidates.size() >= 2); + CHECK(!candidates.empty()); + + // Verify pv_candidates are included in size() + size_t expected_size = candidates.vv_candidates.size() + + candidates.ev_candidates.size() + candidates.ee_candidates.size() + + candidates.fv_candidates.size() + candidates.pv_candidates.size(); + CHECK(candidates.size() == expected_size); + + // Verify operator[] can reach pv_candidates + if (!candidates.pv_candidates.empty()) { + size_t pv_start = candidates.vv_candidates.size() + + 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]); + } + } + + SECTION("CCD build") + { + // Move vertices downward + Eigen::MatrixXd vertices_t1 = vertices; + vertices_t1.col(1).array() -= 1.0; + + Candidates candidates; + candidates.build(mesh, vertices, vertices_t1, inflation_radius); + + // All vertices moving toward or past the plane should be candidates + CHECK(candidates.pv_candidates.size() >= 2); + } + + SECTION("Clear resets pv_candidates") + { + Candidates candidates; + candidates.build(mesh, vertices, inflation_radius); + CHECK(!candidates.pv_candidates.empty()); + candidates.clear(); + CHECK(candidates.pv_candidates.empty()); + CHECK(candidates.empty()); + } +} + +// ============================================================================= +// Integration: NormalCollisions::build with planes +// ============================================================================= + +TEST_CASE( + "NormalCollisions build with ground plane", + "[collisions][normal][plane-vertex][build]") +{ + // Vertex sitting just above the ground plane + Eigen::MatrixXd vertices(2, 3); + vertices << 0, 0.05, 0, // close to ground + 5, 5, 5; // far away + + CollisionMesh mesh(vertices); + mesh.init_area_jacobians(); + mesh.planes.emplace_back(Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero()); + + const double dhat = 0.2; + + SECTION("Basic build") + { + NormalCollisions collisions; + collisions.build(mesh, vertices, dhat); + + // Vertex 0 is at distance 0.05 from the plane (< dhat) + CHECK(!collisions.pv_collisions.empty()); + CHECK(collisions.pv_collisions.size() >= 1); + CHECK(!collisions.empty()); + CHECK(collisions.size() >= 1); + + // Verify is_plane_vertex + size_t pv_idx = collisions.vv_collisions.size() + + collisions.ev_collisions.size() + collisions.ee_collisions.size() + + collisions.fv_collisions.size(); + CHECK(collisions.is_plane_vertex(pv_idx)); + CHECK(!collisions.is_vertex_vertex(pv_idx)); + CHECK(!collisions.is_edge_vertex(pv_idx)); + CHECK(!collisions.is_edge_edge(pv_idx)); + CHECK(!collisions.is_face_vertex(pv_idx)); + + // Verify operator[] reaches pv_collisions + auto& collision_ref = collisions[pv_idx]; + CHECK( + &collision_ref + == static_cast(&collisions.pv_collisions[0])); + + const NormalCollisions& const_collisions = collisions; + auto& const_ref = const_collisions[pv_idx]; + CHECK( + &const_ref + == static_cast( + &collisions.pv_collisions[0])); + } + + SECTION("With area weighting") + { + NormalCollisions collisions; + collisions.set_use_area_weighting(true); + collisions.build(mesh, vertices, dhat); + + if (!collisions.pv_collisions.empty()) { + CHECK(collisions.pv_collisions[0].weight > 0); + } + } + + SECTION("With shape derivatives") + { + NormalCollisions collisions; + collisions.set_use_area_weighting(true); + collisions.set_enable_shape_derivatives(true); + collisions.build(mesh, vertices, dhat); + + if (!collisions.pv_collisions.empty()) { + CHECK(collisions.pv_collisions[0].weight > 0); + // weight_gradient should be set + } + } + + SECTION("Clear resets pv_collisions") + { + NormalCollisions collisions; + collisions.build(mesh, vertices, dhat); + CHECK(!collisions.pv_collisions.empty()); + collisions.clear(); + CHECK(collisions.pv_collisions.empty()); + CHECK(collisions.empty()); + } + + SECTION("Barrier potential with plane-vertex") + { + NormalCollisions collisions; + collisions.build(mesh, vertices, dhat); + REQUIRE(!collisions.pv_collisions.empty()); + + BarrierPotential bp(dhat, 1.0); + double val = bp(collisions, mesh, vertices); + CHECK(val > 0.0); + } +} + +// ============================================================================= +// Integration: TangentialCollisions::build with planes +// ============================================================================= + +TEST_CASE( + "TangentialCollisions build with ground plane", + "[collisions][tangential][plane-vertex][build]") +{ + Eigen::MatrixXd vertices(2, 3); + vertices << 0, 0.05, 0, // close to ground + 5, 5, 5; // far away + + CollisionMesh mesh(vertices); + mesh.init_area_jacobians(); + mesh.planes.emplace_back(Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero()); + + const double dhat = 0.2; + const double mu = 0.5; + + NormalCollisions normal_collisions; + normal_collisions.build(mesh, vertices, dhat); + REQUIRE(!normal_collisions.pv_collisions.empty()); + + BarrierPotential bp(dhat, 1.0); + + SECTION("Basic build") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + + CHECK(!tangential_collisions.pv_collisions.empty()); + CHECK(tangential_collisions.pv_collisions.size() >= 1); + CHECK(!tangential_collisions.empty()); + CHECK(tangential_collisions.size() >= 1); + + // Verify friction coefficient is set + CHECK(tangential_collisions.pv_collisions[0].mu_s == Catch::Approx(mu)); + CHECK(tangential_collisions.pv_collisions[0].mu_k == Catch::Approx(mu)); + } + + SECTION("Per-vertex friction") + { + Eigen::VectorXd mu_s = Eigen::VectorXd::Constant(vertices.rows(), 0.3); + Eigen::VectorXd mu_k = Eigen::VectorXd::Constant(vertices.rows(), 0.2); + mu_s(0) = 0.8; // vertex 0 has different friction + mu_k(0) = 0.6; + + TangentialCollisions tangential_collisions; + tangential_collisions.build( + mesh, vertices, normal_collisions, bp, mu_s, mu_k); + + CHECK(!tangential_collisions.pv_collisions.empty()); + // For pv collision, mu_s/mu_k should come from just the vertex + CHECK( + tangential_collisions.pv_collisions[0].mu_s == Catch::Approx(0.8)); + CHECK( + tangential_collisions.pv_collisions[0].mu_k == Catch::Approx(0.6)); + } + + SECTION("operator[] reaches pv_collisions") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + + size_t pv_idx = tangential_collisions.vv_collisions.size() + + tangential_collisions.ev_collisions.size() + + tangential_collisions.ee_collisions.size() + + tangential_collisions.fv_collisions.size(); + + CHECK( + &tangential_collisions[pv_idx] + == static_cast( + &tangential_collisions.pv_collisions[0])); + + const TangentialCollisions& const_tc = tangential_collisions; + CHECK( + &const_tc[pv_idx] + == static_cast( + &tangential_collisions.pv_collisions[0])); + } + + SECTION("Clear resets pv_collisions") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + CHECK(!tangential_collisions.pv_collisions.empty()); + tangential_collisions.clear(); + CHECK(tangential_collisions.pv_collisions.empty()); + CHECK(tangential_collisions.empty()); + } + + SECTION("Friction potential with plane-vertex") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + REQUIRE(!tangential_collisions.pv_collisions.empty()); + + Eigen::MatrixXd V1 = vertices; + V1(0, 0) += 0.01; // Small lateral displacement + Eigen::MatrixXd U = V1 - vertices; + + FrictionPotential fp(1e-3); + double val = fp(tangential_collisions, mesh, U); + CHECK(val >= 0.0); + } +} + +// ============================================================================= +// Out-of-range tests for operator[] +// ============================================================================= + +TEST_CASE( + "NormalCollisions operator[] out of range with pv", + "[collisions][normal][plane-vertex]") +{ + NormalCollisions collisions; + collisions.pv_collisions.emplace_back( + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 0); + + CHECK(collisions.size() == 1); + CHECK( + &collisions[0] + == static_cast(&collisions.pv_collisions[0])); + + try { + collisions[1]; + FAIL("Should have thrown an exception"); + } catch (const std::out_of_range& e) { + SUCCEED("Exception thrown"); + } +} + +TEST_CASE( + "TangentialCollisions operator[] out of range with pv", + "[collisions][tangential][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Hyperplane plane(n, Eigen::Vector3d::Zero()); + + PlaneVertexNormalCollision nc( + plane, 0, 1.0, Eigen::SparseVector(9)); + TangentialCollisions tangential_collisions; + tangential_collisions.pv_collisions.emplace_back(nc); + + CHECK(tangential_collisions.size() == 1); + CHECK(!tangential_collisions.empty()); + + CHECK( + &tangential_collisions[0] + == static_cast( + &tangential_collisions.pv_collisions[0])); + + try { + tangential_collisions[1]; + FAIL("Should have thrown an exception"); + } catch (const std::out_of_range& e) { + SUCCEED("Exception thrown"); + } +} diff --git a/tests/src/tests/distance/test_edge_edge.cpp b/tests/src/tests/distance/test_edge_edge.cpp index 9c2d5c7a6..9b75d41e1 100644 --- a/tests/src/tests/distance/test_edge_edge.cpp +++ b/tests/src/tests/distance/test_edge_edge.cpp @@ -22,7 +22,7 @@ double edge_edge_distance_stacked(const Eigen::VectorXd& x) assert(x.size() == 12); return edge_edge_distance( x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); -}; +} } // namespace TEST_CASE("Edge-edge distance", "[distance][edge-edge]") diff --git a/tests/src/tests/distance/test_point_plane.cpp b/tests/src/tests/distance/test_point_plane.cpp index 6bf1691c8..29959df40 100644 --- a/tests/src/tests/distance/test_point_plane.cpp +++ b/tests/src/tests/distance/test_point_plane.cpp @@ -15,7 +15,7 @@ double point_plane_distance_stacked(const Eigen::VectorXd& x) assert(x.size() == 12); return point_plane_distance( x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); -}; +} } // namespace TEST_CASE(