Fix dihedral angle calculation; add normal hessians

Author: zfergus

python/src/geometry/normal.cpp

@@ -230,6 +230,23 @@ void define_normal(py::module_& m)
         )ipc_qu8mg5v7",
         py::arg("a"), py::arg("b"), py::arg("c"));
 
+    m.def(
+        "triangle_normal_hessian", &triangle_normal_hessian,
+        R"ipc_qu8mg5v7(
+        Computes the Hessian of the normal vector of a triangle.
+
+        Parameters
+        ----------
+        a: The first vertex of the triangle.
+        b: The second vertex of the triangle.
+        c: The third vertex of the triangle.
+
+        Returns
+        -------
+        The Hessian of the normal vector of the triangle.
+        )ipc_qu8mg5v7",
+        py::arg("a"), py::arg("b"), py::arg("c"));
+
     m.def(
         "edge_edge_unnormalized_normal", &edge_edge_unnormalized_normal,
         R"ipc_qu8mg5v7(
@@ -302,4 +319,41 @@ void define_normal(py::module_& m)
         The Jacobian of the normal vector of the two edges.
         )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,
+        R"ipc_qu8mg5v7(
+        Computes the Hessian of the unnormalized normal vector of two edges.
+
+        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.
+
+        Returns
+        -------
+        The Hessian of the unnormalized normal vector of the two edges.
+        )ipc_qu8mg5v7",
+        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));
+
+    m.def(
+        "edge_edge_normal_hessian", &edge_edge_normal_hessian,
+        R"ipc_qu8mg5v7(
+        Computes the Hessian of the normal vector of two edges.
+
+        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.
+
+        Returns
+        -------
+        The Hessian of the normal vector of the two edges.
+        )ipc_qu8mg5v7",
+        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));
 }

src/ipc/geometry/angle.cpp

