From 6de62d24a436152df736b0a051b423a2b237e18c Mon Sep 17 00:00:00 2001 From: zachferguson Date: Tue, 22 Jul 2025 15:23:57 -0400 Subject: [PATCH 1/8] Add normal computation and Jacobian methods for collision candidates - Implemented compute_normal and compute_normal_jacobian methods for various collision candidates including VertexVertex, EdgeVertex, EdgeEdge, FaceVertex, and PlaneVertex. - Added unit tests for normal computation in test_normals.cpp. - Updated CMakeLists.txt to include the new test file. --- src/ipc/candidates/collision_stencil.cpp | 52 +++++ src/ipc/candidates/collision_stencil.hpp | 65 ++++++ src/ipc/candidates/edge_edge.cpp | 38 ++++ src/ipc/candidates/edge_edge.hpp | 7 + src/ipc/candidates/edge_vertex.cpp | 53 +++++ src/ipc/candidates/edge_vertex.hpp | 7 + src/ipc/candidates/face_vertex.cpp | 35 ++++ src/ipc/candidates/face_vertex.hpp | 7 + src/ipc/candidates/vertex_vertex.cpp | 18 ++ src/ipc/candidates/vertex_vertex.hpp | 7 + src/ipc/collisions/normal/plane_vertex.cpp | 13 ++ src/ipc/collisions/normal/plane_vertex.hpp | 13 ++ tests/src/tests/candidates/CMakeLists.txt | 1 + tests/src/tests/candidates/test_normals.cpp | 219 ++++++++++++++++++++ 14 files changed, 535 insertions(+) create mode 100644 tests/src/tests/candidates/test_normals.cpp diff --git a/src/ipc/candidates/collision_stencil.cpp b/src/ipc/candidates/collision_stencil.cpp index 54cf38af1..f4b8fa440 100644 --- a/src/ipc/candidates/collision_stencil.cpp +++ b/src/ipc/candidates/collision_stencil.cpp @@ -2,6 +2,58 @@ namespace ipc { +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()); + + VectorMax3d n = compute_unnormalized_normal(positions); + + MatrixMax dn = + compute_unnormalized_normal_jacobian(positions); + +#if true + // Derivative of normalization (n̂ = n / ‖n‖) + const double n_norm = n.norm(); + n /= n_norm; // n̂ + + MatrixMax3d A = + (MatrixMax3d::Identity(dim, dim) - n * n.transpose()) / n_norm; + + dn = A * dn; +#endif + + 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, diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index 9156c76fd..36d9aebcc 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -132,6 +132,40 @@ class CollisionStencil { return compute_coefficients(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() // ---------------------------------------------------------------------- @@ -163,6 +197,24 @@ class CollisionStencil { virtual VectorMax4d compute_coefficients(Eigen::ConstRef positions) const = 0; + /// @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. @@ -189,6 +241,19 @@ class CollisionStencil { 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. + /// @return Unnormalized normal of the stencil. + virtual VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const = 0; + + /// @brief Compute the Jacobian of the unnormalized normal of the stencil. + /// @param positions Stencil's vertex positions. + /// @return Jacobian of the unnormalized normal of the stencil. + virtual MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const = 0; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/candidates/edge_edge.cpp b/src/ipc/candidates/edge_edge.cpp index 769aff395..a8f449a23 100644 --- a/src/ipc/candidates/edge_edge.cpp +++ b/src/ipc/candidates/edge_edge.cpp @@ -95,6 +95,44 @@ VectorMax4d EdgeEdgeCandidate::compute_coefficients( return coeffs; } +VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal( + Eigen::ConstRef positions) const +{ + assert(positions.size() == 12); + const Eigen::Vector3d n = + (positions.segment<3>(3) - positions.head<3>()) + .cross(positions.tail<3>() - positions.segment<3>(6)); + assert(n.norm() > 0.0); + return n; +} + +namespace { + inline Eigen::Matrix3d cross_mat(Eigen::ConstRef v) + { + Eigen::Matrix3d m; + m << 0, -v(2), v(1), // + v(2), 0, -v(0), // + -v(1), v(0), 0; + return m; + } +} // namespace + +MatrixMax +EdgeEdgeCandidate::compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const +{ + assert(positions.size() == 12); + MatrixMax dn(3, 12); + dn.leftCols<3>() = cross_mat(positions.tail<3>() - positions.segment<3>(6)); + dn.middleCols<3>(3) = + cross_mat(positions.segment<3>(6) - positions.tail<3>()); + dn.middleCols<3>(6) = + cross_mat(positions.head<3>() - positions.segment<3>(3)); + dn.rightCols<3>() = + cross_mat(positions.segment<3>(3) - positions.head<3>()); + return dn; +} + bool EdgeEdgeCandidate::ccd( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, diff --git a/src/ipc/candidates/edge_edge.hpp b/src/ipc/candidates/edge_edge.hpp index bc3ea0876..c1837f163 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -83,6 +83,13 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { index_t edge0_id; /// @brief ID of the second edge. index_t edge1_id; + +protected: + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override; + + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override; }; } // namespace ipc diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index 6fd069d2f..a908d2c4e 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -77,6 +77,59 @@ VectorMax4d EdgeVertexCandidate::compute_coefficients( return coeffs; } +VectorMax3d EdgeVertexCandidate::compute_unnormalized_normal( + Eigen::ConstRef positions) const +{ + const int dim = this->dim(positions.size()); + + if (dim == 2) { + // In 2D, the normal is simply the perpendicular vector to the edge + const Eigen::Vector2d e = positions.tail<2>() - positions.segment<2>(2); + return Eigen::Vector2d(-e.y(), e.x()); + } + + // Use triple product expansion of the cross product -e × (e × d) + // (https://en.wikipedia.org/wiki/Cross_product#Triple_product_expansion) + // NOTE: This would work in 2D as well, but we handle that case above. + assert(dim == 3); + const Eigen::Vector3d e = positions.tail<3>() - positions.segment<3>(3); + const Eigen::Vector3d d = positions.head<3>() - positions.segment<3>(3); + return d * e.dot(e) - e * e.dot(d); +} + +MatrixMax +EdgeVertexCandidate::compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const +{ + const int dim = this->dim(positions.size()); + assert(dim == 3); + + if (dim == 2) { + // In 2D, the normal is simply the perpendicular vector to the edge + MatrixMax dn(2, 6); + dn.leftCols<2>().setZero(); + dn.middleCols<2>(2) << 0, 1, -1, 0; + dn.rightCols<2>() << 0, -1, 1, 0; + return dn; + } + + assert(dim == 3); + const Eigen::Vector3d e = positions.tail<3>() - positions.segment<3>(3); + const Eigen::Vector3d d = positions.head<3>() - positions.segment<3>(3); + + const auto I = Eigen::Matrix3d::Identity(); + + MatrixMax dn(3, 9); + // ∂n/∂x0 + dn.leftCols<3>() = e.dot(e) * I - e * e.transpose(); + // ∂n/∂x2 + dn.rightCols<3>() = + -e.dot(d) * I - e * d.transpose() + (2 * d) * e.transpose(); + // ∂n/∂x1 + dn.middleCols<3>(3) = -dn.leftCols<3>() - dn.rightCols<3>(); + return dn; +} + bool EdgeVertexCandidate::ccd( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, diff --git a/src/ipc/candidates/edge_vertex.hpp b/src/ipc/candidates/edge_vertex.hpp index cb06e73e8..866d6637a 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -80,6 +80,13 @@ class EdgeVertexCandidate : virtual public CollisionStencil { index_t edge_id; /// @brief ID of the vertex index_t vertex_id; + +protected: + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override; + + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override; }; } // namespace ipc diff --git a/src/ipc/candidates/face_vertex.cpp b/src/ipc/candidates/face_vertex.cpp index a2bb2a9ce..a008f3f82 100644 --- a/src/ipc/candidates/face_vertex.cpp +++ b/src/ipc/candidates/face_vertex.cpp @@ -90,6 +90,41 @@ VectorMax4d FaceVertexCandidate::compute_coefficients( return coeffs; } +VectorMax3d FaceVertexCandidate::compute_unnormalized_normal( + Eigen::ConstRef positions) const +{ + assert(positions.size() == 12); + return (positions.segment<3>(6) - positions.segment<3>(3)) + .cross(positions.tail<3>() - positions.segment<3>(3)); +} + +namespace { + inline Eigen::Matrix3d cross_mat(const Eigen::Vector3d& v) + { + Eigen::Matrix3d m; + m << 0, -v(2), v(1), // + v(2), 0, -v(0), // + -v(1), v(0), 0; + return m; + } +} // namespace + +MatrixMax +FaceVertexCandidate::compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const +{ + assert(positions.size() == 12); + MatrixMax dn(3, 12); + dn.leftCols<3>().setZero(); + dn.middleCols<3>(3) = + cross_mat(positions.tail<3>() - positions.segment<3>(6)); + dn.middleCols<3>(6) = + cross_mat(positions.segment<3>(3) - positions.tail<3>()); + dn.rightCols<3>() = + cross_mat(positions.segment<3>(6) - positions.segment<3>(3)); + return dn; +} + bool FaceVertexCandidate::ccd( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, diff --git a/src/ipc/candidates/face_vertex.hpp b/src/ipc/candidates/face_vertex.hpp index 13c59d78b..9f9fc8bf9 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -83,6 +83,13 @@ class FaceVertexCandidate : virtual public CollisionStencil { index_t face_id; /// @brief ID of the vertex index_t vertex_id; + +protected: + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override; + + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override; }; } // namespace ipc diff --git a/src/ipc/candidates/vertex_vertex.cpp b/src/ipc/candidates/vertex_vertex.cpp index d62340374..1037cc28c 100644 --- a/src/ipc/candidates/vertex_vertex.cpp +++ b/src/ipc/candidates/vertex_vertex.cpp @@ -47,6 +47,24 @@ VectorMax4d VertexVertexCandidate::compute_coefficients( return coeffs; } +VectorMax3d VertexVertexCandidate::compute_unnormalized_normal( + Eigen::ConstRef positions) const +{ + const int dim = this->dim(positions.size()); + return positions.head(dim) - positions.tail(dim); +} + +MatrixMax +VertexVertexCandidate::compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const +{ + const int dim = this->dim(positions.size()); + MatrixMax dn(dim, positions.size()); + dn.leftCols(dim) = MatrixMax3d::Identity(dim, dim); + dn.rightCols(dim) = -MatrixMax3d::Identity(dim, dim); + return dn; +} + bool VertexVertexCandidate::ccd( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, diff --git a/src/ipc/candidates/vertex_vertex.hpp b/src/ipc/candidates/vertex_vertex.hpp index 173647129..943a87ef5 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -77,6 +77,13 @@ class VertexVertexCandidate : virtual public CollisionStencil { index_t vertex0_id; /// @brief ID of the second vertex index_t vertex1_id; + +protected: + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override; + + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override; }; } // namespace ipc diff --git a/src/ipc/collisions/normal/plane_vertex.cpp b/src/ipc/collisions/normal/plane_vertex.cpp index 45c029c1d..974b74205 100644 --- a/src/ipc/collisions/normal/plane_vertex.cpp +++ b/src/ipc/collisions/normal/plane_vertex.cpp @@ -44,6 +44,19 @@ VectorMax4d PlaneVertexNormalCollision::compute_coefficients( 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, diff --git a/src/ipc/collisions/normal/plane_vertex.hpp b/src/ipc/collisions/normal/plane_vertex.hpp index 9dede7020..34901afb2 100644 --- a/src/ipc/collisions/normal/plane_vertex.hpp +++ b/src/ipc/collisions/normal/plane_vertex.hpp @@ -74,6 +74,19 @@ class PlaneVertexNormalCollision : public NormalCollision { /// @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; }; } // namespace ipc diff --git a/tests/src/tests/candidates/CMakeLists.txt b/tests/src/tests/candidates/CMakeLists.txt index 57dc9dbc5..898015b21 100644 --- a/tests/src/tests/candidates/CMakeLists.txt +++ b/tests/src/tests/candidates/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_candidates.cpp test_coefficients.cpp + test_normals.cpp # Benchmarks diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp new file mode 100644 index 000000000..27c1724fa --- /dev/null +++ b/tests/src/tests/candidates/test_normals.cpp @@ -0,0 +1,219 @@ +#include +#include +#include + +#include +#include + +#include + +#include + +using namespace ipc; + +TEST_CASE("Vertex-vertex collision normal", "[vv][normal]") +{ + const int dim = GENERATE(2, 3); + + Eigen::MatrixXd V(2, dim); + V.setZero(); + V(0, 0) = -1; + V(1, 0) = 1; + + VectorMax3d expected_normal = VectorMax3d::Zero(dim); + expected_normal(0) = -1; + + Eigen::MatrixXi E, F; + + VertexVertexCandidate vv(0, 1); + + VectorMax3d normal = vv.compute_normal(V, E, F); + MatrixMax jacobian = vv.compute_normal_jacobian(V, E, F); + + REQUIRE(normal.size() == dim); + REQUIRE(jacobian.rows() == dim); + REQUIRE(jacobian.cols() == 2 * dim); + + CHECK(normal.norm() == Catch::Approx(1.0)); + CHECK(normal.isApprox(expected_normal)); + + // Check jacobian using finite differences + Eigen::MatrixXd fd_jacobian; + VectorMax12d x = vv.dof(V, E, F); + fd::finite_jacobian( + x, [&vv](const Eigen::MatrixXd& x) { return vv.compute_normal(x); }, + fd_jacobian); + CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); +} + +TEST_CASE("Edge-vertex collision normal", "[ev][normal]") +{ + const int dim = GENERATE(2, 3); + + Eigen::MatrixXd V = Eigen::MatrixXd::Zero(3, dim); + V(0, 0) = -1; // Vertex 0 x=-1 + V(1, 0) = 1; // Vertex 1 x=1 + V(2, 1) = 1; // Vertex 2 y=1 + + SECTION("default") { } + SECTION("flip") { V(2, 1) = -1; } + SECTION("e0") { V(2, 0) = -2; } + SECTION("e1") { V(2, 0) = 2; } + + CAPTURE(dim, V(2, 0), V(2, 1)); + + Eigen::MatrixXi E(1, 2), F; + E << 0, 1; + + EdgeVertexCandidate ev(0, 2); + + auto normal = ev.compute_normal(V, E, F); + auto jacobian = ev.compute_normal_jacobian(V, E, F); + + REQUIRE(normal.size() == dim); + REQUIRE(jacobian.rows() == dim); + REQUIRE(jacobian.cols() == 3 * dim); + + CHECK(normal.norm() == Catch::Approx(1.0)); + VectorMax3d expected_normal = VectorMax3d::Zero(dim); + expected_normal(1) = V(2, 1) < 0 ? -1 : 1; + CHECK(normal.isApprox(expected_normal)); + if (!normal.isApprox(expected_normal)) { + std::cout << "Normal: " << normal.transpose() << std::endl; + std::cout << "Expected: " << expected_normal.transpose() << std::endl; + } + + // Check jacobian using finite differences + Eigen::MatrixXd fd_jacobian; + VectorMax12d x = ev.dof(V, E, F); + fd::finite_jacobian( + x, [&ev](const Eigen::MatrixXd& x) { return ev.compute_normal(x); }, + fd_jacobian); + CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); + if (!fd::compare_jacobian(jacobian, fd_jacobian)) { + std::cout << "Jacobian:\n" << jacobian << std::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; + } +} + +TEST_CASE("Edge-edge collision normal", "[ee][normal]") +{ + Eigen::MatrixXd V(4, 3); + V << -1, 0, -0.1, /**/ 1, 0, -0.1, /**/ 0, -1, 0.1, /**/ 0, 1, 0.1; + + // clang-format off + SECTION("default") { } + SECTION("random") { V.bottomRows<2>().setRandom(); } + SECTION("quad 0") { V.block<2, 1>(2, 0).array() += 2; } + SECTION("quad 1") { V.block<2, 2>(2, 0).array() += 2; } + SECTION("quad 2") { V.block<2, 1>(2, 1).array() += 2; } + SECTION("quad 3") { V.bottomRows<2>().rowwise() += Eigen::RowVector3d(-2, 2, 0); } + SECTION("quad 4") { V.block<2, 1>(2, 0).array() -= 2; } + SECTION("quad 5") { V.block<2, 2>(2, 0).array() -= 2; } + SECTION("quad 6") { V.block<2, 1>(2, 1).array() -= 2; } + SECTION("quad 7") { V.bottomRows<2>().rowwise() += Eigen::RowVector3d(2, -2, 0); } + // clang-format on + + Eigen::MatrixXi E(2, 2), F; + E << 0, 1, /**/ 2, 3; + + EdgeEdgeCandidate ee(0, 1); + + Eigen::Vector3d normal = ee.compute_normal(V, E, F); + Eigen::MatrixXd jacobian = ee.compute_normal_jacobian(V, E, F); + + REQUIRE(jacobian.rows() == 3); + REQUIRE(jacobian.cols() == 12); + + CHECK(normal.norm() == Catch::Approx(1.0)); + + // Check jacobian using finite differences + Eigen::MatrixXd fd_jacobian; + VectorMax12d x = ee.dof(V, E, F); + fd::finite_jacobian( + x, [&ee](const Eigen::MatrixXd& x) { return ee.compute_normal(x); }, + fd_jacobian); + CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); + if (!fd::compare_jacobian(jacobian, fd_jacobian)) { + std::cout << "Jacobian:\n" << jacobian << std::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std ::endl; + } +} + +TEST_CASE("Face-vertex collision normal", "[fv][normal]") +{ + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0, /**/ 0.333, 0.333, 1; + + SECTION("default") { } + SECTION("random") { V.row(3).setRandom(); } + SECTION("off triangle 0") { V.row(3) << 2, 0.5, 1; } + SECTION("off triangle 1") { V.row(3) << 2, 2, 1; } + SECTION("off triangle 2") { V.row(3) << 0.5, 2, 1; } + SECTION("off triangle 3") { V.row(3) << -1, 2, 1; } + SECTION("off triangle 4") { V.row(3) << -1, 0.5, 1; } + SECTION("off triangle 5") { V.row(3) << -1, -1, 1; } + SECTION("off triangle 6") { V.row(3) << 0.5, -1, 1; } + SECTION("off triangle 7") { V.row(3) << 2, -0.5, 1; } + SECTION("failure case 1") { V.row(3) << 0.680375, -0.211234, 0.566198; } + SECTION("failure case 2") { V.row(3) << -0.997497, 0.127171, -0.613392; } + + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + + Eigen::MatrixXi E; + igl::edges(F, E); + + FaceVertexCandidate fv(0, 3); + + Eigen::Vector3d normal = fv.compute_normal(V, E, F); + Eigen::MatrixXd jacobian = fv.compute_normal_jacobian(V, E, F); + + REQUIRE(jacobian.rows() == 3); + REQUIRE(jacobian.cols() == 12); + + CHECK(normal.norm() == Catch::Approx(1.0)); + + // Check jacobian using finite differences + Eigen::MatrixXd fd_jacobian; + VectorMax12d x = fv.dof(V, E, F); + fd::finite_jacobian( + x, [&fv](const Eigen::MatrixXd& x) { return fv.compute_normal(x); }, + fd_jacobian); + CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); + if (!fd::compare_jacobian(jacobian, fd_jacobian)) { + std::cout << "Jacobian:\n" << jacobian << std::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std ::endl; + } +} + +TEST_CASE("Plane-vertex collision normal", "[pv][normal]") +{ + Eigen::MatrixXd V(1, 3); + V << 0, 1, 0; + + Eigen::Vector3d n(0, 1, 0), o(0, 0, 0); + + Eigen::MatrixXi E, F; + + PlaneVertexNormalCollision pv(o, n, 0); + + Eigen::Vector3d normal = pv.compute_normal(V, E, F); + Eigen::MatrixXd jacobian = pv.compute_normal_jacobian(V, E, F); + + CHECK(normal.isApprox(n)); + CHECK(jacobian.rows() == 3); + CHECK(jacobian.cols() == 3); + + // Check jacobian using finite differences + Eigen::MatrixXd fd_jacobian; + VectorMax12d x = pv.dof(V, E, F); + fd::finite_jacobian( + x, [&pv](const Eigen::MatrixXd& x) { return pv.compute_normal(x); }, + fd_jacobian); + CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); + if (!fd::compare_jacobian(jacobian, fd_jacobian)) { + std::cout << "Jacobian:\n" << jacobian << std::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std ::endl; + } +} From 23c3f642d4c66052da3a9658afd20f6b881a1415 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 22 Jul 2025 15:33:30 -0400 Subject: [PATCH 2/8] Update src/ipc/candidates/edge_vertex.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/ipc/candidates/edge_vertex.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index a908d2c4e..00b08e8b9 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -102,8 +102,6 @@ EdgeVertexCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const { const int dim = this->dim(positions.size()); - assert(dim == 3); - if (dim == 2) { // In 2D, the normal is simply the perpendicular vector to the edge MatrixMax dn(2, 6); From e04acb1e6a0e81355cb2fa44ced303678d6ef2b6 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 22 Jul 2025 15:34:02 -0400 Subject: [PATCH 3/8] Update tests/src/tests/candidates/test_normals.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- tests/src/tests/candidates/test_normals.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index 27c1724fa..7a787bada 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -183,7 +183,7 @@ TEST_CASE("Face-vertex collision normal", "[fv][normal]") CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std ::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; } } From e76cd6e2d2e353efde6ee1032aa0b9691cb45d19 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 22 Jul 2025 15:34:11 -0400 Subject: [PATCH 4/8] Update tests/src/tests/candidates/test_normals.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- tests/src/tests/candidates/test_normals.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index 7a787bada..37053dbd3 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -214,6 +214,6 @@ TEST_CASE("Plane-vertex collision normal", "[pv][normal]") CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std ::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; } } From 01f5822296528dc6d5dc37f2c44430ef7743c83c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 22 Jul 2025 15:38:12 -0400 Subject: [PATCH 5/8] Update tests/src/tests/candidates/test_normals.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- tests/src/tests/candidates/test_normals.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index 37053dbd3..cbf7b9a2c 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -136,7 +136,7 @@ TEST_CASE("Edge-edge collision normal", "[ee][normal]") CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std ::endl; + std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; } } From 72f09595e659af003c636aa68995e74d781701ec Mon Sep 17 00:00:00 2001 From: zachferguson Date: Tue, 22 Jul 2025 15:37:43 -0400 Subject: [PATCH 6/8] Refactor normal computation methods to use cross_product_matrix for clarity and consistency --- src/ipc/candidates/edge_edge.cpp | 27 +++++++-------------------- src/ipc/candidates/face_vertex.cpp | 17 +++-------------- src/ipc/utils/eigen_ext.hpp | 12 ++++++++++++ 3 files changed, 22 insertions(+), 34 deletions(-) diff --git a/src/ipc/candidates/edge_edge.cpp b/src/ipc/candidates/edge_edge.cpp index a8f449a23..d3620bc10 100644 --- a/src/ipc/candidates/edge_edge.cpp +++ b/src/ipc/candidates/edge_edge.cpp @@ -99,37 +99,24 @@ VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal( Eigen::ConstRef positions) const { assert(positions.size() == 12); - const Eigen::Vector3d n = - (positions.segment<3>(3) - positions.head<3>()) - .cross(positions.tail<3>() - positions.segment<3>(6)); - assert(n.norm() > 0.0); - return n; + return (positions.segment<3>(3) - positions.head<3>()) + .cross(positions.tail<3>() - positions.segment<3>(6)); } -namespace { - inline Eigen::Matrix3d cross_mat(Eigen::ConstRef v) - { - Eigen::Matrix3d m; - m << 0, -v(2), v(1), // - v(2), 0, -v(0), // - -v(1), v(0), 0; - return m; - } -} // namespace - MatrixMax EdgeEdgeCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const { assert(positions.size() == 12); MatrixMax dn(3, 12); - dn.leftCols<3>() = cross_mat(positions.tail<3>() - positions.segment<3>(6)); + dn.leftCols<3>() = + cross_product_matrix(positions.tail<3>() - positions.segment<3>(6)); dn.middleCols<3>(3) = - cross_mat(positions.segment<3>(6) - positions.tail<3>()); + cross_product_matrix(positions.segment<3>(6) - positions.tail<3>()); dn.middleCols<3>(6) = - cross_mat(positions.head<3>() - positions.segment<3>(3)); + cross_product_matrix(positions.head<3>() - positions.segment<3>(3)); dn.rightCols<3>() = - cross_mat(positions.segment<3>(3) - positions.head<3>()); + cross_product_matrix(positions.segment<3>(3) - positions.head<3>()); return dn; } diff --git a/src/ipc/candidates/face_vertex.cpp b/src/ipc/candidates/face_vertex.cpp index a008f3f82..ae98045dc 100644 --- a/src/ipc/candidates/face_vertex.cpp +++ b/src/ipc/candidates/face_vertex.cpp @@ -98,17 +98,6 @@ VectorMax3d FaceVertexCandidate::compute_unnormalized_normal( .cross(positions.tail<3>() - positions.segment<3>(3)); } -namespace { - inline Eigen::Matrix3d cross_mat(const Eigen::Vector3d& v) - { - Eigen::Matrix3d m; - m << 0, -v(2), v(1), // - v(2), 0, -v(0), // - -v(1), v(0), 0; - return m; - } -} // namespace - MatrixMax FaceVertexCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const @@ -117,11 +106,11 @@ FaceVertexCandidate::compute_unnormalized_normal_jacobian( MatrixMax dn(3, 12); dn.leftCols<3>().setZero(); dn.middleCols<3>(3) = - cross_mat(positions.tail<3>() - positions.segment<3>(6)); + cross_product_matrix(positions.tail<3>() - positions.segment<3>(6)); dn.middleCols<3>(6) = - cross_mat(positions.segment<3>(3) - positions.tail<3>()); + cross_product_matrix(positions.segment<3>(3) - positions.tail<3>()); dn.rightCols<3>() = - cross_mat(positions.segment<3>(6) - positions.segment<3>(3)); + cross_product_matrix(positions.segment<3>(6) - positions.segment<3>(3)); return dn; } diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 99521641c..725323caa 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -164,6 +164,18 @@ using ArrayMax4i = ArrayMax4; /**@}*/ +/// @brief Cross product matrix for 3D vectors. +/// @param v Vector to create the cross product matrix for. +/// @return The cross product matrix of the vector. +inline Eigen::Matrix3d cross_product_matrix(Eigen::ConstRef v) +{ + Eigen::Matrix3d m; + m << 0, -v(2), v(1), // + v(2), 0, -v(0), // + -v(1), v(0), 0; + return m; +} + /// @brief Matrix projection onto positive definite cone /// @param A Symmetric matrix to project /// @param eps Minimum eigenvalue threshold From d45ff205f90cf7ebd852cb1d472600d0ac55a80e Mon Sep 17 00:00:00 2001 From: zachferguson Date: Tue, 22 Jul 2025 15:47:54 -0400 Subject: [PATCH 7/8] Add concurrency settings to GitHub workflows for improved job management --- .github/workflows/continuous.yml | 4 ++++ .github/workflows/coverage.yml | 4 ++++ .github/workflows/cuda.yml | 4 ++++ .github/workflows/python.yml | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/.github/workflows/continuous.yml b/.github/workflows/continuous.yml index ec98e3e01..ce1afe6b0 100644 --- a/.github/workflows/continuous.yml +++ b/.github/workflows/continuous.yml @@ -11,6 +11,10 @@ on: - 'tests/**' - 'CMakeLists.txt' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ github.ref != 'refs/heads/main' }} + env: CTEST_OUTPUT_ON_FAILURE: ON CTEST_PARALLEL_LEVEL: 2 diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index a51ee5255..30b24d53c 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -12,6 +12,10 @@ on: - 'CMakeLists.txt' - 'codecov.yml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ github.ref != 'refs/heads/main' }} + jobs: Coverage: name: Code Coverage diff --git a/.github/workflows/cuda.yml b/.github/workflows/cuda.yml index 8b742d3dd..7d5756328 100644 --- a/.github/workflows/cuda.yml +++ b/.github/workflows/cuda.yml @@ -12,6 +12,10 @@ on: - 'python/**' - 'CMakeLists.txt' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ github.ref != 'refs/heads/main' }} + env: CTEST_OUTPUT_ON_FAILURE: ON CTEST_PARALLEL_LEVEL: 2 diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index 946e544be..e7713c989 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -15,6 +15,10 @@ on: - 'pyproject.toml' - 'setup.py' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ github.ref != 'refs/heads/main' }} + jobs: Build: name: ${{ matrix.name }} Python ${{ matrix.python-version }} From 2af11ce505a44b50b46c58e87f61c4ebd6da39bc Mon Sep 17 00:00:00 2001 From: zachferguson Date: Tue, 22 Jul 2025 15:51:56 -0400 Subject: [PATCH 8/8] Update Python version matrix to conditionally use Python 3.13 for pull requests --- .github/workflows/python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index e7713c989..84ad270ae 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -27,7 +27,7 @@ jobs: fail-fast: false matrix: os: [ubuntu-latest, macos-latest, windows-latest] - python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"] + python-version: ${{ github.event_name == 'pull_request' && fromJSON('["3.13"]') || fromJSON('["3.9", "3.10", "3.11", "3.12", "3.13"]') }} include: - os: ubuntu-latest name: Linux