Collect normal utilities into geometry/normal files

src/ipc/CMakeLists.txt

@@ -19,6 +19,7 @@ add_subdirectory(ccd)
 add_subdirectory(collisions)
 add_subdirectory(distance)
 add_subdirectory(friction)
+add_subdirectory(geometry)
 add_subdirectory(implicits)
 add_subdirectory(potentials)
 add_subdirectory(smooth_contact)

src/ipc/candidates/collision_stencil.cpp

@@ -1,5 +1,7 @@
 #include "collision_stencil.hpp"
 
+#include <ipc/geometry/normal.hpp>
+
 namespace ipc {
 
 VectorMax3d CollisionStencil::compute_normal(
@@ -30,19 +32,10 @@ MatrixMax<double, 3, 12> CollisionStencil::compute_normal_jacobian(
 {
     const int dim = this->dim(positions.size());
 
-    VectorMax3d n = compute_unnormalized_normal(positions);
-
-    MatrixMax<double, 3, 12> dn =
-        compute_unnormalized_normal_jacobian(positions);
-
-    // 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;
+    const VectorMax3d n = compute_unnormalized_normal(positions);
 
-    dn = A * dn;
+    MatrixMax<double, 3, 12> dn = normalization_jacobian(n)
+        * compute_unnormalized_normal_jacobian(positions);
 
     if (flip_if_negative
         && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) {

src/ipc/candidates/edge_edge.cpp

@@ -1,6 +1,7 @@
 #include "edge_edge.hpp"
 
 #include <ipc/distance/edge_edge.hpp>
+#include <ipc/geometry/normal.hpp>
 #include <ipc/tangent/closest_point.hpp>
 
 namespace ipc {
@@ -99,25 +100,19 @@ VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal(
     Eigen::ConstRef<VectorMax12d> positions) const
 {
     assert(positions.size() == 12);
-    return (positions.segment<3>(3) - positions.head<3>())
-        .cross(positions.tail<3>() - positions.segment<3>(6));
+    return edge_edge_unnormalized_normal(
+        positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6),
+        positions.tail<3>());
 }
 
 MatrixMax<double, 3, 12>
 EdgeEdgeCandidate::compute_unnormalized_normal_jacobian(
     Eigen::ConstRef<VectorMax12d> positions) const
 {
     assert(positions.size() == 12);
-    MatrixMax<double, 3, 12> dn(3, 12);
-    dn.leftCols<3>() =
-        cross_product_matrix(positions.tail<3>() - positions.segment<3>(6));
-    dn.middleCols<3>(3) =
-        cross_product_matrix(positions.segment<3>(6) - positions.tail<3>());
-    dn.middleCols<3>(6) =
-        cross_product_matrix(positions.head<3>() - positions.segment<3>(3));
-    dn.rightCols<3>() =
-        cross_product_matrix(positions.segment<3>(3) - positions.head<3>());
-    return dn;
+    return edge_edge_unnormalized_normal_jacobian(
+        positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6),
+        positions.tail<3>());
 }
 
 bool EdgeEdgeCandidate::ccd(

src/ipc/candidates/edge_vertex.cpp

@@ -1,6 +1,7 @@
 #include "edge_vertex.hpp"
 
 #include <ipc/distance/point_edge.hpp>
+#include <ipc/geometry/normal.hpp>
 #include <ipc/tangent/closest_point.hpp>
 
 #include <iostream>
@@ -81,51 +82,17 @@ VectorMax3d EdgeVertexCandidate::compute_unnormalized_normal(
     Eigen::ConstRef<VectorMax12d> 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);
+    return edge_vertex_unnormalized_normal(
+        positions.head(dim), positions.segment(dim, dim), positions.tail(dim));
 }
 
 MatrixMax<double, 3, 12>
 EdgeVertexCandidate::compute_unnormalized_normal_jacobian(
     Eigen::ConstRef<VectorMax12d> positions) const
 {
     const int dim = this->dim(positions.size());
-    if (dim == 2) {
-        // In 2D, the normal is simply the perpendicular vector to the edge
-        MatrixMax<double, 3, 12> 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<double, 3, 12> 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;
+    return edge_vertex_unnormalized_normal_jacobian(
+        positions.head(dim), positions.segment(dim, dim), positions.tail(dim));
 }
 
 bool EdgeVertexCandidate::ccd(

src/ipc/candidates/face_vertex.cpp

@@ -1,6 +1,7 @@
 #include "face_vertex.hpp"
 
 #include <ipc/distance/point_triangle.hpp>
+#include <ipc/geometry/normal.hpp>
 #include <ipc/tangent/closest_point.hpp>
 
 #include <iostream>
@@ -94,8 +95,8 @@ VectorMax3d FaceVertexCandidate::compute_unnormalized_normal(
     Eigen::ConstRef<VectorMax12d> positions) const
 {
     assert(positions.size() == 12);
-    return (positions.segment<3>(6) - positions.segment<3>(3))
-        .cross(positions.tail<3>() - positions.segment<3>(3));
+    return triangle_unnormalized_normal(
+        positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>());
 }
 
 MatrixMax<double, 3, 12>
@@ -105,12 +106,8 @@ FaceVertexCandidate::compute_unnormalized_normal_jacobian(
     assert(positions.size() == 12);
     MatrixMax<double, 3, 12> dn(3, 12);
     dn.leftCols<3>().setZero();
-    dn.middleCols<3>(3) =
-        cross_product_matrix(positions.tail<3>() - positions.segment<3>(6));
-    dn.middleCols<3>(6) =
-        cross_product_matrix(positions.segment<3>(3) - positions.tail<3>());
-    dn.rightCols<3>() =
-        cross_product_matrix(positions.segment<3>(6) - positions.segment<3>(3));
+    dn.rightCols<9>() = triangle_unnormalized_normal_jacobian(
+        positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>());
     return dn;
 }
 

src/ipc/geometry/CMakeLists.txt

@@ -0,0 +1,10 @@
+set(SOURCES
+  normal.cpp
+  normal.hpp
+)
+
+target_sources(ipc_toolkit PRIVATE ${SOURCES})
+
+################################################################################
+# Subfolders
+################################################################################

src/ipc/geometry/normal.cpp

@@ -0,0 +1,90 @@
+#include "ipc/geometry/normal.hpp"
+
+namespace ipc {
+
+std::tuple<VectorMax3d, MatrixMax3d>
+normalization_and_jacobian(Eigen::ConstRef<VectorMax3d> x)
+{
+    const double norm = x.norm();
+    const VectorMax3d xhat = x / norm;
+    const auto I = MatrixMax3d::Identity(x.size(), x.size());
+    const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm;
+    return std::make_tuple(xhat, J);
+}
+
+std::tuple<VectorMax3d, MatrixMax3d, std::array<MatrixMax3d, 3>>
+normalization_and_jacobian_and_hessian(Eigen::ConstRef<VectorMax3d> x)
+{
+    // const auto [xhat, J] = normalization_and_jacobian(x);
+    const double norm = x.norm();
+    const VectorMax3d xhat = x / norm;
+    const auto I = MatrixMax3d::Identity(x.size(), x.size());
+    const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm;
+
+    std::array<MatrixMax3d, 3> H;
+    for (int i = 0; i < x.size(); i++) {
+        H[i] = (xhat(i) * J + xhat * J.row(i) + J.col(i) * xhat.transpose())
+            / (-norm);
+    }
+
+    return std::make_tuple(xhat, J, H);
+}
+
+// --- edge-vertex normal functions -------------------------------------------
+
+VectorMax3d edge_vertex_unnormalized_normal(
+    Eigen::ConstRef<VectorMax3d> v,
+    Eigen::ConstRef<VectorMax3d> e0,
+    Eigen::ConstRef<VectorMax3d> 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>();
+        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;
+        return d * e.dot(e) - e * e.dot(d);
+    }
+}
+
+MatrixMax<double, 3, 9> edge_vertex_unnormalized_normal_jacobian(
+    Eigen::ConstRef<VectorMax3d> v,
+    Eigen::ConstRef<VectorMax3d> e0,
+    Eigen::ConstRef<VectorMax3d> e1)
+{
+    assert(v.size() == e0.size() && v.size() == e1.size());
+    assert(v.size() == 2 || v.size() == 3);
+
+    MatrixMax<double, 3, 9> dn(v.size(), 3 * v.size());
+
+    if (v.size() == 2) {
+        // In 2D, the normal is simply the perpendicular vector to the edge
+        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 auto I = Eigen::Matrix3d::Identity();
+
+        // ∂n/∂v
+        dn.leftCols<3>() = e.dot(e) * I - e * e.transpose();
+        // ∂n/∂e1
+        dn.rightCols<3>() =
+            -e.dot(d) * I - e * d.transpose() + (2 * d) * e.transpose();
+        // ∂n/∂e0
+        dn.middleCols<3>(3) = -dn.leftCols<3>() - dn.rightCols<3>();
+    }
+
+    return dn;
+}
+
+} // namespace ipc