From ea533e9447b459ec58e477a2ae347b221b824dc0 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 12 Jan 2026 19:22:30 -0500 Subject: [PATCH 1/2] Add signed distance computations for point-plane, point-line, and line-line geometries - Implemented `point_plane_signed_distance`, `point_plane_signed_distance_gradient`, and `point_plane_signed_distance_hessian` functions for computing signed distances from a point to a triangle's plane. - Introduced `point_line_signed_distance`, `point_line_signed_distance_gradient`, and `point_line_signed_distance_hessian` functions for point-line distance calculations. - Refactored existing normal computation functions to support point-line geometry. - Added unit tests for signed distance calculations, including gradient and Hessian checks using finite differences. - Updated CMake configuration to include new test files for signed distance computations. --- python/src/geometry/normal.cpp | 128 ++++++------ src/ipc/candidates/edge_edge.cpp | 4 +- src/ipc/candidates/edge_vertex.cpp | 4 +- src/ipc/distance/CMakeLists.txt | 8 +- src/ipc/distance/point_plane.cpp | 3 +- src/ipc/distance/signed/CMakeLists.txt | 10 + src/ipc/distance/signed/line_line.cpp | 89 +++++++++ src/ipc/distance/signed/line_line.hpp | 84 ++++++++ src/ipc/distance/signed/point_line.cpp | 88 +++++++++ src/ipc/distance/signed/point_line.hpp | 64 ++++++ src/ipc/distance/signed/point_plane.cpp | 91 +++++++++ src/ipc/distance/signed/point_plane.hpp | 59 ++++++ src/ipc/geometry/normal.cpp | 185 +++++++++++++++--- src/ipc/geometry/normal.hpp | 185 +++++++++++------- tests/src/tests/candidates/test_normals.cpp | 60 +++++- tests/src/tests/distance/CMakeLists.txt | 1 + .../tests/distance/test_signed_distance.cpp | 170 ++++++++++++++++ 17 files changed, 1067 insertions(+), 166 deletions(-) create mode 100644 src/ipc/distance/signed/CMakeLists.txt create mode 100644 src/ipc/distance/signed/line_line.cpp create mode 100644 src/ipc/distance/signed/line_line.hpp create mode 100644 src/ipc/distance/signed/point_line.cpp create mode 100644 src/ipc/distance/signed/point_line.hpp create mode 100644 src/ipc/distance/signed/point_plane.cpp create mode 100644 src/ipc/distance/signed/point_plane.hpp create mode 100644 tests/src/tests/distance/test_signed_distance.cpp diff --git a/python/src/geometry/normal.cpp b/python/src/geometry/normal.cpp index 29a39906c..569aeac3b 100644 --- a/python/src/geometry/normal.cpp +++ b/python/src/geometry/normal.cpp @@ -92,56 +92,56 @@ void define_normal(py::module_& m) )ipc_qu8mg5v7"); m.def( - "edge_vertex_unnormalized_normal", &edge_vertex_unnormalized_normal, + "point_line_unnormalized_normal", &point_line_unnormalized_normal, R"ipc_qu8mg5v7( - Computes the unnormalized normal vector of an edge-vertex pair. + Computes the unnormalized normal vector of a point-line pair. Parameters ---------- - v: The vertex position. - e0: The start position of the edge. - e1: The end position of the edge. + p: The point's position. + e0: The start position of the line. + e1: The end position of the line. Returns ------- The unnormalized normal vector. )ipc_qu8mg5v7", - py::arg("v"), py::arg("e0"), py::arg("e1")); + py::arg("p"), py::arg("e0"), py::arg("e1")); m.def( - "edge_vertex_normal", &edge_vertex_normal, + "point_line_normal", &point_line_normal, R"ipc_qu8mg5v7( - Computes the normal vector of an edge-vertex pair. + Computes the normal vector of a point-line pair. Parameters ---------- - v: The vertex position. - e0: The start position of the edge. - e1: The end position of the edge. + p: The point's position. + e0: The start position of the line. + e1: The end position of the line. Returns ------- The normal vector. )ipc_qu8mg5v7", - py::arg("v"), py::arg("e0"), py::arg("e1")); + py::arg("p"), py::arg("e0"), py::arg("e1")); m.def( - "edge_vertex_unnormalized_normal_jacobian", - &edge_vertex_unnormalized_normal_jacobian, + "point_line_unnormalized_normal_jacobian", + &point_line_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( - Computes the Jacobian of the unnormalized normal vector of an edge-vertex pair. + Computes the Jacobian of the unnormalized normal vector of a point-line pair. Parameters ---------- - v: The vertex position. - e0: The start position of the edge. - e1: The end position of the edge. + p: The point's position. + e0: The start position of the line. + e1: The end position of the line. Returns ------- - The Jacobian of the unnormalized normal vector. + The Jacobian of the unnormalized normal vector of the point-line pair. )ipc_qu8mg5v7", - py::arg("v"), py::arg("e0"), py::arg("e1")); + py::arg("p"), py::arg("e0"), py::arg("e1")); m.def( "triangle_unnormalized_normal", &triangle_unnormalized_normal, @@ -248,112 +248,112 @@ void define_normal(py::module_& m) py::arg("a"), py::arg("b"), py::arg("c")); m.def( - "edge_edge_unnormalized_normal", &edge_edge_unnormalized_normal, + "line_line_unnormalized_normal", &line_line_unnormalized_normal, R"ipc_qu8mg5v7( - Computes the unnormalized normal vector of two edges. + Computes the unnormalized normal vector of two lines. Parameters ---------- - ea0: The first vertex of the first edge. - ea1: The second vertex of the first edge. - eb0: The first vertex of the second edge. - eb1: The second vertex of the second edge. + ea0: The first vertex of the first line. + ea1: The second vertex of the first line. + eb0: The first vertex of the second line. + eb1: The second vertex of the second line. Returns ------- - The unnormalized normal vector of the two edges. + The unnormalized normal vector of the two lines. )ipc_qu8mg5v7", py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1")); m.def( - "edge_edge_normal", &edge_edge_normal, + "line_line_normal", &line_line_normal, R"ipc_qu8mg5v7( - Computes the normal vector of two edges. + Computes the normal vector of two lines. Parameters ---------- - ea0: The first vertex of the first edge. - ea1: The second vertex of the first edge. - eb0: The first vertex of the second edge. - eb1: The second vertex of the second edge. + ea0: The first vertex of the first line. + ea1: The second vertex of the first line. + eb0: The first vertex of the second line. + eb1: The second vertex of the second line. Returns ------- - The normal vector of the two edges. + The normal vector of the two lines. )ipc_qu8mg5v7", py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1")); m.def( - "edge_edge_unnormalized_normal_jacobian", - &edge_edge_unnormalized_normal_jacobian, + "line_line_unnormalized_normal_jacobian", + &line_line_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( - Computes the Jacobian of the unnormalized normal vector of two edges. + Computes the Jacobian of the unnormalized normal vector of two lines. Parameters ---------- - ea0: The first vertex of the first edge. - ea1: The second vertex of the first edge. - eb0: The first vertex of the second edge. - eb1: The second vertex of the second edge. + ea0: The first vertex of the first line. + ea1: The second vertex of the first line. + eb0: The first vertex of the second line. + eb1: The second vertex of the second line. Returns ------- - The Jacobian of the unnormalized normal vector of the two edges. + The Jacobian of the unnormalized normal vector of the two lines. )ipc_qu8mg5v7", py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1")); m.def( - "edge_edge_normal_jacobian", &edge_edge_normal_jacobian, + "line_line_normal_jacobian", &line_line_normal_jacobian, R"ipc_qu8mg5v7( - Computes the Jacobian of the normal vector of two edges. + Computes the Jacobian of the normal vector of two lines. Parameters ---------- - ea0: The first vertex of the first edge. - ea1: The second vertex of the first edge. - eb0: The first vertex of the second edge. - eb1: The second vertex of the second edge. + ea0: The first vertex of the first line. + ea1: The second vertex of the first line. + eb0: The first vertex of the second line. + eb1: The second vertex of the second line. Returns ------- - The Jacobian of the normal vector of the two edges. + The Jacobian of the normal vector of the two lines. )ipc_qu8mg5v7", py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1")); m.def( - "edge_edge_unnormalized_normal_hessian", - &edge_edge_unnormalized_normal_hessian, + "line_line_unnormalized_normal_hessian", + &line_line_unnormalized_normal_hessian, R"ipc_qu8mg5v7( - Computes the Hessian of the unnormalized normal vector of two edges. + Computes the Hessian of the unnormalized normal vector of two lines. Parameters ---------- - ea0: The first vertex of the first edge. - ea1: The second vertex of the first edge. - eb0: The first vertex of the second edge. - eb1: The second vertex of the second edge. + ea0: The first vertex of the first line. + ea1: The second vertex of the first line. + eb0: The first vertex of the second line. + eb1: The second vertex of the second line. Returns ------- - The Hessian of the unnormalized normal vector of the two edges. + The Hessian of the unnormalized normal vector of the two lines. )ipc_qu8mg5v7", py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1")); m.def( - "edge_edge_normal_hessian", &edge_edge_normal_hessian, + "line_line_normal_hessian", &line_line_normal_hessian, R"ipc_qu8mg5v7( - Computes the Hessian of the normal vector of two edges. + Computes the Hessian of the normal vector of two lines. Parameters ---------- - ea0: The first vertex of the first edge. - ea1: The second vertex of the first edge. - eb0: The first vertex of the second edge. - eb1: The second vertex of the second edge. + ea0: The first vertex of the first line. + ea1: The second vertex of the first line. + eb0: The first vertex of the second line. + eb1: The second vertex of the second line. Returns ------- - The Hessian of the normal vector of the two edges. + The Hessian of the normal vector of the two lines. )ipc_qu8mg5v7", py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1")); } diff --git a/src/ipc/candidates/edge_edge.cpp b/src/ipc/candidates/edge_edge.cpp index 5438878e6..fd040b146 100644 --- a/src/ipc/candidates/edge_edge.cpp +++ b/src/ipc/candidates/edge_edge.cpp @@ -100,7 +100,7 @@ VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal( Eigen::ConstRef positions) const { assert(positions.size() == 12); - return edge_edge_unnormalized_normal( + return line_line_unnormalized_normal( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>()); } @@ -110,7 +110,7 @@ EdgeEdgeCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const { assert(positions.size() == 12); - return edge_edge_unnormalized_normal_jacobian( + return line_line_unnormalized_normal_jacobian( positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>()); } diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index 382d7c936..4ea4ef809 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -82,7 +82,7 @@ VectorMax3d EdgeVertexCandidate::compute_unnormalized_normal( Eigen::ConstRef positions) const { const int dim = this->dim(positions.size()); - return edge_vertex_unnormalized_normal( + return point_line_unnormalized_normal( positions.head(dim), positions.segment(dim, dim), positions.tail(dim)); } @@ -91,7 +91,7 @@ EdgeVertexCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const { const int dim = this->dim(positions.size()); - return edge_vertex_unnormalized_normal_jacobian( + return point_line_unnormalized_normal_jacobian( positions.head(dim), positions.segment(dim, dim), positions.tail(dim)); } diff --git a/src/ipc/distance/CMakeLists.txt b/src/ipc/distance/CMakeLists.txt index dc2a541b4..846253b06 100644 --- a/src/ipc/distance/CMakeLists.txt +++ b/src/ipc/distance/CMakeLists.txt @@ -19,4 +19,10 @@ set(SOURCES point_triangle.hpp ) -target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ + +add_subdirectory(signed) \ No newline at end of file diff --git a/src/ipc/distance/point_plane.cpp b/src/ipc/distance/point_plane.cpp index f0cafb18d..5e77342e6 100644 --- a/src/ipc/distance/point_plane.cpp +++ b/src/ipc/distance/point_plane.cpp @@ -1,5 +1,6 @@ #include "point_plane.hpp" +#include #include #include @@ -21,7 +22,7 @@ double point_plane_distance( Eigen::ConstRef t1, Eigen::ConstRef t2) { - return point_plane_distance(p, t0, (t1 - t0).cross(t2 - t0)); + return point_plane_distance(p, t0, triangle_normal(t0, t1, t2)); } Eigen::Vector3d point_plane_distance_gradient( diff --git a/src/ipc/distance/signed/CMakeLists.txt b/src/ipc/distance/signed/CMakeLists.txt new file mode 100644 index 000000000..55e7737ad --- /dev/null +++ b/src/ipc/distance/signed/CMakeLists.txt @@ -0,0 +1,10 @@ +set(SOURCES + line_line.cpp + line_line.hpp + point_line.cpp + point_line.hpp + point_plane.cpp + point_plane.hpp +) + +target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp new file mode 100644 index 000000000..bff298a4a --- /dev/null +++ b/src/ipc/distance/signed/line_line.cpp @@ -0,0 +1,89 @@ +#include "line_line.hpp" + +namespace ipc { + +Vector12d line_line_signed_distance_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const Eigen::Vector3d n = line_line_normal(ea0, ea1, eb0, eb1); + const Eigen::Matrix jac_n = + line_line_normal_jacobian(ea0, ea1, eb0, eb1); + + Vector12d grad = jac_n.transpose() * (ea0 - eb0); + grad.segment<3>(0) += n; + grad.segment<3>(6) -= n; + + return grad; +} + +Matrix12d line_line_signed_distance_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + // Precompute normal's Jacobian and Hessian + const Eigen::Matrix jac_n = + line_line_normal_jacobian(ea0, ea1, eb0, eb1); + const Eigen::Matrix hess_n = + line_line_normal_hessian(ea0, ea1, eb0, eb1); + + // Vector from eb0 to ea0 (Distance vector) + const Eigen::Vector3d v = ea0 - eb0; + + Matrix12d hess; + + // --------------------------------------------------------- + // 1. Tensor Contraction (Curvature Term) + // --------------------------------------------------------- + // Contract the normal Hessian (3x144) with vector v (3x1). + // This computes (v ⋅ d²n/dx²). + // The result is a 1x144 vector, which maps to the 12x12 Hessian matrix. + hess = (v.transpose() * hess_n).reshaped(12, 12); + + // --------------------------------------------------------- + // 2. Add Jacobian Terms (Product Rule Corrections) + // --------------------------------------------------------- + // The gradient is Jₙᵀ v + Jᵥᵀ n. + // The Hessian correction involves Jₙᵀ * Jᵥ + Jᵥᵀ * Jₙ. + // Since v = ea0 - eb0: + // d(v)/d(ea0) = I, d(v)/d(eb0) = -I, others are 0. + + // Extract 3x3 sub-blocks for cleaner code + // Indices: 0->ea0, 1->ea1, 2->eb0, 3->eb1 + const auto J_a0 = jac_n.leftCols<3>(); + const auto J_a1 = jac_n.middleCols<3>(3); + const auto J_b0 = jac_n.middleCols<3>(6); + const auto J_b1 = jac_n.rightCols<3>(); + + // --- Block Row 0 (ea0) --- + // d/d(ea0) [Jₙᵀ v + n] -> adds J + Jᵀ terms + hess.block<3, 3>(0, 0) += J_a0 + J_a0.transpose(); + hess.block<3, 3>(0, 3) += J_a1; + hess.block<3, 3>(0, 6) += J_b0 - J_a0.transpose(); + hess.block<3, 3>(0, 9) += J_b1; + + // --- Block Row 1 (ea1) --- + // d/d(ea1) [Jₙᵀ v] -> adds Jᵀ terms + hess.block<3, 3>(3, 0) += J_a1.transpose(); + hess.block<3, 3>(3, 6) -= J_a1.transpose(); + + // --- Block Row 2 (eb0) --- + // d/d(eb0) [Jₙᵀ v - n] -> adds -J - Jᵀ terms + hess.block<3, 3>(6, 0) += J_b0.transpose() - J_a0; + hess.block<3, 3>(6, 3) -= J_a1; + hess.block<3, 3>(6, 6) -= J_b0 + J_b0.transpose(); + hess.block<3, 3>(6, 9) -= J_b1; + + // --- Block Row 3 (eb1) --- + // d/d(eb1) [Jₙᵀ v] -> adds Jᵀ terms + hess.block<3, 3>(9, 0) += J_b1.transpose(); + hess.block<3, 3>(9, 6) -= J_b1.transpose(); + + return hess; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp new file mode 100644 index 000000000..f60ce5fa7 --- /dev/null +++ b/src/ipc/distance/signed/line_line.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include + +namespace ipc { + +/// Compute the signed distance between two lines in 3D. +/// +/// The two lines are specified by points (ea0, ea1) and (eb0, eb1). +/// The returned value is the scalar projection of the vector (ea0 - eb0) +/// onto the unit normal that is perpendicular to both lines (as produced by +/// line_line_normal). In other words, this is the signed distance along the +/// common normal between the two lines; the sign follows the orientation of +/// the normal returned by line_line_normal. +/// +/// @param ea0 First endpoint (or a point) on the first line. +/// @param ea1 Second endpoint (or a direction-defining point) on the first line. +/// @param eb0 First endpoint (or a point) on the second line. +/// @param eb1 Second endpoint (or a direction-defining point) on the second line. +/// @return Signed distance between the two lines along the common normal. If +/// the lines are parallel (no unique common normal), the result may be +/// ill-conditioned or implementation-defined. +/// @note The points may be any two distinct points on each line (they need not +/// represent segment endpoints). Behavior is undefined if ea0 == ea1 or eb0 == +/// eb1. +inline double line_line_signed_distance( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + return line_line_normal(ea0, ea1, eb0, eb1).dot(ea0 - eb0); +} + +/// Compute the gradient of the signed line-line distance with respect to the +/// four 3D input points: ea0, ea1, eb0, eb1. +/// +/// The returned gradient is a 12-vector ordered as [d/d(ea0); d/d(ea1); +/// d/d(eb0); d/d(eb1)], where each block is a 3-vector. This function +/// differentiates the signed distance produced by line_line_signed_distance. +/// The gradient may be undefined or numerically unstable when the two lines are +/// parallel or when the direction-defining points coincide. +/// +/// @param ea0 First point on the first line (as in line_line_signed_distance). +/// @param ea1 Second point on the first line. +/// @param eb0 First point on the second line. +/// @param eb1 Second point on the second line. +/// @return 12-dimensional gradient of the signed distance with respect to +/// [ea0, ea1, eb0, eb1]. +/// @pre ea0 != ea1 and eb0 != eb1. For near-parallel lines, results may be unstable. +/// +/// @see line_line_signed_distance, line_line_normal +Vector12d line_line_signed_distance_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +/// Compute the Hessian (second derivative) of the signed line-line distance +/// with respect to the four 3D input points: ea0, ea1, eb0, eb1. +/// +/// The returned matrix is 12x12 and corresponds to the second derivatives +/// of the scalar signed distance with respect to the stacked variable +/// [ea0; ea1; eb0; eb1]. The Hessian captures curvature information and is +/// typically required for second-order optimization or sensitivity analysis. +/// As with the gradient, the Hessian is undefined or numerically unstable if +/// either input line is degenerate (coincident points) or if the lines are +/// parallel (no unique perpendicular direction). +/// +/// @param ea0 First point on the first line. +/// @param ea1 Second point on the first line. +/// @param eb0 First point on the second line. +/// @param eb1 Second point on the second line. +/// @return 12x12 Hessian matrix of second derivatives of the signed distance. +/// @pre ea0 != ea1 and eb0 != eb1. Results may be invalid for parallel lines. +/// +/// @see line_line_signed_distance, line_line_signed_distance_gradient +Matrix12d line_line_signed_distance_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp new file mode 100644 index 000000000..1863bdaa2 --- /dev/null +++ b/src/ipc/distance/signed/point_line.cpp @@ -0,0 +1,88 @@ +#include "point_line.hpp" + +namespace ipc { + +Vector6d point_line_signed_distance_gradient( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + const Eigen::Vector2d n = point_line_normal(p, e0, e1); + const Eigen::Matrix jac_n = + point_line_normal_jacobian(p, e0, e1); + + Vector6d grad = jac_n.transpose() * (p - e0); + grad.segment<2>(0) += n; + grad.segment<2>(2) -= n; + + return grad; +} + +Matrix6d point_line_signed_distance_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + // Precompute normal's Jacobian and Hessian + const Eigen::Matrix jac_n = + point_line_normal_jacobian(p, e0, e1); + const Eigen::Matrix hess_n = + point_line_normal_hessian(p, e0, e1); + + // Vector from e0 to p + const Eigen::Vector2d v = p - e0; + + Matrix6d hess; + + // --------------------------------------------------------- + // 1. Tensor Contraction (Curvature Term) + // --------------------------------------------------------- + // Contract the normal Hessian (2x36) with vector v (2x1). + // Result is 1x36, mapped to 6x6. + hess = (v.transpose() * hess_n).reshaped(6, 6); + + // --------------------------------------------------------- + // 2. Add Jacobian Terms (Product Rule Corrections) + // --------------------------------------------------------- + // Formula: H += (Jₙᵀ Jᵥ) + (Jᵥᵀ Jₙ) + // Jᵥ w.r.t [p, e0, e1] is [I, -I, 0] + + // Extract 2x2 Jacobian blocks for e₀ and e₁ + // Note: ∇ n has columns 0-1 (p), 2-3 (e₀), 4-5 (e₁). + // Normal usually doesn't depend on p, so block(0,0) is zero. + assert(bool(jac_n.leftCols<2>().isZero())); + const auto J_e0 = jac_n.middleCols<2>(2); + const auto J_e1 = jac_n.rightCols<2>(); + + // --- Block Row 0 (p) --- + // d/dp [Jₙᵀ v] -> Jₙᵀ => Adds Jᵀ to the row + // (p, e0) += J_e0 + hess.block<2, 2>(0, 2) += J_e0; + // (p, e1) += J_e1 + hess.block<2, 2>(0, 4) += J_e1; + + // --- Block Row 1 (e0) --- + // d/de0 [Jₙᵀ * v + n] -> Jₙᵀ * (-I) + (-I)ᵀ * Jₙ + + // (e0, p) += J_e0^T + hess.block<2, 2>(2, 0) += J_e0.transpose(); + + // (e0, e0) -= (J_e0 + J_e0^T) + hess.block<2, 2>(2, 2) -= (J_e0 + J_e0.transpose()); + + // (e0, e1) -= J_e1 + hess.block<2, 2>(2, 4) -= J_e1; + + // --- Block Row 2 (e1) --- + // d/de1 [Jₙᵀ * v] -> Jₙᵀ (-I) + + // (e1, p) += J_e1^T + hess.block<2, 2>(4, 0) += J_e1.transpose(); + + // (e1, e0) -= J_e1^T + hess.block<2, 2>(4, 2) -= J_e1.transpose(); + + return hess; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/signed/point_line.hpp b/src/ipc/distance/signed/point_line.hpp new file mode 100644 index 000000000..a9be7434f --- /dev/null +++ b/src/ipc/distance/signed/point_line.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include + +namespace ipc { + +/// Compute the signed distance from a point to a directed line segment. +/// +/// The signed distance is computed as d = n · (p - e0), +/// where n = point_line_normal(p, e0, e1) is the unit normal associated with +/// the directed edge (e0 -> e1) chosen consistently for the point p. +/// Positive/negative sign indicates which side of the directed edge the point +/// lies on. +/// +/// @param p The query point (2D). +/// @param e0 The first endpoint of the directed edge (2D). +/// @param e1 The second endpoint of the directed edge (2D). +/// @return The signed scalar distance from p to the infinite line through e0 and e1. +/// @note The edge must be non-degenerate (e0 != e1). +inline double point_line_signed_distance( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + return point_line_normal(p, e0, e1).dot(p - e0); +} + +/// Compute the gradient of the signed point-to-line distance with respect to +/// all input coordinates packed as [p_x, p_y, e0_x, e0_y, e1_x, e1_y]^T. +/// +/// The returned Vector6d contains partial derivatives of the scalar signed +/// distance d with respect to each coordinate in the above ordering: +/// grad = [∂d/∂p, ∂d/∂e0, ∂d/∂e1]^T. +/// +/// @param p The query point (2D). +/// @param e0 The first endpoint of the directed edge (2D). +/// @param e1 The second endpoint of the directed edge (2D). +/// @return A 6-vector containing the gradient of the signed distance. +/// @note The edge must be non-degenerate (e0 != e1). +Vector6d point_line_signed_distance_gradient( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + +/// Compute the Hessian (second derivatives) of the signed point-to-line +/// distance with respect to all input coordinates packed as [p_x, p_y, e0_x, +/// e0_y, e1_x, e1_y]^T. +/// +/// The returned Matrix6d is the symmetric 6x6 matrix of second partial +/// derivatives: +/// H_ij = ∂^2 d / (∂x_i ∂x_j), +/// with the same coordinate ordering as in the gradient. +/// +/// @param p The query point (2D). +/// @param e0 The first endpoint of the directed edge (2D). +/// @param e1 The second endpoint of the directed edge (2D). +/// @return A 6x6 Hessian matrix of the signed distance. +/// @note The edge must be non-degenerate (e0 != e1). +Matrix6d point_line_signed_distance_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/signed/point_plane.cpp b/src/ipc/distance/signed/point_plane.cpp new file mode 100644 index 000000000..4b9c19f4d --- /dev/null +++ b/src/ipc/distance/signed/point_plane.cpp @@ -0,0 +1,91 @@ +#include "point_plane.hpp" + +namespace ipc { + +Vector12d point_plane_signed_distance_gradient( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) +{ + const Eigen::Vector3d n = triangle_normal(t0, t1, t2); + const Eigen::Matrix jac_n = + triangle_normal_jacobian(t0, t1, t2); + + Vector12d grad; + grad.segment<3>(0) = n; + grad.segment<3>(3) = jac_n.leftCols<3>().transpose() * (p - t0) - n; + grad.segment<3>(6) = jac_n.middleCols<3>(3).transpose() * (p - t0); + grad.segment<3>(9) = jac_n.rightCols<3>().transpose() * (p - t0); + + return grad; +} + +Matrix12d point_plane_signed_distance_hessian( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) +{ + // Precompute normal's Jacobian and Hessian + const Eigen::Matrix jac_n = + triangle_normal_jacobian(t0, t1, t2); + const Eigen::Matrix hess_n = + triangle_normal_hessian(t0, t1, t2); + + // Vector from t0 to p + const Eigen::Vector3d v = p - t0; + + Matrix12d hess; + + // --------------------------------------------------------- + // 0. Fill Point-Point Block (p, p) + // --------------------------------------------------------- + // The second derivative w.r.t p is zero since the normal is constant w.r.t + // p. + hess.block<3, 3>(0, 0).setZero(); + + // --------------------------------------------------------- + // 1. Fill Mixed Derivatives (p, t) and (t, p) + // --------------------------------------------------------- + // The gradient w.r.t p is n. + // The mixed derivative is the Jacobian of n w.r.t t. + hess.block<3, 9>(0, 3) = jac_n; + hess.block<9, 3>(3, 0) = jac_n.transpose(); + + // --------------------------------------------------------- + // 2. Fill Triangle-Triangle Block (t, t) + // --------------------------------------------------------- + // Formula: Hₜₜ = (v ⋅ Hₙ) - (δⱼ₀ Jᵢᵀ + δᵢ₀ Jⱼ) + + // A. Contraction of the normal Hessian tensor with vector v + // hess_n is 3x81. v is 3x1. Result is 1x81, which maps to 9x9. + hess.block<9, 9>(3, 3) = (v.transpose() * hess_n).reshaped(9, 9); + + // B. Subtract first derivative terms (Product Rule corrections) + // Extract 3x3 Jacobian blocks for t0, t1, t2 + const auto J0 = jac_n.leftCols<3>(); + const auto J1 = jac_n.middleCols<3>(3); + const auto J2 = jac_n.rightCols<3>(); + + // Apply corrections for terms involving t0 (index 0) + + // Block (t0, t0): i=0, j=0. Subtract J0 + J0^T + hess.block<3, 3>(3, 3) -= (J0 + J0.transpose()); + + // Block (t0, t1): i=0, j=1. Subtract J1 + hess.block<3, 3>(3, 6) -= J1; + + // Block (t0, t2): i=0, j=2. Subtract J2 + hess.block<3, 3>(3, 9) -= J2; + + // Block (t1, t0): i=1, j=0. Subtract J1^T + hess.block<3, 3>(6, 3) -= J1.transpose(); + + // Block (t2, t0): i=2, j=0. Subtract J2^T + hess.block<3, 3>(9, 3) -= J2.transpose(); + + return hess; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/signed/point_plane.hpp b/src/ipc/distance/signed/point_plane.hpp new file mode 100644 index 000000000..04fd90f34 --- /dev/null +++ b/src/ipc/distance/signed/point_plane.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include + +namespace ipc { + +/// Compute the signed distance from a point to the plane of a triangle. +/// +/// The signed distance is computed as: +/// d = triangle_normal(t0, t1, t2) · (p - t0) +/// The sign corresponds to the orientation of the triangle normal. +/// +/// @param p The query point (3D). +/// @param t0 First vertex of the triangle (3D), used as a point on the plane. +/// @param t1 Second vertex of the triangle (3D). +/// @param t2 Third vertex of the triangle (3D). +/// @return The signed distance from p to the plane of the triangle. +inline double point_plane_signed_distance( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) +{ + return triangle_normal(t0, t1, t2).dot(p - t0); +} + +/// Compute the gradient of the signed point-to-plane distance. +/// +/// The returned Vector12d contains derivatives in the order: +/// [ ∂d/∂p (3), ∂d/∂t0 (3), ∂d/∂t1 (3), ∂d/∂t2 (3) ] +/// +/// @param p The query point (3D). +/// @param t0 First vertex of the triangle (3D). +/// @param t1 Second vertex of the triangle (3D). +/// @param t2 Third vertex of the triangle (3D). +/// @return A Vector12d containing the gradient of the signed distance. +Vector12d point_plane_signed_distance_gradient( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2); + +/// Compute the Hessian (matrix of second derivatives) of the signed distance. +/// +/// The returned Matrix12d is the 12x12 Hessian with variables ordered as: +/// [ p (3), t0 (3), t1 (3), t2 (3) ]. +/// +/// @param p The query point (3D). +/// @param t0 First vertex of the triangle (3D). +/// @param t1 Second vertex of the triangle (3D). +/// @param t2 Third vertex of the triangle (3D). +/// @return A Matrix12d representing the Hessian of the signed distance. +Matrix12d point_plane_signed_distance_hessian( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2); + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index 246112149..c082fea97 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -30,48 +30,48 @@ normalization_and_jacobian_and_hessian(Eigen::ConstRef x) return std::make_tuple(xhat, J, H); } -// --- edge-vertex normal functions ------------------------------------------- +// --- point-line normal functions ------------------------------------------- -VectorMax3d edge_vertex_unnormalized_normal( - Eigen::ConstRef v, +VectorMax3d point_line_unnormalized_normal( + Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1) { - assert(v.size() == e0.size() && v.size() == e1.size()); - assert(v.size() == 2 || v.size() == 3); - if (v.size() == 2) { - // In 2D, the normal is simply the perpendicular vector to the edge - const Eigen::Vector2d e = e1.tail<2>() - e0.tail<2>(); + assert(p.size() == e0.size() && p.size() == e1.size()); + assert(p.size() == 2 || p.size() == 3); + if (p.size() == 2) { + // In 2D, the normal is simply the perpendicular vector to the line + const Eigen::Vector2d e = e1.head<2>() - e0.head<2>(); return Eigen::Vector2d(-e.y(), e.x()); } else { // 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. const Eigen::Vector3d e = e1 - e0; - const Eigen::Vector3d d = v - e0; + const Eigen::Vector3d d = p - e0; return d * e.dot(e) - e * e.dot(d); } } -MatrixMax edge_vertex_unnormalized_normal_jacobian( - Eigen::ConstRef v, +MatrixMax point_line_unnormalized_normal_jacobian( + Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1) { - assert(v.size() == e0.size() && v.size() == e1.size()); - assert(v.size() == 2 || v.size() == 3); + assert(p.size() == e0.size() && p.size() == e1.size()); + assert(p.size() == 2 || p.size() == 3); - MatrixMax dn(v.size(), 3 * v.size()); + MatrixMax dn(p.size(), 3 * p.size()); - if (v.size() == 2) { - // In 2D, the normal is simply the perpendicular vector to the edge + if (p.size() == 2) { + // In 2D, the normal is simply the perpendicular vector to the line dn.leftCols<2>().setZero(); dn.middleCols<2>(2) << 0, 1, -1, 0; dn.rightCols<2>() << 0, -1, 1, 0; return dn; } else { const Eigen::Vector3d e = e1 - e0; - const Eigen::Vector3d d = v - e0; + const Eigen::Vector3d d = p - e0; const auto I = Eigen::Matrix3d::Identity(); @@ -87,6 +87,145 @@ MatrixMax edge_vertex_unnormalized_normal_jacobian( return dn; } +MatrixMax point_line_unnormalized_normal_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + assert(p.size() == e0.size() && p.size() == e1.size()); + assert(p.size() == 2 || p.size() == 3); + if (p.size() == 2) { + // In 2D, the normal is simply the perpendicular vector to the line + return MatrixMax::Zero(2, 36); + } else { + const Eigen::Vector3d e = e1 - e0; + const Eigen::Vector3d d = p - e0; + const auto I = Eigen::Matrix3d::Identity(); + + // 3 components (rows), 81 entries (flattened 9x9 Hessian) + MatrixMax hess(3, 81); + hess.setZero(); + + // Compute the Hessian for each component k of the normal vector n + for (int k = 0; k < 3; ++k) { + // Standard basis vector for component k + Eigen::Vector3d ek = Eigen::Vector3d::Zero(); + ek(k) = 1.0; + + // The full 9x9 Hessian for component n[k] + // Order of variables: p (0-2), e0 (3-5), e1 (6-8) + Matrix9d Hk; + Hk.setZero(); + + // ------------------------------------------------------------- + // Block (p, e1): Derivative of ∂n/∂p w.r.t e1 + // ------------------------------------------------------------- + // ∂n/∂p = ||e||^2 * I - e * e^T + // Differentiating row k w.r.t e: + // H_pe1 = 2 * ek * e^T - e * ek^T - e[k] * I + Eigen::Matrix3d H_pe1 = + 2.0 * ek * e.transpose() - e * ek.transpose() - e(k) * I; + + // ------------------------------------------------------------- + // Block (e1, e1): Derivative of ∂n/∂e1 w.r.t e1 + // ------------------------------------------------------------- + // ∂n/∂e1 = -(e.d)I - e*d^T + 2d*e^T + // Differentiating row k w.r.t e (keeping d constant): + // H_e1e1 = -ek * d^T - d * ek^T + 2 * d[k] * I + Eigen::Matrix3d H_e1e1 = + -ek * d.transpose() - d * ek.transpose() + 2.0 * d(k) * I; + + // ------------------------------------------------------------- + // Block (e1, e0): Derivative of ∂n/∂e1 w.r.t e0 + // ------------------------------------------------------------- + // Chain rule: ∂/∂e0 = ∂/∂e * (-1) + ∂/∂d * (-1) + + // Part 1: Contribution from e (-H_e1e1) + + // Part 2: Contribution from d + // Differentiating row k w.r.t d (keeping e constant): + // M = -ek * e^T - e[k] * I + 2 * e * ek^T + Eigen::Matrix3d term_d = + -ek * e.transpose() - e(k) * I + 2.0 * e * ek.transpose(); + + // Combine: -1 * H_e1e1 - 1 * term_d + Eigen::Matrix3d H_e1e0 = -H_e1e1 - term_d; + + // ------------------------------------------------------------- + // Assemble 9x9 Matrix (Symmetry & Translation Invariance) + // ------------------------------------------------------------- + + // (p, p) is Zero because ∂n/∂p is linear in p + + // (p, e1) and (e1, p) + Hk.block<3, 3>(0, 6) = H_pe1; + Hk.block<3, 3>(6, 0) = H_pe1.transpose(); + + // (p, e0) = - (p, e1) + // Because ∂e/∂e0 = -∂e/∂e1, and ∂n/∂p depends only on e + Hk.block<3, 3>(0, 3) = -H_pe1; + Hk.block<3, 3>(3, 0) = -H_pe1.transpose(); + + // (e1, e1) + Hk.block<3, 3>(6, 6) = H_e1e1; + + // (e1, e0) and (e0, e1) + Hk.block<3, 3>(6, 3) = H_e1e0; + Hk.block<3, 3>(3, 6) = H_e1e0.transpose(); + + // (e0, e0) + // Translation invariance: Sum of rows (and cols) must be zero. + // H_e0e0 = -H_pe0 - H_e1e0 + // = -(-H_pe1) - H_e1e0 + // = H_pe1 - H_e1e0 + Hk.block<3, 3>(3, 3) = H_pe1 - H_e1e0; + + // Flatten 9x9 (Column-Major) and assign to row k + hess.row(k) = Hk.reshaped(); + } + + return hess; + } +} + +MatrixMax point_line_normal_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + assert(p.size() == e0.size() && p.size() == e1.size()); + assert(p.size() == 2 || p.size() == 3); + + const VectorMax3d z = point_line_unnormalized_normal(p, e0, e1); + const double z_norm2 = z.squaredNorm(); + const double z_norm = std::sqrt(z_norm2); + const double z_norm3 = z_norm2 * z_norm; + + const int DOF = 3 * z.size(); // total dof (6 or 9) + const int ROWS = z.size(); // dimension (2 or 3) + const int COLS = DOF * DOF; // (dof per variable)² + + const auto dz_dx = point_line_unnormalized_normal_jacobian(p, e0, e1); + const auto d2z_dx2 = point_line_unnormalized_normal_hessian(p, e0, e1); + + MatrixMax d2n_dx2(ROWS, COLS); + for (int k = 0; k < COLS; ++k) { + const int i = k / DOF, j = k % DOF; + + const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + + const double dalpha_dxj = + (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dx2.col(k)) + - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) + / z_norm3; + + d2n_dx2.col(k) = -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 + + d2z_dx2.col(k) / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; + } + + return d2n_dx2; +} + // --- triangle normal functions ---------------------------------------------- namespace { @@ -158,9 +297,9 @@ Eigen::Matrix triangle_normal_hessian( return d2n_dx2; } -// --- edge-edge normal functions --------------------------------------------- +// --- line-line normal functions --------------------------------------------- -Eigen::Matrix edge_edge_unnormalized_normal_hessian( +Eigen::Matrix line_line_unnormalized_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -191,21 +330,21 @@ Eigen::Matrix edge_edge_unnormalized_normal_hessian( return H.reshaped(3, 144); } -Eigen::Matrix edge_edge_normal_hessian( +Eigen::Matrix line_line_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1) { - const Eigen::Vector3d z = edge_edge_unnormalized_normal(ea0, ea1, eb0, eb1); + const Eigen::Vector3d z = line_line_unnormalized_normal(ea0, ea1, eb0, eb1); const double z_norm2 = z.squaredNorm(); const double z_norm = std::sqrt(z_norm2); const double z_norm3 = z_norm2 * z_norm; const auto dz_dx = - edge_edge_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); + line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); const auto d2z_dx2 = - edge_edge_unnormalized_normal_hessian(ea0, ea1, eb0, eb1); + line_line_unnormalized_normal_hessian(ea0, ea1, eb0, eb1); Eigen::Matrix d2n_dx2; for (int k = 0; k < 144; ++k) { diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index adb0a9ba6..c3b6e2248 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -6,6 +6,8 @@ namespace ipc { +// ============================================================================= + /// @brief Computes the normalization and Jacobian of a vector. /// @param x The input vector. /// @return A tuple containing the normalized vector and its Jacobian. @@ -35,6 +37,8 @@ normalization_hessian(Eigen::ConstRef x) return std::get<2>(normalization_and_jacobian_and_hessian(x)); } +// ============================================================================= + /// @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. @@ -51,45 +55,86 @@ inline Eigen::Matrix3d cross_product_matrix(Eigen::ConstRef v) /// @return The Jacobian of the cross product matrix. Eigen::Matrix cross_product_matrix_jacobian(); +// ============================================================================= + /** - * \defgroup geometry Edge-vertex normal - * \brief Functions for computing an edge-vertex normal and resp. Jacobians. + * \defgroup geometry Point-line normal + * \brief Functions for computing a point-line normal and resp. Jacobians. * @{ */ -/// @brief Computes the unnormalized normal vector of an edge-vertex pair. -/// @param v The vertex position. -/// @param e0 The start position of the edge. -/// @param e1 The end position of the edge. +/// @brief Computes the unnormalized normal vector of a point-line pair. +/// @param p The vertex position. +/// @param e0 The start position of the line. +/// @param e1 The end position of the line. /// @return The unnormalized normal vector. -VectorMax3d edge_vertex_unnormalized_normal( - Eigen::ConstRef v, +VectorMax3d point_line_unnormalized_normal( + Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1); -/// @brief Computes the normal vector of an edge-vertex pair. -/// @param v The vertex position. -/// @param e0 The start position of the edge. -/// @param e1 The end position of the edge. +/// @brief Computes the normal vector of a point-line pair. +/// @param p The vertex position. +/// @param e0 The start position of the line. +/// @param e1 The end position of the line. /// @return The normal vector. -inline VectorMax3d edge_vertex_normal( - Eigen::ConstRef v, +inline VectorMax3d point_line_normal( + Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1) { - return edge_vertex_unnormalized_normal(v, e0, e1).normalized(); + return point_line_unnormalized_normal(p, e0, e1).normalized(); } -/// @brief Computes the Jacobian of the unnormalized normal vector of an edge-vertex pair. -/// @param v The vertex position. -/// @param e0 The start position of the edge. -/// @param e1 The end position of the edge. +/// @brief Computes the Jacobian of the unnormalized normal vector of a point-line pair. +/// @param p The vertex position. +/// @param e0 The start position of the line. +/// @param e1 The end position of the line. /// @return The Jacobian of the unnormalized normal vector. -MatrixMax edge_vertex_unnormalized_normal_jacobian( - Eigen::ConstRef v, +MatrixMax point_line_unnormalized_normal_jacobian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + +/// @brief Computes the Hessian of the unnormalized normal vector of a point-line pair. +/// @param p The vertex position. +/// @param e0 The start position of the line. +/// @param e1 The end position of the line. +/// @return The Hessian of the unnormalized normal vector of the point-line pair. +MatrixMax point_line_unnormalized_normal_hessian( + Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1); +/// @brief Computes the Jacobian of the normal vector of a point-line pair. +/// @param p The vertex position. +/// @param e0 The start position of the line. +/// @param e1 The end position of the line. +/// @return The Jacobian of the normal vector. +inline MatrixMax point_line_normal_jacobian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x + return normalization_jacobian(point_line_unnormalized_normal(p, e0, e1)) + * point_line_unnormalized_normal_jacobian(p, e0, e1); +} + +/// @brief Computes the Hessian of the normal vector of a point-line pair. +/// @param p The vertex position. +/// @param e0 The start position of the line. +/// @param e1 The end position of the line. +/// @return The Hessian of the normal vector. +MatrixMax point_line_normal_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + +/** @} */ + +// ============================================================================= + /** * \defgroup geometry Triangle normal * \brief Functions for computing a triangle's normal and resp. Jacobians. @@ -176,19 +221,21 @@ Eigen::Matrix triangle_normal_hessian( /** @} */ +// ============================================================================= + /** - * \defgroup geometry Edge-edge normal - * \brief Functions for computing an edge-edge normal and resp. Jacobians. + * \defgroup geometry Line-line normal + * \brief Functions for computing a line-line normal and resp. Jacobians. * @{ */ -/// @brief Computes the unnormalized normal vector of two edges. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The unnormalized normal vector of the two edges. -inline Eigen::Vector3d edge_edge_unnormalized_normal( +/// @brief Computes the unnormalized normal vector of two lines. +/// @param ea0 The first vertex of the first line. +/// @param ea1 The second vertex of the first line. +/// @param eb0 The first vertex of the second line. +/// @param eb1 The second vertex of the second line. +/// @return The unnormalized normal vector of the two lines. +inline Eigen::Vector3d line_line_unnormalized_normal( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -197,28 +244,28 @@ inline Eigen::Vector3d edge_edge_unnormalized_normal( return (ea1 - ea0).cross(eb1 - eb0); } -/// @brief Computes the normal vector of two edges. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The normal vector of the two edges. -inline Eigen::Vector3d edge_edge_normal( +/// @brief Computes the normal vector of two lines. +/// @param ea0 The first vertex of the first line. +/// @param ea1 The second vertex of the first line. +/// @param eb0 The first vertex of the second line. +/// @param eb1 The second vertex of the second line. +/// @return The normal vector of the two lines. +inline Eigen::Vector3d line_line_normal( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1) { - return edge_edge_unnormalized_normal(ea0, ea1, eb0, eb1).normalized(); + return line_line_unnormalized_normal(ea0, ea1, eb0, eb1).normalized(); } -/// @brief Computes the Jacobian of the unnormalized normal vector of two edges. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The Jacobian of the unnormalized normal vector of the two edges. -inline Eigen::Matrix edge_edge_unnormalized_normal_jacobian( +/// @brief Computes the Jacobian of the unnormalized normal vector of two lines. +/// @param ea0 The first vertex of the first line. +/// @param ea1 The second vertex of the first line. +/// @param eb0 The first vertex of the second line. +/// @param eb1 The second vertex of the second line. +/// @return The Jacobian of the unnormalized normal vector of the two lines. +inline Eigen::Matrix line_line_unnormalized_normal_jacobian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -232,13 +279,13 @@ inline Eigen::Matrix edge_edge_unnormalized_normal_jacobian( return J; } -/// @brief Computes the Jacobian of the normal vector of two edges. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The Jacobian of the normal vector of the two edges. -inline Eigen::Matrix edge_edge_normal_jacobian( +/// @brief Computes the Jacobian of the normal vector of two lines. +/// @param ea0 The first vertex of the first line. +/// @param ea1 The second vertex of the first line. +/// @param eb0 The first vertex of the second line. +/// @param eb1 The second vertex of the second line. +/// @return The Jacobian of the normal vector of the two lines. +inline Eigen::Matrix line_line_normal_jacobian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -246,32 +293,34 @@ inline Eigen::Matrix edge_edge_normal_jacobian( { // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x return normalization_jacobian( - edge_edge_unnormalized_normal(ea0, ea1, eb0, eb1)) - * edge_edge_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); + line_line_unnormalized_normal(ea0, ea1, eb0, eb1)) + * line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); } -/// @brief Computes the Hessian of the unnormalized normal vector of two edges. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The Hessian of the unnormalized normal vector of the two edges. -Eigen::Matrix edge_edge_unnormalized_normal_hessian( +/// @brief Computes the Hessian of the unnormalized normal vector of two lines. +/// @param ea0 The first vertex of the first line. +/// @param ea1 The second vertex of the first line. +/// @param eb0 The first vertex of the second line. +/// @param eb1 The second vertex of the second line. +/// @return The Hessian of the unnormalized normal vector of the two lines. +Eigen::Matrix line_line_unnormalized_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1); -/// @brief Computes the Hessian of the normal vector of two edges. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The Hessian of the normal vector of the two edges. -Eigen::Matrix edge_edge_normal_hessian( +/// @brief Computes the Hessian of the normal vector of two lines. +/// @param ea0 The first vertex of the first line. +/// @param ea1 The second vertex of the first line. +/// @param eb0 The first vertex of the second line. +/// @param eb1 The second vertex of the second line. +/// @return The Hessian of the normal vector of the two lines. +Eigen::Matrix line_line_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1); +/** @} */ + } // namespace ipc \ No newline at end of file diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index c9f3ae1a5..6fdb32e60 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -98,6 +98,56 @@ TEST_CASE("Edge-vertex collision normal", "[ev][normal]") } } +TEST_CASE("Point-line normal hessian", "[pl][normal]") +{ + const int DIM = GENERATE(2, 3); + + VectorMax3d p(DIM); + VectorMax3d e0(DIM); + VectorMax3d e1(DIM); + Eigen::VectorXd x(3 * DIM); + + const int case_i = GENERATE(range(0, 10)); + p.setRandom(); + e0.setRandom(); + e1.setRandom(); + + x << p, e0, e1; + + // Check hessian using finite differences + Eigen::MatrixXd hessian = point_line_unnormalized_normal_hessian(p, e0, e1); + Eigen::MatrixXd fd_hessian; + fd::finite_jacobian( + x, + [DIM](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { + return point_line_unnormalized_normal_jacobian( + x_fd.segment(0, DIM), x_fd.segment(DIM, DIM), + x_fd.segment(2 * DIM, DIM)); + }, + fd_hessian); + CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); + if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { + std::cout << "Hessian:\n" << hessian << std::endl; + std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + } + + // Check hessian using finite differences + hessian = point_line_normal_hessian(p, e0, e1); + fd::finite_jacobian( + x, + [DIM](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { + return point_line_normal_jacobian( + x_fd.segment(0, DIM), x_fd.segment(DIM, DIM), + x_fd.segment(2 * DIM, DIM)); + }, + fd_hessian); + CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); + if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { + std::cout << "Hessian:\n" << hessian << std::endl; + std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + } +} + TEST_CASE("Edge-edge collision normal", "[ee][normal]") { Eigen::MatrixXd V(4, 3); @@ -143,7 +193,7 @@ TEST_CASE("Edge-edge collision normal", "[ee][normal]") } } -TEST_CASE("Edge-edge normal hessian", "[ee][normal][hessian]") +TEST_CASE("Line-line normal hessian", "[ee][normal][hessian]") { Eigen::Vector3d a(0, 0, 0); Eigen::Vector3d b(1, 0, 0); @@ -162,12 +212,12 @@ TEST_CASE("Edge-edge normal hessian", "[ee][normal][hessian]") x << a, b, c, d; // Check hessian using finite differences - Eigen::MatrixXd hessian = edge_edge_unnormalized_normal_hessian(a, b, c, d); + Eigen::MatrixXd hessian = line_line_unnormalized_normal_hessian(a, b, c, d); Eigen::MatrixXd fd_hessian; fd::finite_jacobian( x, [](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { - return edge_edge_unnormalized_normal_jacobian( + return line_line_unnormalized_normal_jacobian( x_fd.segment<3>(0), x_fd.segment<3>(3), x_fd.segment<3>(6), x_fd.segment<3>(9)); }, @@ -179,11 +229,11 @@ TEST_CASE("Edge-edge normal hessian", "[ee][normal][hessian]") } // Check hessian using finite differences - hessian = edge_edge_normal_hessian(a, b, c, d); + hessian = line_line_normal_hessian(a, b, c, d); fd::finite_jacobian( x, [](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { - return edge_edge_normal_jacobian( + return line_line_normal_jacobian( x_fd.segment<3>(0), x_fd.segment<3>(3), x_fd.segment<3>(6), x_fd.segment<3>(9)); }, diff --git a/tests/src/tests/distance/CMakeLists.txt b/tests/src/tests/distance/CMakeLists.txt index 9fc0f584f..84e05e6d5 100644 --- a/tests/src/tests/distance/CMakeLists.txt +++ b/tests/src/tests/distance/CMakeLists.txt @@ -9,6 +9,7 @@ set(SOURCES test_point_plane.cpp test_point_point.cpp test_point_triangle.cpp + test_signed_distance.cpp # Benchmarks diff --git a/tests/src/tests/distance/test_signed_distance.cpp b/tests/src/tests/distance/test_signed_distance.cpp new file mode 100644 index 000000000..01098977d --- /dev/null +++ b/tests/src/tests/distance/test_signed_distance.cpp @@ -0,0 +1,170 @@ +#include +#include +#include + +#include +#include +#include + +#include +#include + +using namespace ipc; + +TEST_CASE( + "Signed distance point-plane derivatives", "[normal][gradient][hessian]") +{ + Eigen::Vector3d p(0.25, 0.25, 1.0); + Eigen::Vector3d a(0, 0, 0); + Eigen::Vector3d b(1, 0, 0); + Eigen::Vector3d c(0, 1, 0); + Eigen::VectorXd x(12); + + const int case_i = GENERATE(range(0, 10)); + if (case_i > 0) { + p.setRandom(); + a.setRandom(); + b.setRandom(); + c.setRandom(); + } + + x << p, a, b, c; + + // Check gradient using finite differences + Vector12d gradient = point_plane_signed_distance_gradient(p, a, b, c); + Eigen::VectorXd fd_gradient; + fd::finite_gradient( + x, + [](const Eigen::VectorXd& x_fd) -> double { + return point_plane_signed_distance( + x_fd.segment<3>(0), x_fd.segment<3>(3), x_fd.segment<3>(6), + x_fd.segment<3>(9)); + }, + fd_gradient); + CHECK(fd::compare_gradient(gradient, fd_gradient, 1e-6)); + if (!fd::compare_gradient(gradient, fd_gradient, 1e-6)) { + std::cout << "Gradient:\n" << gradient.transpose() << std::endl; + std::cout << "FD Gradient:\n" << fd_gradient.transpose() << std::endl; + } + + // Check hessian using finite differences + Matrix12d hessian = point_plane_signed_distance_hessian(p, a, b, c); + Eigen::MatrixXd fd_hessian; + fd::finite_jacobian( + x, + [](const Eigen::VectorXd& x_fd) -> Eigen::VectorXd { + return point_plane_signed_distance_gradient( + x_fd.segment<3>(0), x_fd.segment<3>(3), x_fd.segment<3>(6), + x_fd.segment<3>(9)); + }, + fd_hessian); + CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); + if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { + std::cout << "Hessian:\n" << hessian << std::endl; + std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + } +} + +TEST_CASE( + "Signed distance line-line derivatives", "[normal][gradient][hessian]") +{ + Eigen::Vector3d ea0(0.25, 0.25, 1.0); + Eigen::Vector3d ea1(0, 0, 0); + Eigen::Vector3d eb0(1, 0, 0); + Eigen::Vector3d eb1(0, 1, 0); + Eigen::VectorXd x(12); + + const int case_i = GENERATE(range(0, 10)); + while (case_i > 0 && line_line_signed_distance(ea0, ea1, eb0, eb1) == 0.0) { + ea0.setRandom(); + ea1.setRandom(); + eb0.setRandom(); + eb1.setRandom(); + } + + x << ea0, ea1, eb0, eb1; + + // Check gradient using finite differences + Vector12d gradient = line_line_signed_distance_gradient(ea0, ea1, eb0, eb1); + Eigen::VectorXd fd_gradient; + fd::finite_gradient( + x, + [](const Eigen::VectorXd& x_fd) -> double { + return line_line_signed_distance( + x_fd.segment<3>(0), x_fd.segment<3>(3), x_fd.segment<3>(6), + x_fd.segment<3>(9)); + }, + fd_gradient); + CHECK(fd::compare_gradient(gradient, fd_gradient, 1e-6)); + if (!fd::compare_gradient(gradient, fd_gradient, 1e-6)) { + std::cout << "Gradient:\n" << gradient.transpose() << std::endl; + std::cout << "FD Gradient:\n" << fd_gradient.transpose() << std::endl; + } + + // Check hessian using finite differences + Matrix12d hessian = line_line_signed_distance_hessian(ea0, ea1, eb0, eb1); + Eigen::MatrixXd fd_hessian; + fd::finite_jacobian( + x, + [](const Eigen::VectorXd& x_fd) -> Eigen::VectorXd { + return line_line_signed_distance_gradient( + x_fd.segment<3>(0), x_fd.segment<3>(3), x_fd.segment<3>(6), + x_fd.segment<3>(9)); + }, + fd_hessian); + CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); + if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { + std::cout << "Hessian:\n" << hessian << std::endl; + std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + } +} + +TEST_CASE( + "Signed distance point-line derivatives", "[normal][gradient][hessian]") +{ + Eigen::Vector2d p(0.25, 0.25); + Eigen::Vector2d a(0, 0); + Eigen::Vector2d b(1, 0); + Eigen::VectorXd x(6); + + const int case_i = GENERATE(range(0, 10)); + if (case_i > 0) { + p.setRandom(); + a.setRandom(); + b.setRandom(); + } + + x << p, a, b; + + // Check gradient using finite differences + Vector6d gradient = point_line_signed_distance_gradient(p, a, b); + Eigen::VectorXd fd_gradient; + fd::finite_gradient( + x, + [](const Eigen::VectorXd& x_fd) -> double { + return point_line_signed_distance( + x_fd.segment<2>(0), x_fd.segment<2>(2), x_fd.segment<2>(4)); + }, + fd_gradient); + CHECK(fd::compare_gradient(gradient, fd_gradient, 1e-6)); + if (!fd::compare_gradient(gradient, fd_gradient, 1e-6)) { + std::cout << "Gradient:\n" << gradient.transpose() << std::endl; + std::cout << "FD Gradient:\n" << fd_gradient.transpose() << std::endl; + } + + // Check hessian using finite differences + Matrix6d hessian = point_line_signed_distance_hessian(p, a, b); + Eigen::MatrixXd fd_hessian; + fd::finite_jacobian( + x, + [](const Eigen::VectorXd& x_fd) -> Eigen::VectorXd { + return point_line_signed_distance_gradient( + x_fd.segment<2>(0), x_fd.segment<2>(2), x_fd.segment<2>(4)); + }, + fd_hessian); + CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); + if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { + std::cout << "Hessian:\n" << hessian << std::endl; + std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + } +} \ No newline at end of file From 34c46d36fbf884b118d6606ee3c65e2c1ff77b4f Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 13 Jan 2026 08:21:12 -0500 Subject: [PATCH 2/2] Add bindings for signed distance --- python/src/bindings.cpp | 1 + python/src/distance/CMakeLists.txt | 1 + python/src/distance/bindings.hpp | 3 +- python/src/distance/signed_distance.cpp | 117 ++++++++++++++++++++++++ 4 files changed, 121 insertions(+), 1 deletion(-) create mode 100644 python/src/distance/signed_distance.cpp diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 479bc5255..25744756b 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -83,6 +83,7 @@ PYBIND11_MODULE(ipctk, m) define_point_point_distance(m); define_point_plane_distance(m); define_point_triangle_distance(m); + define_signed_distance(m); // friction define_smooth_friction_mollifier(m); diff --git a/python/src/distance/CMakeLists.txt b/python/src/distance/CMakeLists.txt index bf326309b..a8468b5a9 100644 --- a/python/src/distance/CMakeLists.txt +++ b/python/src/distance/CMakeLists.txt @@ -8,6 +8,7 @@ set(SOURCES point_plane.cpp point_point.cpp point_triangle.cpp + signed_distance.cpp ) target_sources(ipctk PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/python/src/distance/bindings.hpp b/python/src/distance/bindings.hpp index 1388f8384..50bdc304b 100644 --- a/python/src/distance/bindings.hpp +++ b/python/src/distance/bindings.hpp @@ -10,4 +10,5 @@ void define_point_edge_distance(py::module_& m); void define_point_line_distance(py::module_& m); void define_point_point_distance(py::module_& m); void define_point_plane_distance(py::module_& m); -void define_point_triangle_distance(py::module_& m); \ No newline at end of file +void define_point_triangle_distance(py::module_& m); +void define_signed_distance(py::module_& m); \ No newline at end of file diff --git a/python/src/distance/signed_distance.cpp b/python/src/distance/signed_distance.cpp new file mode 100644 index 000000000..3fe174c67 --- /dev/null +++ b/python/src/distance/signed_distance.cpp @@ -0,0 +1,117 @@ +#include + +#include +#include +#include + +using namespace ipc; + +void define_signed_distance(py::module_& m) +{ + // Point-line (2D) signed distance + m.def( + "point_line_signed_distance", point_line_signed_distance, + R"ipc_Qu8mg5v7( + Compute the signed distance from a point to a directed line segment (2D). + + Parameters: + p: The query point (2D). + e0: The first endpoint of the directed edge (2D). + e1: The second endpoint of the directed edge (2D). + + Returns: + The signed scalar distance from `p` to the (infinite) line through `e0` and `e1`. + )ipc_Qu8mg5v7", + "p"_a, "e0"_a, "e1"_a); + + m.def( + "point_line_signed_distance_gradient", + point_line_signed_distance_gradient, + R"ipc_Qu8mg5v7( + Compute the gradient of the signed point-to-line distance (2D). + + Returns a 6-vector ordered as [dp, de0, de1]. + )ipc_Qu8mg5v7", + "p"_a, "e0"_a, "e1"_a); + + m.def( + "point_line_signed_distance_hessian", + point_line_signed_distance_hessian, + R"ipc_Qu8mg5v7( + Compute the Hessian of the signed point-to-line distance (2D). + + Returns a 6x6 Hessian matrix with variables ordered as [p, e0, e1]. + )ipc_Qu8mg5v7", + "p"_a, "e0"_a, "e1"_a); + + // Point-plane (3D) signed distance + m.def( + "point_plane_signed_distance", point_plane_signed_distance, + R"ipc_Qu8mg5v7( + Compute the signed distance from a point to the plane of a triangle (3D). + + Parameters: + p: The query point (3D). + t0: First vertex of the triangle (3D). + t1: Second vertex of the triangle (3D). + t2: Third vertex of the triangle (3D). + + Returns: + The signed distance from `p` to the triangle plane. + )ipc_Qu8mg5v7", + "p"_a, "t0"_a, "t1"_a, "t2"_a); + + m.def( + "point_plane_signed_distance_gradient", + point_plane_signed_distance_gradient, + R"ipc_Qu8mg5v7( + Compute the gradient of the signed point-to-plane distance (3D). + + Returns a 12-vector ordered as [dp, dt0, dt1, dt2]. + )ipc_Qu8mg5v7", + "p"_a, "t0"_a, "t1"_a, "t2"_a); + + m.def( + "point_plane_signed_distance_hessian", + point_plane_signed_distance_hessian, + R"ipc_Qu8mg5v7( + Compute the Hessian of the signed point-to-plane distance (3D). + + Returns a 12x12 Hessian matrix with variables ordered as [p, t0, t1, t2]. + )ipc_Qu8mg5v7", + "p"_a, "t0"_a, "t1"_a, "t2"_a); + + // Line-line (3D) signed distance + m.def( + "line_line_signed_distance", line_line_signed_distance, + R"ipc_Qu8mg5v7( + Compute the signed distance between two lines in 3D. + + Parameters: + ea0, ea1: Two points on the first line. + eb0, eb1: Two points on the second line. + + Returns: + The signed distance along the common normal between the two lines. + )ipc_Qu8mg5v7", + "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); + + m.def( + "line_line_signed_distance_gradient", + line_line_signed_distance_gradient, + R"ipc_Qu8mg5v7( + Compute the gradient of the signed line-line distance (3D). + + Returns a 12-vector ordered as [d/d(ea0); d/d(ea1); d/d(eb0); d/d(eb1)]. + )ipc_Qu8mg5v7", + "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); + + m.def( + "line_line_signed_distance_hessian", line_line_signed_distance_hessian, + R"ipc_Qu8mg5v7( + Compute the Hessian of the signed line-line distance (3D). + + Returns a 12x12 Hessian matrix with variables ordered as [ea0, ea1, eb0, eb1]. + )ipc_Qu8mg5v7", + "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); +}