@@ -10,10 +10,14 @@ double dihedral_angle(
     Eigen::ConstRef<Eigen::Vector3d> x2,
     Eigen::ConstRef<Eigen::Vector3d> x3)
 {
-    double cos_theta =
-        triangle_normal(x0, x1, x2).dot(triangle_normal(x1, x0, x3));
-    assert(std::abs(cos_theta) <= 1.0 + 1e-6); // Numerical tolerance
-    return std::acos(std::clamp(cos_theta, -1.0, 1.0));
+    const Eigen::Vector3d n0 = triangle_normal(x0, x1, x2);
+    const Eigen::Vector3d n1 = triangle_normal(x1, x0, x3);
+    const Eigen::Vector3d e = (x1 - x0).normalized();
+
+    const double sin_theta = n0.cross(n1).dot(e);
+    const double cos_theta = n0.dot(n1);
+
+    return std::atan2(sin_theta, cos_theta);
 }
 
 Eigen::Vector<double, 12> dihedral_angle_gradient(
@@ -24,33 +28,42 @@ Eigen::Vector<double, 12> dihedral_angle_gradient(
 {
     const Eigen::Vector3d n0 = triangle_normal(x0, x1, x2);
     const Eigen::Vector3d n1 = triangle_normal(x1, x0, x3);
+    const Eigen::Vector3d e = (x1 - x0).normalized();
+
+    // --- Normal gradients ---
+
+    Eigen::Matrix<double, 3, 12> dn0_dx;
+    dn0_dx.leftCols<9>() = triangle_normal_jacobian(x0, x1, x2);
+    dn0_dx.rightCols<3>().setZero();
+
+    Eigen::Matrix<double, 3, 12> dn1_dx;
+    const std::array<int, 9> idx = { { 3, 4, 5, 0, 1, 2, 9, 10, 11 } };
+    dn1_dx(Eigen::all, idx) = triangle_normal_jacobian(x1, x0, x3);
+    dn1_dx.middleCols<3>(6).setZero();
+
+    // --- Edge gradient ---
+
+    Eigen::Matrix<double, 3, 12> de_dx;
+    de_dx.middleCols<3>(3) = normalization_jacobian(x1 - x0);
+    de_dx.leftCols<3>() = -de_dx.middleCols<3>(3);
+    de_dx.rightCols<6>().setZero();
+
+    // --- Angle gradient ---
+
+    const Eigen::Vector<double, 12> dcos_dx =
+        dn0_dx.transpose() * n1 + dn1_dx.transpose() * n0;
+
+    const Eigen::Vector<double, 12> dsin_dx =
+        (cross_product_matrix(n0) * dn1_dx - cross_product_matrix(n1) * dn0_dx)
+            .transpose()
+        * e;
+
+    // --- Product rule ---
 
-    const Eigen::Matrix<double, 3, 9> dn0_dx =
-        triangle_normal_jacobian(x0, x1, x2);
-    const Eigen::Matrix<double, 3, 9> dn1_dx =
-        triangle_normal_jacobian(x1, x0, x3);
-
-    const Eigen::Matrix3d dn0_dx0 = dn0_dx.block<3, 3>(0, 0);
-    const Eigen::Matrix3d dn0_dx1 = dn0_dx.block<3, 3>(0, 3);
-    const Eigen::Matrix3d dn0_dx2 = dn0_dx.block<3, 3>(0, 6);
-    const Eigen::Matrix3d dn1_dx0 = dn1_dx.block<3, 3>(0, 3);
-    const Eigen::Matrix3d dn1_dx1 = dn1_dx.block<3, 3>(0, 0);
-    const Eigen::Matrix3d dn1_dx3 = dn1_dx.block<3, 3>(0, 6);
-
-    Eigen::Vector<double, 12> gradient;
-    // Product rule
-    gradient.segment<3>(0) =
-        dn0_dx0.transpose() * n1 + dn1_dx0.transpose() * n0;
-    gradient.segment<3>(3) =
-        dn0_dx1.transpose() * n1 + dn1_dx1.transpose() * n0;
-    gradient.segment<3>(6) = dn0_dx2.transpose() * n1;
-    gradient.segment<3>(9) = dn1_dx3.transpose() * n0;
-
-    // Chain rule through acos
+    const double sin_theta = n0.cross(n1).dot(e);
     const double cos_theta = n0.dot(n1);
-    gradient *= -1 / sqrt(1.0 - cos_theta * cos_theta);
 
-    return gradient;
+    return dsin_dx * cos_theta - dcos_dx * sin_theta;
 }
 
 } // namespace ipc

src/ipc/geometry/normal.cpp

@@ -127,4 +127,102 @@ Eigen::Matrix<double, 3, 81> triangle_unnormalized_normal_hessian(
     return H.reshaped(3, 81);
 }
 
+Eigen::Matrix<double, 3, 81> triangle_normal_hessian(
+    Eigen::ConstRef<Eigen::Vector3d> a,
+    Eigen::ConstRef<Eigen::Vector3d> b,
+    Eigen::ConstRef<Eigen::Vector3d> c)
+{
+    const Eigen::Vector3d z = triangle_unnormalized_normal(a, b, c);
+    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 = triangle_unnormalized_normal_jacobian(a, b, c);
+    const auto d2z_dx2 = triangle_unnormalized_normal_hessian(a, b, c);
+
+    Eigen::Matrix<double, 3, 81> d2n_dx2;
+    for (int k = 0; k < 81; ++k) {
+        const int i = k / 9, j = k % 9;
+
+        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;
+}
+
+// --- edge-edge normal functions ---------------------------------------------
+
+Eigen::Matrix<double, 3, 144> edge_edge_unnormalized_normal_hessian(
+    Eigen::ConstRef<Eigen::Vector3d> ea0,
+    Eigen::ConstRef<Eigen::Vector3d> ea1,
+    Eigen::ConstRef<Eigen::Vector3d> eb0,
+    Eigen::ConstRef<Eigen::Vector3d> eb1)
+{
+    Eigen::Matrix<double, 36, 12> H = Eigen::Matrix<double, 36, 12>::Zero();
+
+    // ∂²n/∂a² = 0
+    // ∂²n/∂a∂b = 0
+    set_cross_product_matrix_jacobian(H.block<9, 3>(0, 6), -1.0); // ∂²n/∂a∂c
+    set_cross_product_matrix_jacobian(H.block<9, 3>(0, 9), 1.0);  // ∂²n/∂a∂d
+    /**/
+    // ∂²n/∂b∂a = 0
+    // ∂²n/∂b² = 0
+    set_cross_product_matrix_jacobian(H.block<9, 3>(9, 6), 1.0);  // ∂²n/∂b∂c
+    set_cross_product_matrix_jacobian(H.block<9, 3>(9, 9), -1.0); // ∂²n/∂b∂d
+    /**/
+    set_cross_product_matrix_jacobian(H.block<9, 3>(18, 0), 1.0);  // ∂²n/∂c∂a
+    set_cross_product_matrix_jacobian(H.block<9, 3>(18, 3), -1.0); // ∂²n/∂c∂b
+    // ∂²n/∂c² = 0
+    // ∂²n/∂d² = 0
+    /**/
+    set_cross_product_matrix_jacobian(H.block<9, 3>(27, 0), -1.0); // ∂²n/∂d∂a
+    set_cross_product_matrix_jacobian(H.block<9, 3>(27, 3), 1.0);  // ∂²n/∂d∂b
+    // ∂²n/∂d∂c = 0
+    // ∂²n/∂d² = 0
+
+    return H.reshaped(3, 144);
+}
+
+Eigen::Matrix<double, 3, 144> edge_edge_normal_hessian(
+    Eigen::ConstRef<Eigen::Vector3d> ea0,
+    Eigen::ConstRef<Eigen::Vector3d> ea1,
+    Eigen::ConstRef<Eigen::Vector3d> eb0,
+    Eigen::ConstRef<Eigen::Vector3d> eb1)
+{
+    const Eigen::Vector3d z = edge_edge_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);
+    const auto d2z_dx2 =
+        edge_edge_unnormalized_normal_hessian(ea0, ea1, eb0, eb1);
+
+    Eigen::Matrix<double, 3, 144> d2n_dx2;
+    for (int k = 0; k < 144; ++k) {
+        const int i = k / 12, j = k % 12;
+
+        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;
+}
+
 } // namespace ipc

src/ipc/geometry/normal.hpp

@@ -164,6 +164,16 @@ inline Eigen::Matrix<double, 3, 9> triangle_normal_jacobian(
         * triangle_unnormalized_normal_jacobian(a, b, c);
 }
 
+/// @brief Computes the Hessian of the normal vector of a triangle.
+/// @param a The first vertex of the triangle.
+/// @param b The second vertex of the triangle.
+/// @param c The third vertex of the triangle.
+/// @return The Hessian of the normal vector of the triangle.
+Eigen::Matrix<double, 3, 81> triangle_normal_hessian(
+    Eigen::ConstRef<Eigen::Vector3d> a,
+    Eigen::ConstRef<Eigen::Vector3d> b,
+    Eigen::ConstRef<Eigen::Vector3d> c);
+
 /** @} */
 
 /**
@@ -240,4 +250,28 @@ inline Eigen::Matrix<double, 3, 12> edge_edge_normal_jacobian(
         * edge_edge_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<double, 3, 144> edge_edge_unnormalized_normal_hessian(
+    Eigen::ConstRef<Eigen::Vector3d> ea0,
+    Eigen::ConstRef<Eigen::Vector3d> ea1,
+    Eigen::ConstRef<Eigen::Vector3d> eb0,
+    Eigen::ConstRef<Eigen::Vector3d> 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<double, 3, 144> edge_edge_normal_hessian(
+    Eigen::ConstRef<Eigen::Vector3d> ea0,
+    Eigen::ConstRef<Eigen::Vector3d> ea1,
+    Eigen::ConstRef<Eigen::Vector3d> eb0,
+    Eigen::ConstRef<Eigen::Vector3d> eb1);
+
 } // namespace ipc