Add normal computation and Jacobian methods for collision candidates

src/ipc/candidates/collision_stencil.cpp

@@ -2,6 +2,58 @@
 
 namespace ipc {
 
+VectorMax3d CollisionStencil::compute_normal(
+    Eigen::ConstRef<VectorMax12d> positions,
+    bool flip_if_negative,
+    double* sign) const
+{
+    const int dim = this->dim(positions.size());
+
+    VectorMax3d n = compute_unnormalized_normal(positions).normalized();
+
+    if (sign != nullptr) {
+        *sign = (positions.head(dim) - positions.tail(dim)).dot(n) < 0 ? -1 : 1;
+    }
+
+    // Flip the normal if the point is on the negative side.
+    // Any point on the second object will do, so we use the last point.
+    if (flip_if_negative
+        && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) {
+        n *= -1;
+    }
+
+    return n;
+}
+
+MatrixMax<double, 3, 12> CollisionStencil::compute_normal_jacobian(
+    Eigen::ConstRef<VectorMax12d> positions, bool flip_if_negative) const
+{
+    const int dim = this->dim(positions.size());
+
+    VectorMax3d n = compute_unnormalized_normal(positions);
+
+    MatrixMax<double, 3, 12> dn =
+        compute_unnormalized_normal_jacobian(positions);
+
+#if true
+    // Derivative of normalization (n̂ = n / ‖n‖)
+    const double n_norm = n.norm();
+    n /= n_norm; // n̂
+
+    MatrixMax3d A =
+        (MatrixMax3d::Identity(dim, dim) - n * n.transpose()) / n_norm;
+
+    dn = A * dn;
+#endif
+
+    if (flip_if_negative
+        && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) {
+        dn *= -1;
+    }
+
+    return dn;
+}
+
 std::ostream& CollisionStencil::write_ccd_query(
     std::ostream& out,
     Eigen::ConstRef<VectorMax12d> vertices_t0,

src/ipc/candidates/collision_stencil.hpp

@@ -132,6 +132,40 @@ class CollisionStencil {
         return compute_coefficients(dof(vertices, edges, faces));
     }
 
+    /// @brief Compute the normal of the stencil.
+    /// @param vertices Collision mesh vertices
+    /// @param edges Collision mesh edges
+    /// @param faces Collision mesh faces
+    /// @param flip_if_negative If true, flip the normal if the point is on the negative side.
+    /// @param sign If not nullptr, set to the sign of the normal before any flipping.
+    /// @return Normal of the stencil.
+    VectorMax3d compute_normal(
+        Eigen::ConstRef<Eigen::MatrixXd> vertices,
+        Eigen::ConstRef<Eigen::MatrixXi> edges,
+        Eigen::ConstRef<Eigen::MatrixXi> faces,
+        const bool flip_if_negative = true,
+        double* sign = nullptr) const
+    {
+        return compute_normal(
+            dof(vertices, edges, faces), flip_if_negative, sign);
+    }
+
+    /// @brief Compute the Jacobian of the normal of the stencil.
+    /// @param vertices Collision mesh vertices
+    /// @param edges Collision mesh edges
+    /// @param faces Collision mesh faces
+    /// @param flip_if_negative If true, flip the normal if the point is on the negative side.
+    /// @return Jacobian of the normal of the stencil.
+    MatrixMax<double, 3, 12> compute_normal_jacobian(
+        Eigen::ConstRef<Eigen::MatrixXd> vertices,
+        Eigen::ConstRef<Eigen::MatrixXi> edges,
+        Eigen::ConstRef<Eigen::MatrixXi> faces,
+        const bool flip_if_negative = true) const
+    {
+        return compute_normal_jacobian(
+            dof(vertices, edges, faces), flip_if_negative);
+    }
+
     // ----------------------------------------------------------------------
     // NOTE: The following functions take stencil vertices as output by dof()
     // ----------------------------------------------------------------------
@@ -163,6 +197,24 @@ class CollisionStencil {
     virtual VectorMax4d
     compute_coefficients(Eigen::ConstRef<VectorMax12d> positions) const = 0;
 
+    /// @brief Compute the normal of the stencil.
+    /// @param positions Stencil's vertex positions.
+    /// @param flip_if_negative If true, flip the normal if the point is on the negative side.
+    /// @param sign If not nullptr, set to the sign of the normal before any flipping.
+    /// @return Normal of the stencil.
+    VectorMax3d compute_normal(
+        Eigen::ConstRef<VectorMax12d> positions,
+        bool flip_if_negative = true,
+        double* sign = nullptr) const;
+
+    /// @brief Compute the Jacobian of the normal of the stencil.
+    /// @param positions Stencil's vertex positions.
+    /// @param flip_if_negative If true, flip the normal if the point is on the negative side.
+    /// @return Jacobian of the normal of the stencil.
+    MatrixMax<double, 3, 12> compute_normal_jacobian(
+        Eigen::ConstRef<VectorMax12d> positions,
+        bool flip_if_negative = true) const;
+
     /// @brief Perform narrow-phase CCD on the candidate.
     /// @param[in] vertices_t0 Stencil vertices at the start of the time step.
     /// @param[in] vertices_t1 Stencil vertices at the end of the time step.
@@ -189,6 +241,19 @@ class CollisionStencil {
         std::ostream& out,
         Eigen::ConstRef<VectorMax12d> vertices_t0,
         Eigen::ConstRef<VectorMax12d> vertices_t1) const;
+
+protected:
+    /// @brief Compute the unnormalized normal of the stencil.
+    /// @param positions Stencil's vertex positions.
+    /// @return Unnormalized normal of the stencil.
+    virtual VectorMax3d compute_unnormalized_normal(
+        Eigen::ConstRef<VectorMax12d> positions) const = 0;
+
+    /// @brief Compute the Jacobian of the unnormalized normal of the stencil.
+    /// @param positions Stencil's vertex positions.
+    /// @return Jacobian of the unnormalized normal of the stencil.
+    virtual MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
+        Eigen::ConstRef<VectorMax12d> positions) const = 0;
 };
 
 } // namespace ipc

