diff --git a/python/src/candidates/collision_stencil.cpp b/python/src/candidates/collision_stencil.cpp index bb97c92d6..33612e86c 100644 --- a/python/src/candidates/collision_stencil.cpp +++ b/python/src/candidates/collision_stencil.cpp @@ -255,5 +255,138 @@ void define_collision_stencil(py::module_& m) vertices_t0: Stencil vertices at the start of the time step. vertices_t1: Stencil vertices at the end of the time step. )ipc_Qu8mg5v7", - "vertices_t0"_a, "vertices_t1"_a); + "vertices_t0"_a, "vertices_t1"_a) + .def( + "compute_distance_vector", + py::overload_cast>( + &CollisionStencil::compute_distance_vector, py::const_), + R"ipc_Qu8mg5v7( + Compute the distance vector of the stencil: t = sum(c_i * x_i). + + The distance vector is the vector between the closest points on the + collision primitives. Its squared norm equals the squared distance. + + Note: + positions can be computed as stencil.dof(vertices, edges, faces) + + Parameters: + positions: Stencil's vertex positions. + + Returns: + The distance vector (dim-dimensional, i.e., 2D or 3D). + )ipc_Qu8mg5v7", + "positions"_a) + .def( + "compute_distance_vector_with_coefficients", + [](const CollisionStencil& self, + Eigen::ConstRef positions) { + VectorMax4d coeffs; + VectorMax3d dv = + self.compute_distance_vector(positions, coeffs); + return std::make_tuple(dv, coeffs); + }, + R"ipc_Qu8mg5v7( + Compute the distance vector and the coefficients together. + + Note: + positions can be computed as stencil.dof(vertices, edges, faces) + + Parameters: + positions: Stencil's vertex positions. + + Returns: + Tuple of: + The distance vector (dim-dimensional). + The computed coefficients c_i. + )ipc_Qu8mg5v7", + "positions"_a) + .def( + "compute_distance_vector", + py::overload_cast< + Eigen::ConstRef, + Eigen::ConstRef, + Eigen::ConstRef>( + &CollisionStencil::compute_distance_vector, py::const_), + R"ipc_Qu8mg5v7( + Compute the distance vector of the stencil. + + Parameters: + vertices: Collision mesh vertices. + edges: Collision mesh edges. + faces: Collision mesh faces. + + Returns: + The distance vector (dim-dimensional). + )ipc_Qu8mg5v7", + "vertices"_a, "edges"_a, "faces"_a) + .def( + "compute_distance_vector_jacobian", + &CollisionStencil::compute_distance_vector_jacobian, + R"ipc_Qu8mg5v7( + Compute the Jacobian of the distance vector w.r.t. positions. + + J = [c_0 I, c_1 I, ..., c_n I]^T where I is the dim x dim identity. + + Note: + positions can be computed as stencil.dof(vertices, edges, faces) + + Parameters: + positions: Stencil's vertex positions. + + Returns: + The Jacobian dt/dx as a matrix of shape (ndof, dim). + )ipc_Qu8mg5v7", + "positions"_a) + .def_static( + "diag_distance_vector_outer", + &CollisionStencil::diag_distance_vector_outer, + R"ipc_Qu8mg5v7( + Compute diag((dt/dx)(dt/dx)^T) efficiently (Eq. 11). + + Result is [c_0^2, c_0^2, c_0^2, c_1^2, ...] (each c_i^2 repeated + dim times). + + Parameters: + coeffs: The coefficients c_i (from compute_coefficients). + dim: The spatial dimension (2 or 3). + + Returns: + The diagonal of (dt/dx)(dt/dx)^T as a vector of size ndof. + )ipc_Qu8mg5v7", + "coeffs"_a, "dim"_a) + .def_static( + "diag_distance_vector_t_outer", + &CollisionStencil::diag_distance_vector_t_outer, + R"ipc_Qu8mg5v7( + Compute diag((dt/dx * t)(dt/dx * t)^T) efficiently (Eq. 12). + + Result is element-wise square of [c_0*t^T, c_1*t^T, ..., c_n*t^T]. + + Parameters: + coeffs: The coefficients c_i. + distance_vector: The distance vector t. + + Returns: + The diagonal of (dt/dx*t)(dt/dx*t)^T as a vector of size ndof. + )ipc_Qu8mg5v7", + "coeffs"_a, "distance_vector"_a) + .def_static( + "contract_distance_vector_jacobian", + &CollisionStencil::contract_distance_vector_jacobian, + R"ipc_Qu8mg5v7( + Compute p^T (dt/dx) efficiently as sum(c_i * p_i) (Eqs. 13-14). + + Given p = [p_0, p_1, ..., p_n]^T where p_i are dim-dimensional, + this computes p^T (dt/dx) = sum(c_i * p_i) which is a + dim-dimensional vector. + + Parameters: + coeffs: The coefficients c_i. + p: A vector of size ndof (the direction for the quadratic form). + dim: The spatial dimension (2 or 3). + + Returns: + p^T (dt/dx) as a dim-dimensional vector. + )ipc_Qu8mg5v7", + "coeffs"_a, "p"_a, "dim"_a); } diff --git a/python/src/potentials/normal_potential.cpp b/python/src/potentials/normal_potential.cpp index 2d5189f5b..327185b3d 100644 --- a/python/src/potentials/normal_potential.cpp +++ b/python/src/potentials/normal_potential.cpp @@ -82,5 +82,110 @@ void define_normal_potential(py::module_& m) Returns: The gradient of the force. )ipc_Qu8mg5v7", - "distance_squared"_a, "distance_squared_gradient"_a, "dmin"_a); + "distance_squared"_a, "distance_squared_gradient"_a, "dmin"_a) + .def( + "gauss_newton_hessian_diagonal", + py::overload_cast< + const NormalCollisions&, const CollisionMesh&, + Eigen::ConstRef>( + &NormalPotential::gauss_newton_hessian_diagonal, py::const_), + R"ipc_Qu8mg5v7( + Compute the diagonal of the cumulative Gauss-Newton Hessian of the potential. + + Uses the distance-vector formulation to efficiently compute the + diagonal of the Gauss-Newton Hessian without forming full local + 12x12 Hessian matrices. This is useful as a Jacobi preconditioner + for iterative solvers. + + Note: + This is a Gauss-Newton approximation (drops derivatives of + closest-point coefficients), not the exact Hessian. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The diagonal of the Gauss-Newton Hessian as a vector of size vertices.size(). + )ipc_Qu8mg5v7", + "collisions"_a, "mesh"_a, "vertices"_a) + .def( + "gauss_newton_hessian_diagonal", + py::overload_cast< + const NormalCollision&, Eigen::ConstRef>( + &NormalPotential::gauss_newton_hessian_diagonal, py::const_), + R"ipc_Qu8mg5v7( + Compute the diagonal of the Gauss-Newton Hessian for a single collision. + + Uses the distance-vector formulation (Eqs. 10-12) to efficiently + compute the diagonal without forming the full local Hessian. + + Note: + This is a Gauss-Newton approximation, not the exact Hessian diagonal. + + Parameters: + collision: The collision. + positions: The collision stencil's positions. + + Returns: + The diagonal of the Gauss-Newton Hessian as a vector of size ndof. + )ipc_Qu8mg5v7", + "collision"_a, "positions"_a) + .def( + "gauss_newton_hessian_quadratic_form", + py::overload_cast< + const NormalCollisions&, const CollisionMesh&, + Eigen::ConstRef, + Eigen::ConstRef>( + &NormalPotential::gauss_newton_hessian_quadratic_form, + py::const_), + R"ipc_Qu8mg5v7( + Compute the product p^T H p for the cumulative Gauss-Newton Hessian. + + Uses the distance-vector formulation to efficiently compute the + quadratic form without forming full local 12x12 Hessian matrices + nor the global sparse Hessian. This is useful for nonlinear + conjugate gradient methods. + + Note: + This is a Gauss-Newton approximation (drops derivatives of + closest-point coefficients), not the exact Hessian. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + p: The direction vector of size vertices.size(). + + Returns: + The scalar value p^T H p (approximate). + )ipc_Qu8mg5v7", + "collisions"_a, "mesh"_a, "vertices"_a, "p"_a) + .def( + "gauss_newton_hessian_quadratic_form", + py::overload_cast< + const NormalCollision&, Eigen::ConstRef, + Eigen::ConstRef>( + &NormalPotential::gauss_newton_hessian_quadratic_form, + py::const_), + R"ipc_Qu8mg5v7( + Compute p^T H p for a single collision using the Gauss-Newton Hessian. + + Uses the distance-vector formulation (Eqs. 10, 13-14) to + efficiently compute the quadratic form without forming the full + local Hessian. + + Note: + This is a Gauss-Newton approximation, not the exact Hessian. + + Parameters: + collision: The collision. + positions: The collision stencil's positions. + p: The local direction vector (size ndof). + + Returns: + The scalar value p^T H p (approximate). + )ipc_Qu8mg5v7", + "collision"_a, "positions"_a, "p"_a); } diff --git a/src/ipc/candidates/collision_stencil.cpp b/src/ipc/candidates/collision_stencil.cpp index ff2cceef9..c41c41b27 100644 --- a/src/ipc/candidates/collision_stencil.cpp +++ b/src/ipc/candidates/collision_stencil.cpp @@ -4,6 +4,82 @@ namespace ipc { +VectorMax3d CollisionStencil::compute_distance_vector( + Eigen::ConstRef positions) const +{ + VectorMax4d _; // Unused output + return compute_distance_vector(positions, _); +} + +VectorMax3d CollisionStencil::compute_distance_vector( + Eigen::ConstRef positions, VectorMax4d& coeffs) const +{ + const int n = num_vertices(); + const int d = dim(positions.size()); + coeffs = compute_coefficients(positions); + VectorMax3d dv = VectorMax3d::Zero(d); + for (int i = 0; i < n; i++) { + dv += coeffs[i] * positions.segment(d * i, d); + } + return dv; +} + +MatrixMax CollisionStencil::compute_distance_vector_jacobian( + Eigen::ConstRef positions) const +{ + const int n = num_vertices(); + const int d = dim(positions.size()); + const int ndof = n * d; + const VectorMax4d c = compute_coefficients(positions); + // ∂t/∂x is ndof × dim + // Each block row i is cᵢ * I_{d×d} + MatrixMax J; + J.setZero(ndof, d); + for (int i = 0; i < n; i++) { + J.block(i * d, 0, d, d).diagonal().array() = c[i]; + } + return J; +} + +VectorMax12d CollisionStencil::diag_distance_vector_outer( + Eigen::ConstRef coeffs, const int d) +{ + const int n = coeffs.size(); + VectorMax12d diag(n * d); + for (int i = 0; i < n; i++) { + const double ci2 = coeffs[i] * coeffs[i]; + diag.segment(i * d, d).setConstant(ci2); + } + return diag; +} + +VectorMax12d CollisionStencil::diag_distance_vector_t_outer( + Eigen::ConstRef coeffs, + Eigen::ConstRef distance_vector) +{ + const int n = coeffs.size(); + const int d = distance_vector.size(); + const VectorMax3d t2 = distance_vector.array().square(); + VectorMax12d diag(n * d); + for (int i = 0; i < n; i++) { + diag.segment(i * d, d).array() = (coeffs[i] * coeffs[i]) * t2; + } + return diag; +} + +VectorMax3d CollisionStencil::contract_distance_vector_jacobian( + Eigen::ConstRef coeffs, + Eigen::ConstRef p, + const int d) +{ + const int n = coeffs.size(); + VectorMax3d result = VectorMax3d::Zero(d); + for (int i = 0; i < n; i++) { + result += coeffs[i] * p.segment(d * i, d); + } + return result; +} + VectorMax3d CollisionStencil::compute_normal( Eigen::ConstRef positions, bool flip_if_negative, diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index 18b00f121..aca2156a6 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -160,6 +160,19 @@ class CollisionStencil { return compute_coefficients(dof(vertices, edges, faces)); } + /// @brief Compute the distance vector using the mesh vertices. + /// @param vertices Collision mesh vertices. + /// @param edges Collision mesh edges. + /// @param faces Collision mesh faces. + /// @return The distance vector (dim-dimensional). + VectorMax3d compute_distance_vector( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return compute_distance_vector(dof(vertices, edges, faces)); + } + /// @brief Compute the normal of the stencil. /// @param vertices Collision mesh vertices /// @param edges Collision mesh edges @@ -225,6 +238,89 @@ class CollisionStencil { virtual VectorMax4d compute_coefficients(Eigen::ConstRef positions) const = 0; + // ------------------------------------------------------------------ + // Efficient distance-vector based methods [Shen et al. 2024] + // + // The distance vector t = ∑ cᵢ xᵢ is the vector between the + // closest points, where cᵢ are the coefficients and xᵢ are vertex + // positions. Then d = ‖t‖ and d² = tᵀt. + // + // ∂t/∂x = [c₀ I, c₁ I, c₂ I, c₃ I]ᵀ ∈ ℝ^{n×dim} + // ∂²t/∂x² = 0 + // + // These allow efficient computation of: + // - diag(∂²b/∂x²) without forming the full Hessian + // - pᵀ(∂²b/∂x²)p without forming the full Hessian + // ------------------------------------------------------------------ + + /// @brief Compute the distance vector of the stencil: t = ∑ cᵢ xᵢ. + /// + /// The distance vector is the vector between the closest points on the + /// collision primitives. Its squared norm equals the squared distance. + /// + /// @param positions Stencil's vertex positions. + /// @note positions can be computed as stencil.dof(vertices, edges, faces) + /// @return The distance vector (dim-dimensional, i.e., 2D or 3D). + VectorMax3d + compute_distance_vector(Eigen::ConstRef positions) const; + + /// @brief Compute the distance vector and the coefficients together. + /// @param positions Stencil's vertex positions. + /// @param[out] coeffs The computed coefficients cᵢ. + /// @return The distance vector. + VectorMax3d compute_distance_vector( + Eigen::ConstRef positions, VectorMax4d& coeffs) const; + + /// @brief Compute the Jacobian of the distance vector w.r.t. positions: ∂t/∂x = [c₀I c₁I ... cₙI]ᵀ. + /// + /// Since ∂t/∂x has a very simple structure (block-diagonal with scalar + /// coefficients times identity), many operations can be done without + /// forming this matrix. This method is provided for completeness and + /// verification. + /// + /// @param positions Stencil's vertex positions. + /// @return The Jacobian ∂t/∂x ∈ ℝ^{ndof × dim}. + MatrixMax compute_distance_vector_jacobian( + Eigen::ConstRef positions) const; + + /// @brief Compute diag((∂t/∂x)(∂t/∂x)ᵀ) efficiently (Eq. 11 of the paper). + /// + /// Result is [c₀², c₀², c₀², c₁², c₁², c₁², ...] (each cᵢ² repeated dim + /// times). This is used for computing diag(∂²b/∂x²) without the full + /// Hessian. + /// + /// @param coeffs The coefficients cᵢ (from compute_coefficients). + /// @param d The spatial dimension (2 or 3). + /// @return The diagonal of (∂t/∂x)(∂t/∂x)ᵀ as a vector of size ndof. + static VectorMax12d diag_distance_vector_outer( + Eigen::ConstRef coeffs, const int d); + + /// @brief Compute diag((∂t/∂x · t)(∂t/∂x · t)ᵀ) efficiently (Eq. 12). + /// + /// Result is element-wise square of [c₀tᵀ, c₁tᵀ, ..., cₙtᵀ]. + /// This is used for computing diag(∂²b/∂x²) without the full Hessian. + /// + /// @param coeffs The coefficients cᵢ. + /// @param distance_vector The distance vector t. + /// @return The diagonal of (∂t/∂x·t)(∂t/∂x·t)ᵀ as a vector of size ndof. + static VectorMax12d diag_distance_vector_t_outer( + Eigen::ConstRef coeffs, + Eigen::ConstRef distance_vector); + + /// @brief Compute pᵀ(∂t/∂x) efficiently as ∑ cᵢ pᵢ (Eqs. 13-14). + /// + /// Given p = [p₀, p₁, ..., pₙ]ᵀ where pᵢ ∈ ℝ^dim, this computes + /// pᵀ(∂t/∂x) = ∑ cᵢ pᵢ which is a dim-dimensional vector. + /// + /// @param coeffs The coefficients cᵢ. + /// @param p A vector of size ndof (the direction for the quadratic form). + /// @param d The spatial dimension (2 or 3). + /// @return pᵀ(∂t/∂x) as a dim-dimensional vector. + static VectorMax3d contract_distance_vector_jacobian( + Eigen::ConstRef coeffs, + Eigen::ConstRef p, + const int d); + /// @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. diff --git a/src/ipc/potentials/normal_potential.cpp b/src/ipc/potentials/normal_potential.cpp index 94eebdda1..5b6c171c8 100644 --- a/src/ipc/potentials/normal_potential.cpp +++ b/src/ipc/potentials/normal_potential.cpp @@ -2,13 +2,91 @@ #include +#include +#include #include #include +#include namespace ipc { // -- Cumulative methods ------------------------------------------------------- +Eigen::VectorXd NormalPotential::gauss_newton_hessian_diagonal( + const NormalCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const +{ + assert(vertices.rows() == mesh.num_vertices()); + + if (collisions.empty()) { + return Eigen::VectorXd::Zero(vertices.size()); + } + + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + const int dim = vertices.cols(); + + tbb::combinable diag_storage( + Eigen::VectorXd::Zero(vertices.size())); + + tbb::parallel_for( + tbb::blocked_range(size_t(0), collisions.size()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); i++) { + const NormalCollision& collision = collisions[i]; + + const VectorMax12d local_diag = + this->gauss_newton_hessian_diagonal( + collision, collision.dof(vertices, edges, faces)); + + const auto vids = collision.vertex_ids(edges, faces); + + // Don't be confused by the "gradient" in the name -- this just + // scatters a local vector into a global vector. + local_gradient_to_global_gradient( + local_diag, vids, dim, diag_storage.local()); + } + }); + + return diag_storage.combine([](const Eigen::VectorXd& a, + const Eigen::VectorXd& b) { return a + b; }); +} + +double NormalPotential::gauss_newton_hessian_quadratic_form( + const NormalCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + Eigen::ConstRef p) const +{ + assert(vertices.rows() == mesh.num_vertices()); + assert(p.size() == vertices.size()); + + if (collisions.empty()) { + return 0.0; + } + + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + const int dim = vertices.cols(); + + // Reshape p into a matrix for convenient DOF extraction + const auto p_mat = + p.reshaped(vertices.rows(), dim); + + return tbb::parallel_reduce( + tbb::blocked_range(size_t(0), collisions.size()), 0.0, + [&](const tbb::blocked_range& r, double partial_sum) { + for (size_t i = r.begin(); i < r.end(); i++) { + partial_sum += this->gauss_newton_hessian_quadratic_form( + collisions[i], collisions[i].dof(vertices, edges, faces), + collisions[i].dof(p_mat, edges, faces)); + } + return partial_sum; + }, + std::plus()); +} + Eigen::SparseMatrix NormalPotential::shape_derivative( const NormalCollisions& collisions, const CollisionMesh& mesh, @@ -146,6 +224,82 @@ MatrixMax12d NormalPotential::hessian( return project_to_psd(hess, project_hessian_to_psd); } +// -- Single collision distance-vector methods --------------------------------- + +VectorMax12d NormalPotential::gauss_newton_hessian_diagonal( + const NormalCollision& collision, + Eigen::ConstRef positions) const +{ + const int d = collision.dim(positions.size()); + + // Compute coefficients and distance vector together + VectorMax4d coeffs; + const VectorMax3d t = collision.compute_distance_vector(positions, coeffs); + + // d² = tᵀt + const double d_sqr = t.squaredNorm(); + // f'(d²) + const double grad_f = gradient(d_sqr, collision.dmin); + // f''(d²) + const double hess_f = hessian(d_sqr, collision.dmin); + + // diag(∂²b/∂x²) ≈ w · [4·f"(d²)·diag((∂t/∂x·t)(∂t/∂x·t)ᵀ) + // + 2·f'(d²)·diag((∂t/∂x)(∂t/∂x)ᵀ)] + // + // From Eq. 11: diag((∂t/∂x)(∂t/∂x)ᵀ) = [c₀²,...,c₀²,c₁²,...,cₙ²] + // (each cᵢ² repeated dim times) + // From Eq. 12: diag((∂t/∂x·t)(∂t/∂x·t)ᵀ) = [c₀t, c₁t, ..., cₙt]^∘2 + // (element-wise square) + + const VectorMax12d diag_JtJ = + CollisionStencil::diag_distance_vector_outer(coeffs, d); + const VectorMax12d diag_JtttJt = + CollisionStencil::diag_distance_vector_t_outer(coeffs, t); + + const double s1 = collision.weight * 4.0 * hess_f; + const double s2 = collision.weight * 2.0 * grad_f; + + return s1 * diag_JtttJt + s2 * diag_JtJ; +} + +double NormalPotential::gauss_newton_hessian_quadratic_form( + const NormalCollision& collision, + Eigen::ConstRef positions, + Eigen::ConstRef p) const +{ + const int d = collision.dim(positions.size()); + + // Compute coefficients and distance vector together + VectorMax4d coeffs; + const VectorMax3d t = collision.compute_distance_vector(positions, coeffs); + + // d² = tᵀt + const double d_sqr = t.squaredNorm(); + // f'(d²) + const double grad_f = gradient(d_sqr, collision.dmin); + // f''(d²) + const double hess_f = hessian(d_sqr, collision.dmin); + + // pᵀHp ≈ w · [4·f"(d²)·(pᵀ·(∂t/∂x)·t)² + 2·f'(d²)·‖pᵀ·(∂t/∂x)‖²] + // + // From Eq. 13: pᵀ(∂t/∂x·t)(∂t/∂x·t)ᵀp = (pᵀ·∂t/∂x·t)² + // From Eq. 14: pᵀ(∂t/∂x)(∂t/∂x)ᵀp = ‖pᵀ·∂t/∂x‖² + // + // pᵀ·∂t/∂x = ∑ cᵢ pᵢ (a dim-dimensional vector) + + const VectorMax3d pT_J = + CollisionStencil::contract_distance_vector_jacobian(coeffs, p, d); + + // (pᵀ·∂t/∂x·t)² = (pT_J · t)² + const double pTJt = pT_J.dot(t); + const double term1 = pTJt * pTJt; + + // ‖pᵀ·∂t/∂x‖² = ‖pT_J‖² + const double term2 = pT_J.squaredNorm(); + + return collision.weight * (4.0 * hess_f * term1 + 2.0 * grad_f * term2); +} + void NormalPotential::shape_derivative( const NormalCollision& collision, const std::array& vertex_ids, diff --git a/src/ipc/potentials/normal_potential.hpp b/src/ipc/potentials/normal_potential.hpp index 30bb4827f..d6ad8525e 100644 --- a/src/ipc/potentials/normal_potential.hpp +++ b/src/ipc/potentials/normal_potential.hpp @@ -32,6 +32,44 @@ class NormalPotential : public Potential { const CollisionMesh& mesh, Eigen::ConstRef vertices) const; + /// @brief Compute the diagonal of the cumulative Gauss-Newton Hessian of the potential. + /// + /// Uses the distance-vector formulation to efficiently compute the + /// diagonal of the Gauss-Newton Hessian without forming full local 12×12 + /// Hessian matrices. This is useful as a Jacobi preconditioner for + /// iterative solvers. + /// + /// @note This is a Gauss-Newton approximation (drops derivatives of closest-point coefficients), not the exact Hessian. + /// + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @return The diagonal of the Gauss-Newton Hessian as a vector of size vertices.size(). + Eigen::VectorXd gauss_newton_hessian_diagonal( + const NormalCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const; + + /// @brief Compute the product pᵀHp for the cumulative Gauss-Newton Hessian of the potential. + /// + /// Uses the distance-vector formulation to efficiently compute the + /// quadratic form pᵀHp without forming full local 12×12 Hessian matrices + /// nor the global sparse Hessian. This is useful for nonlinear conjugate + /// gradient methods. + /// + /// @note This is a Gauss-Newton approximation (drops derivatives of closest-point coefficients), not the exact Hessian. + /// + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @param p The direction vector of size vertices.size(). + /// @return The scalar value pᵀHp. + double gauss_newton_hessian_quadratic_form( + const NormalCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + Eigen::ConstRef p) const; + // -- Single collision methods --------------------------------------------- /// @brief Compute the potential for a single collision. @@ -61,6 +99,40 @@ class NormalPotential : public Potential { const PSDProjectionMethod project_hessian_to_psd = PSDProjectionMethod::NONE) const override; + // -- Single collision distance-vector methods ----------------------------- + + /// @brief Compute the diagonal of the Gauss-Newton Hessian of the + /// potential for a single collision using the distance-vector formulation + /// (Eqs. 10-12). + /// + /// diag(∂²b/∂x²) ≈ w · [4·f"(d²) · diag((∂dv/∂x·dv)(∂dv/∂x·dv)ᵀ) + /// + 2·f'(d²) · diag((∂dv/∂x)(∂dv/∂x)ᵀ)] + /// + /// @note This is a Gauss-Newton approximation, not the exact Hessian diagonal. It drops derivatives of the closest-point coefficients. + /// + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The diagonal of the Gauss-Newton Hessian as a vector of size ndof. + VectorMax12d gauss_newton_hessian_diagonal( + const NormalCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute pᵀHp for a single collision using the Gauss-Newton + /// Hessian via the distance-vector formulation (Eqs. 10, 13-14). + /// + /// pᵀHp ≈ w · [4·f"(d²) · (pᵀ·∂dv/∂x·dv)² + 2·f'(d²) · ‖pᵀ·∂dv/∂x‖²] + /// + /// @note This is a Gauss-Newton approximation, not the exact Hessian quadratic form. It drops derivatives of the closest-point coefficients. + /// + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @param p The local direction vector (size ndof, i.e., the stencil's DOF slice of the global p). + /// @return The scalar value pᵀHp (approximate). + double gauss_newton_hessian_quadratic_form( + const NormalCollision& collision, + Eigen::ConstRef positions, + Eigen::ConstRef p) const; + /// @brief Compute the shape derivative of the potential for a single collision. /// @param[in] collision The collision. /// @param[in] vertex_ids The collision stencil's vertex ids. diff --git a/src/ipc/utils/local_to_global.hpp b/src/ipc/utils/local_to_global.hpp index ffd0f4360..7aec559f8 100644 --- a/src/ipc/utils/local_to_global.hpp +++ b/src/ipc/utils/local_to_global.hpp @@ -33,6 +33,7 @@ void local_gradient_to_global_gradient( const int n_total_verts = grad.size() / dim; assert(grad.size() % dim == 0); // Ensure grad is properly sized for (int i = 0; i < n_verts; i++) { + assert(ids[i] >= 0); // Ensure valid vertex id if constexpr (GlobalOrder == Eigen::RowMajor) { grad.segment(dim * ids[i], dim) += local_grad.segment(dim * i, dim); } else { @@ -65,6 +66,7 @@ void local_gradient_to_global_gradient( const int n_total_verts = grad.size() / dim; assert(grad.size() % dim == 0); // Ensure grad is properly sized for (int i = 0; i < n_verts; i++) { + assert(ids[i] >= 0); // Ensure valid vertex id for (int d = 0; d < dim; d++) { if constexpr (GlobalOrder == Eigen::RowMajor) { grad.coeffRef(dim * ids[i] + d) += local_grad(dim * i + d); @@ -109,7 +111,9 @@ void local_hessian_to_global_triplets( } } for (int i = 0; i < n_verts; i++) { + assert(ids[i] >= 0); // Ensure valid vertex id for (int j = 0; j < n_verts; j++) { + assert(ids[j] >= 0); // Ensure valid vertex id for (int k = 0; k < dim; k++) { for (int l = 0; l < dim; l++) { if constexpr (GlobalOrder == Eigen::RowMajor) { @@ -174,7 +178,9 @@ void local_jacobian_to_global_triplets( } } for (int i = 0; i < n_rows; i++) { + assert(row_ids[i] >= 0); // Ensure valid vertex id for (int j = 0; j < n_cols; j++) { + assert(col_ids[j] >= 0); // Ensure valid vertex id for (int k = 0; k < dim; k++) { for (int l = 0; l < dim; l++) { if constexpr (GlobalOrder == Eigen::RowMajor) { diff --git a/tests/src/tests/potential/CMakeLists.txt b/tests/src/tests/potential/CMakeLists.txt index 03c08d97a..f2ff672f6 100644 --- a/tests/src/tests/potential/CMakeLists.txt +++ b/tests/src/tests/potential/CMakeLists.txt @@ -4,6 +4,7 @@ set(SOURCES test_barrier_potential.cpp test_smooth_potential.cpp test_friction_potential.cpp + test_distance_vector_methods.cpp # Benchmarks diff --git a/tests/src/tests/potential/test_distance_vector_methods.cpp b/tests/src/tests/potential/test_distance_vector_methods.cpp new file mode 100644 index 000000000..ca5cf7def --- /dev/null +++ b/tests/src/tests/potential/test_distance_vector_methods.cpp @@ -0,0 +1,589 @@ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace ipc; + +// ============================================================================= +// Helper: build the explicit distance-vector Hessian for a single unmollified +// collision so we can compare the efficient methods against it. +// +// H_t = w * [4 * f''(d²) * (J*t)(J*t)ᵀ + 2 * f'(d²) * J*Jᵀ] +// +// where J = ∂t/∂x = [c₀I, c₁I, ..., cₙI]ᵀ (ndof × dim) +// t = ∑ cᵢ xᵢ (distance vector) +// d² = tᵀt +// ============================================================================= + +static MatrixMax12d build_gauss_newton_hessian( + const NormalCollision& collision, + Eigen::ConstRef positions, + const BarrierPotential& barrier_potential) +{ + const int n = collision.num_vertices(); + const int d = collision.dim(positions.size()); + const int ndof = n * d; + + VectorMax4d coeffs; + VectorMax3d t = collision.compute_distance_vector(positions, coeffs); + + // Construct J (ndof × dim) + MatrixMax12d J; + J.setZero(ndof, d); + for (int i = 0; i < n; i++) { + for (int j = 0; j < d; j++) { + J(i * d + j, j) = coeffs[i]; + } + } + + // J*t ∈ ℝ^{ndof} + VectorMax12d Jt = J * t; + + // grad = w * f'(d²) * ∇d(x) = w * f'(d²) * 2 * Jᵀt + VectorMax12d grad = barrier_potential.gradient(collision, positions); + + // Find a nonzero index in Jt + double w_grad_f = 0; // = w * f'(d²) + for (int i = 0; i < ndof; i++) { + if (std::abs(Jt[i]) > 1e-15) { + w_grad_f = grad[i] / (2.0 * Jt[i]); + break; + } + } + + // For f"(d²), use the diagonal: + // diag[i] = w * 4 * f"(d²) * Jt[i]² + w * 2 * f'(d²) * J_diag[i] + // where J_diag[i] = cₖ² for the vertex k that owns index i. + // + // So: w * 4 * f''(d²) = (diag[i] - 2 * w_grad_f * cₖ²) / Jt[i]² + // for some i where Jt[i] ≠ 0. + VectorMax12d diag_vec = + barrier_potential.gauss_newton_hessian_diagonal(collision, positions); + + double w_hess_f = 0; // = w * f"(d²) + for (int i = 0; i < ndof; i++) { + if (std::abs(Jt[i]) > 1e-15) { + int vi = i / d; + double ci2 = coeffs[vi] * coeffs[vi]; + w_hess_f = + (diag_vec[i] - 2.0 * w_grad_f * ci2) / (4.0 * Jt[i] * Jt[i]); + break; + } + } + + MatrixMax12d H_t(ndof, ndof); + H_t = 4.0 * w_hess_f * Jt * Jt.transpose() + + 2.0 * w_grad_f * J * J.transpose(); + + return H_t; +} + +// ============================================================================= +// Tests for CollisionStencil::compute_distance_vector +// ============================================================================= + +TEST_CASE( + "distance vector methods match distance and gradient", + "[stencil][distance_vector]") +{ + std::unique_ptr collision; + VectorMax12d positions; + + SECTION("Vertex-vertex") + { + Eigen::MatrixXd V(2, 3); + V << -1, 0, 0, /**/ 1, 0, 0; + Eigen::MatrixXi E, F; + collision = std::make_unique(0, 1); + positions = collision->dof(V, E, F); + } + SECTION("Edge-vertex") + { + Eigen::MatrixXd V(3, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0; + Eigen::MatrixXi E(1, 2), F; + E << 0, 1; + collision = std::make_unique(0, 2); + positions = collision->dof(V, E, F); + } + SECTION("Edge-edge") + { + Eigen::MatrixXd V(4, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, -1, 1, /**/ 0, 1, 1; + Eigen::MatrixXi E(2, 2), F; + E << 0, 1, /**/ 2, 3; + collision = std::make_unique(0, 1); + positions = collision->dof(V, E, F); + } + SECTION("Face-vertex") + { + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0, /**/ 0.25, 0.25, 1; + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + Eigen::MatrixXi E; + igl::edges(F, E); + collision = std::make_unique(0, 3); + positions = collision->dof(V, E, F); + } + + VectorMax3d t = collision->compute_distance_vector(positions); + CHECK( + t.squaredNorm() + == Catch::Approx(collision->compute_distance(positions))); + + MatrixMax dt_dx = + collision->compute_distance_vector_jacobian(positions); + VectorMax12d grad = collision->compute_distance_gradient(positions); + CAPTURE(dt_dx * t, grad); + CHECK((dt_dx * (2 * t)).isApprox(grad)); +} + +TEST_CASE( + "compute_distance_vector with coeffs output matches separate calls", + "[stencil][distance_vector]") +{ + Eigen::MatrixXd V(4, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, -1, 1, /**/ 0, 1, 1; + Eigen::MatrixXi E(2, 2), F; + E << 0, 1, /**/ 2, 3; + + EdgeEdgeCandidate ee(0, 1); + VectorMax12d positions = ee.dof(V, E, F); + + VectorMax4d coeffs_separate = ee.compute_coefficients(positions); + VectorMax3d t_separate = ee.compute_distance_vector(positions); + + VectorMax4d coeffs_joint; + VectorMax3d t_joint = ee.compute_distance_vector(positions, coeffs_joint); + + CHECK(t_joint.isApprox(t_separate)); + CHECK(coeffs_joint.isApprox(coeffs_separate)); +} + +// ============================================================================= +// Tests for CollisionStencil static helper methods +// ============================================================================= + +TEST_CASE( + "Collision stencil distance vector diagonals match explicit computation", + "[stencil][distance_vector]") +{ + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0, /**/ 0.25, 0.25, 1; + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + Eigen::MatrixXi E; + igl::edges(F, E); + + FaceVertexCandidate fv(0, 3); + VectorMax12d positions = fv.dof(V, E, F); + const int dim = fv.dim(positions.size()); + + VectorMax4d coeffs; + VectorMax3d t = fv.compute_distance_vector(positions, coeffs); + + { + // Efficient method + VectorMax12d diag_efficient = + CollisionStencil::diag_distance_vector_outer(coeffs, dim); + + // Explicit: form J = ∂t/∂x, compute diag(JJᵀ) + MatrixMax12d J = fv.compute_distance_vector_jacobian(positions); + MatrixMax12d JJT = J * J.transpose(); + VectorMax12d diag_explicit = JJT.diagonal(); + + CHECK(diag_efficient.isApprox(diag_explicit)); + } + + { + // Efficient method + VectorMax12d diag_efficient = + CollisionStencil::diag_distance_vector_t_outer(coeffs, t); + + // Explicit: form J·t, compute diag((J·t)(J·t)ᵀ) + MatrixMax12d J = fv.compute_distance_vector_jacobian(positions); + VectorMax12d Jt = J * t; + VectorMax12d diag_explicit = (Jt.array() * Jt.array()).matrix(); + + CHECK(diag_efficient.isApprox(diag_explicit)); + } + + { + // Some arbitrary direction vector + VectorMax12d p(12); + p << 0.1, -0.2, 0.3, 0.4, -0.5, 0.6, -0.7, 0.8, -0.9, 1.0, -1.1, 1.2; + + // Efficient method + VectorMax3d pTJ_efficient = + CollisionStencil::contract_distance_vector_jacobian(coeffs, p, dim); + + // Explicit: Jᵀp (J is ndof×dim, so Jᵀ is dim×ndof, Jᵀp is dim×1) + MatrixMax12d J = fv.compute_distance_vector_jacobian(positions); + Eigen::VectorXd pTJ_explicit = J.transpose() * p; + + CHECK(pTJ_efficient.isApprox(pTJ_explicit)); + } +} + +// ============================================================================= +// Tests for NormalPotential::gauss_newton_hessian_diagonal (single collision) +// +// The distance-vector Hessian is a Gauss-Newton-like approximation: +// H_t = w * [4·f''(d²)·(J·t)(J·t)ᵀ + 2·f'(d²)·JJᵀ] +// It omits terms from the derivative of the closest-point parameters. +// For VV the coefficients are constant so H_t == H_full exactly. +// For other types we verify self-consistency: the efficient diagonal must +// match the diagonal of the explicitly-constructed distance-vector Hessian. +// ============================================================================= + +TEST_CASE( + "gauss_newton_hessian_diagonal matches diagonal of full hessian for VV (exact case)", + "[potential][barrier_potential][distance_vector][gauss_newton_hessian_diagonal]") +{ + const double dhat = 1.0; + const double kappa = 1.0; + BarrierPotential barrier_potential(dhat, kappa); + + Eigen::MatrixXd V(2, 3); + V << 0, 0, 0, /**/ 0.5, 0, 0; + Eigen::MatrixXi E, F; + + VertexVertexNormalCollision collision(0, 1); + collision.dmin = 0; + collision.weight = 1.0; + + VectorMax12d positions = collision.dof(V, E, F); + + MatrixMax12d hess_full = barrier_potential.hessian( + collision, positions, PSDProjectionMethod::NONE); + VectorMax12d diag_standard = hess_full.diagonal(); + VectorMax12d diag_t = + barrier_potential.gauss_newton_hessian_diagonal(collision, positions); + + // VV coefficients are constant → distance-vector Hessian is exact + CHECK(diag_t.isApprox(diag_standard)); +} + +TEST_CASE( + "gauss_newton_hessian_diagonal is self-consistent with explicit distance-vector Hessian", + "[potential][barrier_potential][distance_vector][gauss_newton_hessian_diagonal]") +{ + const double dhat = 1.0; + const double kappa = 1.0; + BarrierPotential barrier_potential(dhat, kappa); + + std::unique_ptr collision; + VectorMax12d positions; + + SECTION("Edge-vertex") + { + Eigen::MatrixXd V(3, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, 0.5, 0; + Eigen::MatrixXi E(1, 2), F; + E << 0, 1; + + collision = std::make_unique(0, 2); + collision->dmin = 0; + collision->weight = 1.0; + + positions = collision->dof(V, E, F); + } + SECTION("Face-vertex") + { + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0, /**/ 0.25, 0.25, 0.5; + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + Eigen::MatrixXi E; + igl::edges(F, E); + + collision = std::make_unique(0, 3); + collision->dmin = 0; + collision->weight = 1.0; + + positions = collision->dof(V, E, F); + } + SECTION("Face-vertex with weight and dmin") + { + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0, /**/ 0.25, 0.25, 0.5; + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + Eigen::MatrixXi E; + igl::edges(F, E); + + collision = std::make_unique(0, 3); + collision->dmin = 0.01; + collision->weight = 2.5; + + positions = collision->dof(V, E, F); + } + SECTION("Edge-edge") + { + Eigen::MatrixXd V(4, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, -1, 0.5, /**/ 0, 1, 0.5; + Eigen::MatrixXi E(2, 2), F; + E << 0, 1, /**/ 2, 3; + + collision = std::make_unique(0, 1, 0.0); + collision->dmin = 0; + collision->weight = 1.0; + + positions = collision->dof(V, E, F); + } + + // Build the explicit distance-vector Hessian matrix + MatrixMax12d H_t = + build_gauss_newton_hessian(*collision, positions, barrier_potential); + VectorMax12d diag_explicit = H_t.diagonal(); + + VectorMax12d diag_efficient = + barrier_potential.gauss_newton_hessian_diagonal(*collision, positions); + + CHECK(diag_efficient.isApprox(diag_explicit)); +} + +// ============================================================================= +// Tests for NormalPotential::gauss_newton_hessian_quadratic_form (single +// collision) +// +// Same logic: exact for VV, self-consistent with the explicit distance-vector +// Hessian matrix for other types. +// ============================================================================= + +TEST_CASE( + "gauss_newton_hessian_quadratic_form matches pTHp from full hessian for VV (exact case)", + "[potential][barrier_potential][distance_vector][gauss_newton_hessian_quadratic_form]") +{ + const double dhat = 1.0; + const double kappa = 1.0; + BarrierPotential barrier_potential(dhat, kappa); + + Eigen::MatrixXd V(2, 3); + V << 0, 0, 0, /**/ 0.5, 0, 0; + Eigen::MatrixXi E, F; + + VertexVertexNormalCollision collision(0, 1); + collision.dmin = 0; + collision.weight = 1.0; + + VectorMax12d positions = collision.dof(V, E, F); + VectorMax12d p(6); + p << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6; + + MatrixMax12d hess_full = barrier_potential.hessian( + collision, positions, PSDProjectionMethod::NONE); + double pTHp_standard = p.dot(hess_full * p); + double pTHp_t = barrier_potential.gauss_newton_hessian_quadratic_form( + collision, positions, p); + + // VV: exact match + CHECK(pTHp_t == Catch::Approx(pTHp_standard).epsilon(1e-10)); +} + +TEST_CASE( + "gauss_newton_hessian_quadratic_form is self-consistent with explicit distance-vector Hessian", + "[potential][barrier_potential][distance_vector][gauss_newton_hessian_quadratic_form]") +{ + const double dhat = 1.0; + const double kappa = 1.0; + BarrierPotential barrier_potential(dhat, kappa); + + std::unique_ptr collision; + VectorMax12d positions; + + SECTION("Edge-vertex") + { + Eigen::MatrixXd V(3, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, 0.5, 0; + Eigen::MatrixXi E(1, 2), F; + E << 0, 1; + + collision = std::make_unique(0, 2); + + positions = collision->dof(V, E, F); + } + SECTION("Face-vertex") + { + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, /**/ 1, 0, 0, /**/ 0, 1, 0, /**/ 0.25, 0.25, 0.5; + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + Eigen::MatrixXi E; + igl::edges(F, E); + + collision = std::make_unique(0, 3); + + positions = collision->dof(V, E, F); + } + SECTION("Edge-edge") + { + Eigen::MatrixXd V(4, 3); + V << -1, 0, 0, /**/ 1, 0, 0, /**/ 0, -1, 0.5, /**/ 0, 1, 0.5; + Eigen::MatrixXi E(2, 2), F; + E << 0, 1, /**/ 2, 3; + + collision = std::make_unique(0, 1, 0.0); + + positions = collision->dof(V, E, F); + } + + collision->dmin = 0; + collision->weight = 1.0; + + VectorMax12d p = VectorMax12d::Random(positions.size()); + + MatrixMax12d H_t = + build_gauss_newton_hessian(*collision, positions, barrier_potential); + double pTHp_explicit = p.dot(H_t * p); + + double pTHp_efficient = + barrier_potential.gauss_newton_hessian_quadratic_form( + *collision, positions, p); + CHECK(pTHp_efficient == Catch::Approx(pTHp_explicit).epsilon(1e-10)); +} + +// ============================================================================= +// Tests for cumulative methods on a mesh +// +// The cumulative methods assemble contributions from all collisions (including +// mollified EE). We test self-consistency by computing per-collision +// contributions and summing them up, then comparing diagonal/quadratic-form. +// ============================================================================= + +TEST_CASE( + "Cumulative gauss_newton_hessian_diagonal is self-consistent", + "[potential][barrier_potential][distance_vector][cumulative]") +{ + const double dhat = sqrt(2.0); + const double kappa = 1.0; + const std::string mesh_name = "cube.ply"; + + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + bool success = tests::load_mesh(mesh_name, vertices, edges, faces); + CAPTURE(mesh_name); + REQUIRE(success); + + CollisionMesh mesh(vertices, edges, faces); + + NormalCollisions collisions; + collisions.set_use_area_weighting(false); + collisions.build(mesh, vertices, dhat); + REQUIRE(!collisions.empty()); + + BarrierPotential barrier_potential(dhat, kappa); + + // Compute diagonal via the efficient cumulative method + Eigen::VectorXd diag_efficient = + barrier_potential.gauss_newton_hessian_diagonal( + collisions, mesh, vertices); + + // Compute diagonal by summing up individual collision contributions + const int dim = vertices.cols(); + Eigen::VectorXd diag_reference = Eigen::VectorXd::Zero(vertices.size()); + + for (size_t i = 0; i < collisions.size(); i++) { + const NormalCollision& collision = collisions[i]; + const VectorMax12d positions = collision.dof(vertices, edges, faces); + + VectorMax12d local_diag = + barrier_potential.gauss_newton_hessian_diagonal( + collision, positions); + + const auto vids = collision.vertex_ids(edges, faces); + for (int vi = 0; vi < collision.num_vertices(); vi++) { + if (vids[vi] >= 0) { + for (int d = 0; d < dim; d++) { + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + diag_reference[vids[vi] * dim + d] += + local_diag[vi * dim + d]; + } else { + diag_reference[vids[vi] + d * vertices.rows()] += + local_diag[vi + d * collision.num_vertices()]; + } + } + } + } + } + + REQUIRE(diag_efficient.size() == diag_reference.size()); + CHECK( + (diag_efficient - diag_reference).norm() + < 1e-10 * (1.0 + diag_reference.norm())); +} + +TEST_CASE( + "Cumulative gauss_newton_hessian_quadratic_form is self-consistent", + "[potential][barrier_potential][distance_vector][cumulative]") +{ + const double dhat = sqrt(2.0); + const double kappa = 1.0; + const std::string mesh_name = "cube.ply"; + + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + bool success = tests::load_mesh(mesh_name, vertices, edges, faces); + CAPTURE(mesh_name); + REQUIRE(success); + + CollisionMesh mesh(vertices, edges, faces); + + NormalCollisions collisions; + collisions.set_use_area_weighting(false); + collisions.build(mesh, vertices, dhat); + REQUIRE(!collisions.empty()); + + BarrierPotential barrier_potential(dhat, kappa); + + // Random direction vector + Eigen::VectorXd p = Eigen::VectorXd::Random(vertices.size()); + p *= 0.1; // keep small for numerical stability + + // Compute pᵀHp via the efficient cumulative method + double pTHp_efficient = + barrier_potential.gauss_newton_hessian_quadratic_form( + collisions, mesh, vertices, p); + + // Compute pᵀHp by summing up individual collision contributions + const int dim = vertices.cols(); + const auto p_mat = + p.reshaped(vertices.rows(), dim); + + double pTHp_reference = 0.0; + for (size_t i = 0; i < collisions.size(); i++) { + const NormalCollision& collision = collisions[i]; + const VectorMax12d positions = collision.dof(vertices, edges, faces); + + const auto vids = collision.vertex_ids(edges, faces); + VectorMax12d local_p(collision.num_vertices() * dim); + for (int vi = 0; vi < collision.num_vertices(); vi++) { + local_p.segment(vi * dim, dim) = p_mat.row(vids[vi]); + } + + pTHp_reference += barrier_potential.gauss_newton_hessian_quadratic_form( + collision, positions, local_p); + } + + CHECK( + pTHp_efficient + == Catch::Approx(pTHp_reference).epsilon(1e-10).margin(1e-14)); +} \ No newline at end of file