diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp index e0941e2ff..13908f89b 100644 --- a/python/src/collision_mesh.cpp +++ b/python/src/collision_mesh.cpp @@ -282,6 +282,43 @@ void define_collision_mesh(py::module_& m) .def_property_readonly( "faces_to_edges", &CollisionMesh::faces_to_edges, "Get the mapping from faces to edges of the collision mesh (#F × 3).") + .def( + "face_normals", + [](const CollisionMesh& self, + Eigen::ConstRef vertices) -> Eigen::MatrixXd { + // face_normals() only asserts this, which is a no-op in a + // release build, so check it here to avoid UB from Python. + if (vertices.cols() != 3) { + throw py::value_error( + "face_normals() is only implemented for 3D meshes, but " + "got vertices.shape = [" + + std::to_string(vertices.rows()) + ", " + + std::to_string(vertices.cols()) + "]"); + } + + const std::vector normals = + self.face_normals(vertices); + // Return an (#F × 3) array rather than a list of vectors, to + // match the rest of the per-element accessors. + Eigen::MatrixXd N(normals.size(), 3); + for (size_t f = 0; f < normals.size(); ++f) { + N.row(f) = normals[f]; + } + return N; + }, + R"ipc_Qu8mg5v7( + Compute the unit normal of each face for the given vertex positions. + + Note: + 3D only (requires triangular faces). + + Parameters: + vertices: The vertex positions of the collision mesh (#V × 3). + + Returns: + The per-face unit normals (#F × 3). + )ipc_Qu8mg5v7", + "vertices"_a) .def( "vertices", &CollisionMesh::vertices, R"ipc_Qu8mg5v7( diff --git a/python/src/geometry/intersection.cpp b/python/src/geometry/intersection.cpp index 946568f81..e0b202833 100644 --- a/python/src/geometry/intersection.cpp +++ b/python/src/geometry/intersection.cpp @@ -12,6 +12,47 @@ void define_intersection(py::module_& m) "is_edge_intersecting_triangle", &is_edge_intersecting_triangle, "e0"_a, "e1"_a, "t0"_a, "t1"_a, "t2"_a); + m.def( + "edge_triangle_intersection", + [](Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) { + double u, v, t; + const bool hit = + edge_triangle_intersection(e0, e1, t0, t1, t2, u, v, t); + return std::make_tuple(hit, u, v, t); + }, + R"ipc_Qu8mg5v7( + Edge-triangle intersection test that also returns the hit location. + + Uses the same robust orient3d plane-side gate as + is_edge_intersecting_triangle, then solves for the barycentric (u, v) on + the triangle and the parameter t along the edge. Boundary hits count as + intersections. + + Note: + The coordinates are only meaningful when intersects is True, and are + NaN otherwise. The one exception is a degenerate configuration (edge + coplanar with the triangle, or a degenerate triangle), where this + conservatively reports True but the coordinates are not uniquely + defined and are left as NaN. + + Parameters: + e0: Edge start point. + e1: Edge end point. + t0: Triangle vertex 0. + t1: Triangle vertex 1. + t2: Triangle vertex 2. + + Returns: + Tuple of (intersects, u, v, t) where (u, v) are the triangle + barycentric coordinates (along t1-t0, t2-t0) and t is the edge + parameter in [0, 1]. + )ipc_Qu8mg5v7", + "e0"_a, "e1"_a, "t0"_a, "t1"_a, "t2"_a); + m.def( "segment_segment_intersect", [](Eigen::ConstRef A, diff --git a/python/tests/test_collision_mesh.py b/python/tests/test_collision_mesh.py index 781833f1b..1279f4cfc 100644 --- a/python/tests/test_collision_mesh.py +++ b/python/tests/test_collision_mesh.py @@ -126,3 +126,35 @@ def patches_can_collide(i, j): for i in range(V.shape[0]): for j in range(V.shape[0]): assert mesh.can_collide(i, j) == can_collide(i, j) + + +def test_face_normals(): + # Two triangles of a unit square in the z = 0 plane. + V = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0]], dtype=float) + F = np.array([[0, 1, 2], [0, 2, 3]], dtype=int) + E = np.array([[0, 1], [1, 2], [2, 0], [2, 3], [3, 0]], dtype=int) + + mesh = CollisionMesh(V, E, F) + + N = mesh.face_normals(V) + assert N.shape == (mesh.num_faces, 3) + assert np.allclose(N, np.array([[0, 0, 1], [0, 0, 1]], dtype=float)) + assert np.allclose(np.linalg.norm(N, axis=1), 1.0) + + # Rotate 90° about the x-axis: +z normal ⇒ −y normal. + V_rot = np.column_stack((V[:, 0], -V[:, 2], V[:, 1])) + N_rot = mesh.face_normals(V_rot) + assert np.allclose(N_rot, np.array([[0, -1, 0], [0, -1, 0]], dtype=float)) + + +def test_face_normals_2d_raises(): + V = np.array([[0, 0], [1, 0], [0, 1], [1, 1]], dtype=float) + E = np.array([[0, 1], [1, 3], [3, 2], [2, 0]], dtype=int) + + mesh = CollisionMesh(V, E) + + try: + mesh.face_normals(V) + assert False + except ValueError: + pass diff --git a/python/tests/test_intersections.py b/python/tests/test_intersections.py index 8069a9b60..beb1d237f 100644 --- a/python/tests/test_intersections.py +++ b/python/tests/test_intersections.py @@ -6,3 +6,44 @@ def test_segment_segment_intersect(): assert ipctk.segment_segment_intersect( np.array([-1, 0]), np.array([1, 0]), np.array([0, -1]), np.array([0, 1])) + +def test_edge_triangle_intersection(): + t0 = np.array([-1.0, -1.0, 0.0]) + t1 = np.array([1.0, -1.0, 0.0]) + t2 = np.array([0.0, 1.0, 0.0]) + + # Vertical edge through the interior. + e0 = np.array([0.1, -0.3, -2.0]) + e1 = np.array([0.1, -0.3, 1.0]) + + intersects, u, v, t = ipctk.edge_triangle_intersection(e0, e1, t0, t1, t2) + assert intersects + assert u >= 0 and v >= 0 and u + v <= 1 + 1e-12 + assert 0 <= t <= 1 + + # The two parameterizations of the hit point must agree. + p_tri = t0 + u * (t1 - t0) + v * (t2 - t0) + p_edge = e0 + t * (e1 - e0) + assert np.linalg.norm(p_tri - p_edge) < 1e-12 + + # Consistent with the boolean-only predicate. + assert intersects == ipctk.is_edge_intersecting_triangle( + e0, e1, t0, t1, t2) + + +def test_edge_triangle_intersection_miss(): + t0 = np.array([-1.0, -1.0, 0.0]) + t1 = np.array([1.0, -1.0, 0.0]) + t2 = np.array([0.0, 1.0, 0.0]) + + # Edge entirely on one side of the triangle's plane. + intersects, u, v, t = ipctk.edge_triangle_intersection( + np.array([0.0, 0.0, 0.5]), np.array([0.0, 0.0, 1.5]), t0, t1, t2) + assert not intersects + assert np.isnan([u, v, t]).all() + + # Edge crosses the plane, but outside the triangle. + intersects, u, v, t = ipctk.edge_triangle_intersection( + np.array([5.0, 5.0, -1.0]), np.array([5.0, 5.0, 1.0]), t0, t1, t2) + assert not intersects + assert np.isnan([u, v, t]).all() diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 23c1a56c6..3105b1de2 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -1,6 +1,7 @@ #include "collision_mesh.hpp" #include +#include #include #include #include @@ -557,4 +558,17 @@ double CollisionMesh::max_edge_length() const } return val; } + +std::vector +CollisionMesh::face_normals(Eigen::ConstRef vertices) const +{ + assert(vertices.cols() == 3); // Only implemented for 3D meshes + std::vector normals(num_faces()); + tbb::parallel_for(size_t(0), num_faces(), [&](size_t f) { + normals[f] = triangle_normal( + vertices.row(m_faces(f, 0)), vertices.row(m_faces(f, 1)), + vertices.row(m_faces(f, 2))); + }); + return normals; +} } // namespace ipc diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index d335c7de5..e867edf2b 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -131,6 +131,13 @@ class CollisionMesh { /// @brief Compute the maximum rest length of all edges. double max_edge_length() const; + /// @brief Compute the unit normal of each face for the given vertex positions. + /// @note 3D only (requires triangular faces). + /// @param vertices The vertex positions of the collision mesh (|V| × 3). + /// @return The per-face unit normals (size |F|). + std::vector + face_normals(Eigen::ConstRef vertices) const; + /// @brief Get the mapping from vertices to edges of the collision mesh. const std::vector>& vertices_to_edges() const { diff --git a/src/ipc/geometry/intersection.cpp b/src/ipc/geometry/intersection.cpp index eaa2b21ec..4183aed0b 100644 --- a/src/ipc/geometry/intersection.cpp +++ b/src/ipc/geometry/intersection.cpp @@ -6,6 +6,8 @@ #include #include +#include + #ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION #include #endif @@ -14,12 +16,15 @@ namespace ipc { #ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION namespace { - bool is_edge_intersecting_triangle_rational( + bool edge_intersecting_triangle_rational( Eigen::ConstRef e0_float, Eigen::ConstRef e1_float, Eigen::ConstRef t0_float, Eigen::ConstRef t1_float, - Eigen::ConstRef t2_float) + Eigen::ConstRef t2_float, + double& _u, + double& _v, + double& _t) { using namespace rational; @@ -57,6 +62,10 @@ namespace { + e1[2] * t0[1] * t1[0] - e1[2] * t0[1] * t2[0] - e1[2] * t1[0] * t2[1] + e1[2] * t1[1] * t2[0]; if (d.sign() == 0) { + // Degenerate: the edge is coplanar with the triangle (or the + // triangle is degenerate). Conservatively report an intersection, + // leaving the coordinates as the caller's NaN seed since they are + // not uniquely defined here. return true; } @@ -93,6 +102,7 @@ namespace { - e1[1] * t0[0] * t2[2] + e1[1] * t0[2] * t2[0] + e1[2] * t0[0] * t2[1] - e1[2] * t0[1] * t2[0]) / d; + // v is the second barycentric coordinate for the triangle const Rational v = (e0[0] * e1[1] * t0[2] - e0[0] * e1[1] * t1[2] - e0[0] * e1[2] * t0[1] + e0[0] * e1[2] * t1[1] @@ -108,7 +118,18 @@ namespace { - e1[2] * t0[0] * t1[1] + e1[2] * t0[1] * t1[0]) / d; - return u >= 0 && u <= 1 && v >= 0 && v <= 1 && u + v <= 1; + const bool intersects = + u >= 0 && u <= 1 && v >= 0 && v <= 1 && u + v <= 1; + + // Only write the outputs once the intersection is confirmed, so they + // are never partially populated on a negative result. + if (intersects) { + _u = double(u); + _v = double(v); + _t = double(t); + } + + return intersects; } } // namespace #endif @@ -120,26 +141,61 @@ bool is_edge_intersecting_triangle( Eigen::ConstRef t1, Eigen::ConstRef t2) { + double u, v, t; + return edge_triangle_intersection(e0, e1, t0, t1, t2, u, v, t); +} + +bool edge_triangle_intersection( + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + double& u, + double& v, + double& t) +{ + // Seed the outputs so every return path leaves them deterministic. Paths + // that bail out early (or that cannot define the coordinates uniquely, as + // in the degenerate rational case below) leave them as NaN rather than + // indeterminate. + u = v = t = std::numeric_limits::quiet_NaN(); + + // Robust plane-side gate (same as is_edge_intersecting_triangle): both edge + // endpoints strictly on one side of the triangle's plane ⇒ no crossing. igl::predicates::exactinit(); const auto ori1 = igl::predicates::orient3d(t0, t1, t2, e0); const auto ori2 = igl::predicates::orient3d(t0, t1, t2, e1); if (ori1 != igl::predicates::Orientation::COPLANAR && ori2 != igl::predicates::Orientation::COPLANAR && ori1 == ori2) { - // edge is completly on one side of the plane that triangle is in + // edge is completely on one side of the plane that triangle is in return false; } #ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION - return is_edge_intersecting_triangle_rational(e0, e1, t0, t1, t2); + return edge_intersecting_triangle_rational(e0, e1, t0, t1, t2, u, v, t); #else + // Solve e0 − t0 = u·(t1−t0) + v·(t2−t0) + t·(e0−e1) for (u, v, t). Eigen::Matrix3d M; M.col(0) = t1 - t0; M.col(1) = t2 - t0; M.col(2) = e0 - e1; - Eigen::Vector3d uvt = M.fullPivLu().solve(e0 - t0); - return uvt[0] >= 0.0 && uvt[1] >= 0.0 && uvt[0] + uvt[1] <= 1.0 - && uvt[2] >= 0.0 && uvt[2] <= 1.0; + const Eigen::Vector3d uvt = M.fullPivLu().solve(e0 - t0); + + const bool intersects = uvt[0] >= 0.0 && uvt[1] >= 0.0 + && uvt[0] + uvt[1] <= 1.0 && uvt[2] >= 0.0 && uvt[2] <= 1.0; + + // Only write the outputs once the intersection is confirmed, so they are + // never partially populated on a negative result (matching the rational + // implementation above). + if (intersects) { + u = uvt[0]; + v = uvt[1]; + t = uvt[2]; + } + + return intersects; #endif } diff --git a/src/ipc/geometry/intersection.hpp b/src/ipc/geometry/intersection.hpp index 02b6b0818..4927a2922 100644 --- a/src/ipc/geometry/intersection.hpp +++ b/src/ipc/geometry/intersection.hpp @@ -18,4 +18,33 @@ bool is_edge_intersecting_triangle( Eigen::ConstRef t1, Eigen::ConstRef t2); +/// @brief Edge–triangle intersection test that also returns the hit location. +/// +/// Uses the same robust orient3d plane-side gate as +/// `is_edge_intersecting_triangle`, then solves for the barycentric (u, v) on +/// the triangle and the parameter t along the edge. Boundary hits count as +/// intersections (the comparisons are inclusive), matching +/// `is_edge_intersecting_triangle`. +/// +/// @note The out-parameters are only meaningful when this returns true, and +/// are set to NaN otherwise. The one exception is a degenerate configuration +/// (edge coplanar with the triangle, or a degenerate triangle), where this +/// conservatively returns true but the coordinates are not uniquely defined +/// and are left as NaN. Callers that need the hit point must check for NaN. +/// +/// @param[in] e0,e1 Edge endpoints. +/// @param[in] t0,t1,t2 Triangle vertices. +/// @param[out] u,v Triangle barycentric coordinates (along t1−t0, t2−t0). +/// @param[out] t Edge parameter in [0, 1]. +/// @return True if the edge intersects the triangle (including its boundary). +bool edge_triangle_intersection( + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + double& u, + double& v, + double& t); + } // namespace ipc diff --git a/tests/src/tests/geometry/CMakeLists.txt b/tests/src/tests/geometry/CMakeLists.txt index 1c61142dd..77c567134 100644 --- a/tests/src/tests/geometry/CMakeLists.txt +++ b/tests/src/tests/geometry/CMakeLists.txt @@ -1,5 +1,6 @@ set(SOURCES test_angle.cpp + test_intersection.cpp ) target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) diff --git a/tests/src/tests/geometry/test_intersection.cpp b/tests/src/tests/geometry/test_intersection.cpp new file mode 100644 index 000000000..11ba829fc --- /dev/null +++ b/tests/src/tests/geometry/test_intersection.cpp @@ -0,0 +1,72 @@ +#include +#include + +#include + +#include + +using namespace ipc; + +TEST_CASE("edge_triangle_intersection barycentric", "[intersections]") +{ + const Eigen::Vector3d t0(-1, -1, 0), t1(1, -1, 0), t2(0, 1, 0); + + SECTION("vertical edge through the interior") + { + double u, v, t; + const bool hit = edge_triangle_intersection( + Eigen::Vector3d(0, -0.2, -1), Eigen::Vector3d(0, -0.2, 1), t0, t1, + t2, u, v, t); + CHECK(hit); + CHECK_THAT(t, Catch::Matchers::WithinAbs(0.5, 1e-9)); + CHECK(u >= 0.0); + CHECK(v >= 0.0); + CHECK(u + v <= 1.0 + 1e-9); + } + + SECTION("recovered point matches the barycentric coordinates") + { + const Eigen::Vector3d e0(0.1, -0.3, -2), e1(0.1, -0.3, 1); + + double u, v, t; + REQUIRE(edge_triangle_intersection(e0, e1, t0, t1, t2, u, v, t)); + + // The two parameterizations of the hit point must agree. + const Eigen::Vector3d p_tri = t0 + u * (t1 - t0) + v * (t2 - t0); + const Eigen::Vector3d p_edge = e0 + t * (e1 - e0); + CHECK((p_tri - p_edge).norm() < 1e-12); + CHECK_THAT(p_edge.z(), Catch::Matchers::WithinAbs(0.0, 1e-12)); + } + + SECTION("edge entirely above the plane misses") + { + double u, v, t; + CHECK(!edge_triangle_intersection( + Eigen::Vector3d(0, 0, 0.5), Eigen::Vector3d(0, 0, 1.5), t0, t1, t2, + u, v, t)); + // Outputs are NaN, not indeterminate, on a miss. + CHECK(std::isnan(u)); + CHECK(std::isnan(v)); + CHECK(std::isnan(t)); + } + + SECTION("edge crosses the plane outside the triangle") + { + double u, v, t; + CHECK(!edge_triangle_intersection( + Eigen::Vector3d(5, 5, -1), Eigen::Vector3d(5, 5, 1), t0, t1, t2, u, + v, t)); + CHECK(std::isnan(u)); + CHECK(std::isnan(v)); + CHECK(std::isnan(t)); + } + + SECTION("agrees with is_edge_intersecting_triangle") + { + const Eigen::Vector3d e0(0, -0.2, -1), e1(0, -0.2, 1); + double u, v, t; + CHECK( + edge_triangle_intersection(e0, e1, t0, t1, t2, u, v, t) + == is_edge_intersecting_triangle(e0, e1, t0, t1, t2)); + } +} diff --git a/tests/src/tests/test_collision_mesh.cpp b/tests/src/tests/test_collision_mesh.cpp index d71654db2..67e35985e 100644 --- a/tests/src/tests/test_collision_mesh.cpp +++ b/tests/src/tests/test_collision_mesh.cpp @@ -188,4 +188,66 @@ TEST_CASE( Eigen::VectorXd actual_dof = M_dof * x_dof; CHECK(actual_dof.isApprox(expected_dof)); -} \ No newline at end of file +} +TEST_CASE("face_normals", "[collision_mesh][face_normals]") +{ + // Two triangles of a unit square in the z = 0 plane, wound oppositely so + // their normals point in opposite directions. + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, // + 1, 0, 0, // + 1, 1, 0, // + 0, 1, 0; // + + Eigen::MatrixXi F(2, 3); + F << 0, 1, 2, // CCW ⇒ +z + 0, 2, 3; // CCW ⇒ +z + + Eigen::MatrixXi E(5, 2); + E << 0, 1, 1, 2, 2, 0, 2, 3, 3, 0; + + const CollisionMesh mesh(V, E, F); + + SECTION("rest positions") + { + const std::vector N = mesh.face_normals(V); + + REQUIRE(N.size() == mesh.num_faces()); + for (const Eigen::Vector3d& n : N) { + CHECK(n.isApprox(Eigen::Vector3d::UnitZ())); + CHECK(n.norm() == Catch::Approx(1.0).margin(1e-15)); + } + } + + SECTION("normals follow deformed positions") + { + // Rotate the mesh 90° about the x-axis: +z normal ⇒ −y normal. + Eigen::MatrixXd V_rot = V; + for (int i = 0; i < V.rows(); ++i) { + V_rot.row(i) << V(i, 0), -V(i, 2), V(i, 1); + } + + const std::vector N = mesh.face_normals(V_rot); + + REQUIRE(N.size() == mesh.num_faces()); + for (const Eigen::Vector3d& n : N) { + CHECK(n.isApprox(-Eigen::Vector3d::UnitY())); + } + } + + SECTION("winding flips the normal") + { + Eigen::MatrixXi F_flipped(2, 3); + F_flipped << 0, 2, 1, // reversed ⇒ −z + 0, 3, 2; // reversed ⇒ −z + + const CollisionMesh flipped(V, E, F_flipped); + + const std::vector N = flipped.face_normals(V); + + REQUIRE(N.size() == flipped.num_faces()); + for (const Eigen::Vector3d& n : N) { + CHECK(n.isApprox(-Eigen::Vector3d::UnitZ())); + } + } +}