src/ipc/candidates/edge_edge.cpp

@@ -95,6 +95,44 @@ VectorMax4d EdgeEdgeCandidate::compute_coefficients(
     return coeffs;
 }
 
+VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    assert(positions.size() == 12);
+    const Eigen::Vector3d n =
+        (positions.segment<3>(3) - positions.head<3>())
+            .cross(positions.tail<3>() - positions.segment<3>(6));
+    assert(n.norm() > 0.0);
+    return n;
+}
+
+namespace {
+    inline Eigen::Matrix3d cross_mat(Eigen::ConstRef<Eigen::Vector3d> v)
+    {
+        Eigen::Matrix3d m;
+        m << 0, -v(2), v(1), //
+            v(2), 0, -v(0),  //
+            -v(1), v(0), 0;
+        return m;
+    }
+} // namespace
+
+MatrixMax<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_mat(positions.tail<3>() - positions.segment<3>(6));
+    dn.middleCols<3>(3) =
+        cross_mat(positions.segment<3>(6) - positions.tail<3>());
+    dn.middleCols<3>(6) =
+        cross_mat(positions.head<3>() - positions.segment<3>(3));
+    dn.rightCols<3>() =
+        cross_mat(positions.segment<3>(3) - positions.head<3>());
+    return dn;
+}
+
 bool EdgeEdgeCandidate::ccd(
     Eigen::ConstRef<VectorMax12d> vertices_t0,
     Eigen::ConstRef<VectorMax12d> vertices_t1,

src/ipc/candidates/edge_edge.hpp

@@ -83,6 +83,13 @@ class EdgeEdgeCandidate : virtual public CollisionStencil {
     index_t edge0_id;
     /// @brief ID of the second edge.
     index_t edge1_id;
+
+protected:
+    VectorMax3d compute_unnormalized_normal(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
+
+    MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
 };
 
 } // namespace ipc

src/ipc/candidates/edge_vertex.cpp

@@ -77,6 +77,59 @@ VectorMax4d EdgeVertexCandidate::compute_coefficients(
     return coeffs;
 }
 
+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);
+}
+
+MatrixMax<double, 3, 12>
+EdgeVertexCandidate::compute_unnormalized_normal_jacobian(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    const int dim = this->dim(positions.size());
+    assert(dim == 3);
+
+    if (dim == 2) {
+        // In 2D, the normal is simply the perpendicular vector to the edge
+        MatrixMax<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;
+}
+
 bool EdgeVertexCandidate::ccd(
     Eigen::ConstRef<VectorMax12d> vertices_t0,
     Eigen::ConstRef<VectorMax12d> vertices_t1,

src/ipc/candidates/edge_vertex.hpp

@@ -80,6 +80,13 @@ class EdgeVertexCandidate : virtual public CollisionStencil {
     index_t edge_id;
     /// @brief ID of the vertex
     index_t vertex_id;
+
+protected:
+    VectorMax3d compute_unnormalized_normal(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
+
+    MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
 };
 
 } // namespace ipc

src/ipc/candidates/face_vertex.cpp

@@ -90,6 +90,41 @@ VectorMax4d FaceVertexCandidate::compute_coefficients(
     return coeffs;
 }
 
+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));
+}
+
+namespace {
+    inline Eigen::Matrix3d cross_mat(const Eigen::Vector3d& v)
+    {
+        Eigen::Matrix3d m;
+        m << 0, -v(2), v(1), //
+            v(2), 0, -v(0),  //
+            -v(1), v(0), 0;
+        return m;
+    }
+} // namespace
+
+MatrixMax<double, 3, 12>
+FaceVertexCandidate::compute_unnormalized_normal_jacobian(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    assert(positions.size() == 12);
+    MatrixMax<double, 3, 12> dn(3, 12);
+    dn.leftCols<3>().setZero();
+    dn.middleCols<3>(3) =
+        cross_mat(positions.tail<3>() - positions.segment<3>(6));
+    dn.middleCols<3>(6) =
+        cross_mat(positions.segment<3>(3) - positions.tail<3>());
+    dn.rightCols<3>() =
+        cross_mat(positions.segment<3>(6) - positions.segment<3>(3));
+    return dn;
+}
+
 bool FaceVertexCandidate::ccd(
     Eigen::ConstRef<VectorMax12d> vertices_t0,
     Eigen::ConstRef<VectorMax12d> vertices_t1,

src/ipc/candidates/face_vertex.hpp

@@ -83,6 +83,13 @@ class FaceVertexCandidate : virtual public CollisionStencil {
     index_t face_id;
     /// @brief ID of the vertex
     index_t vertex_id;
+
+protected:
+    VectorMax3d compute_unnormalized_normal(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
+
+    MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
 };
 
 } // namespace ipc

src/ipc/candidates/vertex_vertex.cpp

@@ -47,6 +47,24 @@ VectorMax4d VertexVertexCandidate::compute_coefficients(
     return coeffs;
 }
 
+VectorMax3d VertexVertexCandidate::compute_unnormalized_normal(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    const int dim = this->dim(positions.size());
+    return positions.head(dim) - positions.tail(dim);
+}
+
+MatrixMax<double, 3, 12>
+VertexVertexCandidate::compute_unnormalized_normal_jacobian(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    const int dim = this->dim(positions.size());
+    MatrixMax<double, 3, 12> dn(dim, positions.size());
+    dn.leftCols(dim) = MatrixMax3d::Identity(dim, dim);
+    dn.rightCols(dim) = -MatrixMax3d::Identity(dim, dim);
+    return dn;
+}
+
 bool VertexVertexCandidate::ccd(
     Eigen::ConstRef<VectorMax12d> vertices_t0,
     Eigen::ConstRef<VectorMax12d> vertices_t1,

src/ipc/candidates/vertex_vertex.hpp

@@ -77,6 +77,13 @@ class VertexVertexCandidate : virtual public CollisionStencil {
     index_t vertex0_id;
     /// @brief ID of the second vertex
     index_t vertex1_id;
+
+protected:
+    VectorMax3d compute_unnormalized_normal(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
+
+    MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
+        Eigen::ConstRef<VectorMax12d> positions) const override;
 };
 
 } // namespace ipc

src/ipc/collisions/normal/plane_vertex.cpp

@@ -44,6 +44,19 @@ VectorMax4d PlaneVertexNormalCollision::compute_coefficients(
     return coeffs;
 }
 
+VectorMax3d PlaneVertexNormalCollision::compute_unnormalized_normal(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    return plane_normal;
+}
+
+MatrixMax<double, 3, 12>
+PlaneVertexNormalCollision::compute_unnormalized_normal_jacobian(
+    Eigen::ConstRef<VectorMax12d> positions) const
+{
+    return MatrixMax<double, 3, 12>::Zero(positions.size(), positions.size());
+}
+
 bool PlaneVertexNormalCollision::ccd(
     Eigen::ConstRef<VectorMax12d> vertices_t0,
     Eigen::ConstRef<VectorMax12d> vertices_t1,