From 21f71674ed277852269e24922bb3504f591d480c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 9 Feb 2026 00:51:39 -0500 Subject: [PATCH 01/16] Support configurable vertex derivative layout Add a VERTEX_DERIVATIVE_LAYOUT constant to ipc::config and include Eigen. Update local_to_global.hpp to parameterize all assembly helpers (gradient, sparse gradient, Hessian triplets, Jacobian triplets, MatrixCache Hessian) with an optional GlobalOrder template (defaulting to VERTEX_DERIVATIVE_LAYOUT). Implement both RowMajor and ColMajor indexing code paths and add static_asserts and detailed doc comments. Also include ipc/config.hpp, convert LocalThreadMatStorage to a struct with docs, and adjust MatrixCache-based hessian assembly to respect the chosen global ordering. --- src/ipc/config.hpp.in | 4 + src/ipc/utils/local_to_global.hpp | 134 ++++++++++++++++++++++++++---- 2 files changed, 121 insertions(+), 17 deletions(-) diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index 04711cf7b..c4f473b7f 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -1,5 +1,7 @@ #pragma once +#include + #include // WARNING: Do not modify config.hpp directly. Instead, modify config.hpp.in. @@ -37,4 +39,6 @@ using index_t = int32_t; // using scalar_t = float; // #endif +inline constexpr int VERTEX_DERIVATIVE_LAYOUT = Eigen::RowMajor; + } // namespace ipc \ No newline at end of file diff --git a/src/ipc/utils/local_to_global.hpp b/src/ipc/utils/local_to_global.hpp index aa1f3c74f..b6710e8ec 100644 --- a/src/ipc/utils/local_to_global.hpp +++ b/src/ipc/utils/local_to_global.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -9,45 +10,86 @@ namespace ipc { -template +/// @brief Add a local gradient to the global gradient, given the vertex ids and dimension. +/// @tparam IDContainer A container type that supports operator[] and size() to access vertex ids. Can be a std::vector, Eigen::VectorXi, etc. +/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @param local_grad The local gradient to be added to the global gradient. Must be of size n_verts * dim, where n_verts is the number of vertices in the local element and dim is the dimension of the problem. +/// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). +/// @param grad The global gradient to which the local gradient will be added. Must be of size at least max(ids) * dim, but can be larger. +template void local_gradient_to_global_gradient( Eigen::ConstRef local_grad, const IDContainer& ids, const int dim, Eigen::Ref grad) { + static_assert( + GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); assert(local_grad.size() % dim == 0); const int n_verts = local_grad.size() / dim; assert(ids.size() >= n_verts); // Can be extra ids for (int i = 0; i < n_verts; i++) { - grad.segment(dim * ids[i], dim) += local_grad.segment(dim * i, dim); + if constexpr (GlobalOrder == Eigen::RowMajor) { + grad.segment(dim * ids[i], dim) += local_grad.segment(dim * i, dim); + } else { + for (int d = 0; d < dim; d++) { + grad[n_verts * d + ids[i]] += local_grad[dim * i + d]; + } + } } } -template +/// @brief Add a local gradient to the global gradient, given the vertex ids and dimension. +/// @tparam IDContainer A container type that supports operator[] and size() to access vertex ids. Can be a std::vector, Eigen::VectorXi, etc. +/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @param local_grad The local gradient to be added to the global gradient. Must be of size n_verts * dim, where n_verts is the number of vertices in the local element and dim is the dimension of the problem. +/// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). +/// @param grad The global gradient to which the local gradient will be added. Must be of size at least max(ids) * dim, but can be larger. +template void local_gradient_to_global_gradient( Eigen::ConstRef local_grad, const IDContainer& ids, const int dim, Eigen::SparseVector& grad) { + static_assert( + GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); assert(local_grad.size() % dim == 0); const int n_verts = local_grad.size() / dim; assert(ids.size() >= n_verts); // Can be extra ids for (int i = 0; i < n_verts; i++) { for (int d = 0; d < dim; d++) { - grad.coeffRef(dim * ids[i] + d) += local_grad(dim * i + d); + if constexpr (GlobalOrder == Eigen::RowMajor) { + grad.coeffRef(dim * ids[i] + d) += local_grad(dim * i + d); + } else { + grad.coeffRef(n_verts * d + ids[i]) += local_grad(dim * i + d); + } } } } -template +/// @brief Add a local Hessian to the global Hessian, given the vertex ids and dimension. +/// +/// The local Hessian is added to the global Hessian in triplet form, so that it +/// can be efficiently assembled into a sparse matrix later. +/// +/// @tparam IDContainer A container type that supports operator[] and size() to access vertex ids. Can be a std::vector, Eigen::VectorXi, etc. +/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @param local_hessian The local Hessian to be added to the global Hessian. Must be of size (n_verts * dim) x (n_verts * dim), where n_verts is the number of vertices in the local element and dim is the dimension of the problem. +/// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). +/// @param triplets The vector of triplets to which the local Hessian will be added. +template void local_hessian_to_global_triplets( Eigen::ConstRef local_hessian, const IDContainer& ids, const int dim, std::vector>& triplets) { + static_assert( + GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); assert(local_hessian.rows() == local_hessian.cols()); assert(local_hessian.rows() % dim == 0); const int n_verts = local_hessian.rows() / dim; @@ -56,16 +98,40 @@ void local_hessian_to_global_triplets( for (int j = 0; j < n_verts; j++) { for (int k = 0; k < dim; k++) { for (int l = 0; l < dim; l++) { - triplets.emplace_back( - dim * ids[i] + k, dim * ids[j] + l, - local_hessian(dim * i + k, dim * j + l)); + if constexpr (GlobalOrder == Eigen::RowMajor) { + triplets.emplace_back( + dim * ids[i] + k, dim * ids[j] + l, + local_hessian(dim * i + k, dim * j + l)); + } else { + triplets.emplace_back( + n_verts * k + ids[i], n_verts * l + ids[j], + local_hessian(dim * i + k, dim * j + l)); + } } } } } } -template +/// @brief Add a local Jacobian to the global Jacobian, given the vertex ids and dimension. +/// +/// The local Jacobian is added to the global Jacobian in triplet form, so that +/// it can be efficiently assembled into a sparse matrix later. +/// +/// @tparam Derived The derived type of the local Jacobian matrix. Must be a matrix expression that can be evaluated to an Eigen::MatrixXd. The local Jacobian must be of size (n_rows * dim) x (n_cols * dim), where n_rows and n_cols are the number of vertices in the local element for the row and column respectively, and dim is the dimension of the problem. +/// @tparam IDContainer1 A container type that supports operator[] and size() to access vertex ids for the row indices. Can be a std::vector, Eigen::VectorXi, etc. Must be of size at least n_rows, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @tparam IDContainer2 A container type that supports operator[] and size() to access vertex ids for the column indices. Can be a std::vector, Eigen::VectorXi, etc. Must be of size at least n_cols, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @param local_jacobian The local Jacobian to be added to the global Jacobian. Must be of size (n_rows * dim) x (n_cols * dim), where n_rows and n_cols are the number of vertices in the local element for the row and column respectively, and dim is the dimension of the problem. +/// @param row_ids The vertex ids corresponding to the row indices of the local Jacobian. Must be of size at least n_rows, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @param col_ids The vertex ids corresponding to the column indices of the local Jacobian. Must be of size at least n_cols, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). +/// @param triplets The vector of triplets to which the local Hessian will be added. +template < + typename Derived, + typename IDContainer1, + typename IDContainer2, + int GlobalOrder = VERTEX_DERIVATIVE_LAYOUT> void local_jacobian_to_global_triplets( const Eigen::MatrixBase& local_jacobian, const IDContainer1& row_ids, @@ -73,6 +139,8 @@ void local_jacobian_to_global_triplets( int dim, std::vector>& triplets) { + static_assert( + GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); // assert(local_jacobian.rows() == local_jacobian.cols()); assert(local_jacobian.rows() % dim == 0); assert(local_jacobian.cols() % dim == 0); @@ -84,17 +152,26 @@ void local_jacobian_to_global_triplets( for (int j = 0; j < n_cols; j++) { for (int k = 0; k < dim; k++) { for (int l = 0; l < dim; l++) { - triplets.emplace_back( - dim * row_ids[i] + k, dim * col_ids[j] + l, - local_jacobian(dim * i + k, dim * j + l)); + if constexpr (GlobalOrder == Eigen::RowMajor) { + triplets.emplace_back( + dim * row_ids[i] + k, dim * col_ids[j] + l, + local_jacobian(dim * i + k, dim * j + l)); + } else { + triplets.emplace_back( + n_rows * k + row_ids[i], n_cols * l + col_ids[j], + local_jacobian(dim * i + k, dim * j + l)); + } } } } } } -class LocalThreadMatStorage { -public: +/// @brief A helper class to store a local matrix cache for each thread. +/// This is useful for parallelizing the assembly of the global Hessian, where +/// each thread can have its own local cache to store the triplets before they +/// are merged into the global cache. +struct LocalThreadMatStorage { std::unique_ptr cache = nullptr; LocalThreadMatStorage() = delete; @@ -138,13 +215,30 @@ class LocalThreadMatStorage { } }; -template +/// @brief Add a local Hessian to the global Hessian, given the vertex ids and dimension. +/// +/// The local Hessian is added to the global Hessian in triplet form, so that it +/// can be efficiently assembled into a sparse matrix later. +/// +/// @tparam Derived The derived type of the local Hessian matrix. Must be a matrix expression that can be evaluated to an Eigen::MatrixXd. The local Hessian must be of size (n_verts * dim) x (n_verts * dim), where n_verts is the number of vertices in the local element and dim is the dimension of the problem. +/// @tparam IDContainer A container type that supports operator[] and size() to access vertex ids. Can be a std::vector, Eigen::VectorXi, etc. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @param local_hessian The local Hessian to be added to the global Hessian. Must be of size (n_verts * dim) x (n_verts * dim), where n_verts is the number of vertices in the local element and dim is the dimension of the problem. +/// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). +/// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). +/// @param triplets The MatrixCache to which the local Hessian will be added. The triplets will be added to the first matrix in the cache (i.e., matrix index 0). The cache should be initialized with the correct number of rows and columns for the global Hessian, and should have enough reserved space for the number of triplets that will be added. +template < + typename Derived, + typename IDContainer, + int GlobalOrder = VERTEX_DERIVATIVE_LAYOUT> void local_hessian_to_global_triplets( const Eigen::MatrixBase& local_hessian, const IDContainer& ids, int dim, MatrixCache& triplets) { + static_assert( + GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); assert(local_hessian.rows() == local_hessian.cols()); assert(local_hessian.rows() % dim == 0); const int n_verts = local_hessian.rows() / dim; @@ -155,8 +249,14 @@ void local_hessian_to_global_triplets( for (int l = 0; l < dim; l++) { const auto& val = local_hessian(dim * i + k, dim * j + l); if (val != 0) { - triplets.add_value( - 0, dim * ids[i] + k, dim * ids[j] + l, val); + if constexpr (GlobalOrder == Eigen::RowMajor) { + triplets.add_value( + 0, dim * ids[i] + k, dim * ids[j] + l, val); + } else { + triplets.add_value( + 0, n_verts * k + ids[i], n_verts * l + ids[j], + val); + } } } } From 86a0a91ce7b45c40a9e989bd88e2aefcdc5bb865 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 9 Feb 2026 01:28:35 -0500 Subject: [PATCH 02/16] Add barrier stiffness and simplify tangential API (#215) * Add barrier stiffness and simplify tangential API Introduce a barrier stiffness (kappa) to BarrierPotential: new constructors, member, getters/setters, and scale operator/gradient/hessian by stiffness. Remove the redundant normal_stiffness parameter from tangential collision constructors and tangential potential interfaces, updating all call sites, headers, and implementations accordingly. Update Python bindings, tutorials, examples (ogc, solver), and tests (test_ipc) to match the new API and default stiffness usage; also move a notebook to python/examples. Miscellaneous formatting/import cleanups and adjustments to pybind argument lists to reflect the simplified tangential API. --- docs/source/tutorials/getting_started.rst | 10 ++-- docs/source/tutorials/simulation.rst | 12 ++-- {notebooks => python/examples}/ogc.py | 3 +- python/examples/solver.py | 37 ++++++------ .../src/collisions/tangential/edge_edge.cpp | 5 +- .../src/collisions/tangential/edge_vertex.cpp | 5 +- .../src/collisions/tangential/face_vertex.cpp | 5 +- .../tangential/tangential_collisions.cpp | 20 +++---- .../collisions/tangential/vertex_vertex.cpp | 6 +- python/src/potentials/barrier_potential.cpp | 14 +++-- python/src/potentials/normal_potential.cpp | 6 +- .../src/potentials/tangential_potential.cpp | 26 ++++----- python/tests/test_ipc.py | 58 +++++++++++++++---- src/ipc/collisions/tangential/edge_edge.cpp | 6 +- src/ipc/collisions/tangential/edge_edge.hpp | 3 +- src/ipc/collisions/tangential/edge_vertex.cpp | 6 +- src/ipc/collisions/tangential/edge_vertex.hpp | 3 +- src/ipc/collisions/tangential/face_vertex.cpp | 6 +- src/ipc/collisions/tangential/face_vertex.hpp | 3 +- .../tangential/tangential_collision.cpp | 7 +-- .../tangential/tangential_collision.hpp | 4 +- .../tangential/tangential_collisions.cpp | 13 ++--- .../tangential/tangential_collisions.hpp | 12 +--- .../collisions/tangential/vertex_vertex.cpp | 6 +- .../collisions/tangential/vertex_vertex.hpp | 3 +- src/ipc/potentials/barrier_potential.cpp | 27 +++++---- src/ipc/potentials/barrier_potential.hpp | 37 +++++++++--- .../potentials/normal_adhesion_potential.cpp | 7 +-- .../potentials/normal_adhesion_potential.hpp | 9 +-- src/ipc/potentials/normal_potential.hpp | 9 +-- src/ipc/potentials/tangential_potential.cpp | 19 +++--- src/ipc/potentials/tangential_potential.hpp | 8 --- tests/src/tests/barrier/test_barrier.cpp | 12 ++-- .../collisions/test_normal_collisions.cpp | 4 +- .../tests/friction/test_force_jacobian.cpp | 44 +++++++------- tests/src/tests/friction/test_friction.cpp | 2 +- .../potential/test_adhesion_potentials.cpp | 6 +- .../potential/test_barrier_potential.cpp | 13 +++-- .../potential/test_friction_potential.cpp | 2 +- 39 files changed, 236 insertions(+), 242 deletions(-) rename {notebooks => python/examples}/ogc.py (99%) diff --git a/docs/source/tutorials/getting_started.rst b/docs/source/tutorials/getting_started.rst index 82f858810..0f945cc37 100644 --- a/docs/source/tutorials/getting_started.rst +++ b/docs/source/tutorials/getting_started.rst @@ -117,28 +117,26 @@ Now we can compute the barrier potential using the ``BarrierPotential`` class. .. code-block:: c++ - const ipc::BarrierPotential B(dhat); + const ipc::BarrierPotential B(dhat, stiffness); double barrier_potential = B(collisions, collision_mesh, vertices); .. md-tab-item:: Python .. code-block:: python - B = ipctk.BarrierPotential(dhat) + B = ipctk.BarrierPotential(dhat, stiffness) barrier_potential = B(collisions, collision_mesh, vertices) +Here ``stiffness`` (:math:`\kappa`) is the barrier stiffness (a weight that is multiplied by the barrier potential to better scale it relative to the energy potential). This returns a scalar value ``barrier_potential`` which is the sum of the barrier potentials for each active collision. Mathematically this is defined as .. math:: - B(\mathbf{x}) = \sum_{k \in C} b(d_k(\mathbf{x}); \hat{d}), + B(\mathbf{x}) = \sum_{k \in C} \kappa b(d_k(\mathbf{x}); \hat{d}), where :math:`\mathbf{x}` is our deformed vertex positions, :math:`C` is the active collisions, :math:`d_k` is the distance (squared) of the :math:`k`-th active collision, and :math:`b` is IPC's C2-clamped log-barrier function. -.. note:: - This is **not** premultiplied by the barrier stiffness :math:`\kappa`. - Barrier Potential Derivatives ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/docs/source/tutorials/simulation.rst b/docs/source/tutorials/simulation.rst index f0b306e9c..7faf069be 100644 --- a/docs/source/tutorials/simulation.rst +++ b/docs/source/tutorials/simulation.rst @@ -78,7 +78,7 @@ This ``CollisionMesh`` can then be used just as any other ``CollisionMesh``. How collisions.build(collision_mesh, vertices, dhat); // Construct a barrier potential - ipc::BarrierPotential B(dhat); + ipc::BarrierPotential B(dhat, stiffness); // Evaluate the potential double b = B(collisions, collision_mesh, vertices); @@ -88,7 +88,7 @@ This ``CollisionMesh`` can then be used just as any other ``CollisionMesh``. How // Construct the set of friction collisions ipc::TangentialCollisions tangential_collisions; - tangential_collisions.build(collision_mesh, vertices, collisions, B, barrier_stiffness, mu); + tangential_collisions.build(collision_mesh, vertices, collisions, B, mu); // Construct a friction dissipative potential ipc::FrictionPotential D(eps_v); @@ -107,7 +107,7 @@ This ``CollisionMesh`` can then be used just as any other ``CollisionMesh``. How collisions.build(collision_mesh, vertices, dhat) # Construct a barrier potential - B = ipctk.BarrierPotential(dhat) + B = ipctk.BarrierPotential(dhat, stiffness) # Evaluate the potential b = B(collisions, collision_mesh, vertices) @@ -117,7 +117,7 @@ This ``CollisionMesh`` can then be used just as any other ``CollisionMesh``. How # Construct the set of friction collisions tangential_collisions = ipctk.TangentialCollisions() - tangential_collisions.build(collision_mesh, vertices, collisions, B, barrier_stiffness, mu) + tangential_collisions.build(collision_mesh, vertices, collisions, B, mu) # Construct a friction dissipative potential D = ipctk.FrictionPotential(eps_v) @@ -132,7 +132,7 @@ When computing the gradient and Hessian of the potentials, the derivatives will .. code-block:: c++ - const BarrierPotential B(dhat); + const BarrierPotential B(dhat, stiffness); Eigen::VectorXd grad = B.gradient(collisions, collision_mesh, vertices); Eigen::VectorXd grad_full = collision_mesh.to_full_dof(grad); @@ -144,7 +144,7 @@ When computing the gradient and Hessian of the potentials, the derivatives will .. code-block:: python - B = BarrierPotential(dhat) + B = BarrierPotential(dhat, stiffness) grad = B.gradient(collision, collision_mesh, vertices) grad_full = collision_mesh.to_full_dof(grad) diff --git a/notebooks/ogc.py b/python/examples/ogc.py similarity index 99% rename from notebooks/ogc.py rename to python/examples/ogc.py index 9cb3d94f9..6bb992e49 100644 --- a/notebooks/ogc.py +++ b/python/examples/ogc.py @@ -56,9 +56,8 @@ def contact_force(): collisions.use_area_weighting = use_area_weighting collisions.build(candidates, cmesh, cmesh.rest_positions, dhat) - B = ipctk.BarrierPotential(dhat) + B = ipctk.BarrierPotential(dhat, stiffness=1e5) f = -B.gradient(collisions, cmesh, cmesh.rest_positions).reshape(-1, 3, order="C") - f *= 1e5 return f diff --git a/python/examples/solver.py b/python/examples/solver.py index 3ef1a1c9f..e3f4f49cc 100644 --- a/python/examples/solver.py +++ b/python/examples/solver.py @@ -1,8 +1,8 @@ import pathlib + +import meshio import numpy as np import scipy -import meshio - from find_ipctk import ipctk root = pathlib.Path(__file__).parents[2] @@ -17,12 +17,12 @@ class Inertia: def __init__(self): import igl + mid_x = (vertices[:, 0].min() + vertices[:, 0].max()) / 2 self.xhat = vertices.copy() self.xhat[vertices[:, 0] < mid_x, 0] += 0.2 self.xhat[vertices[:, 0] > mid_x, 0] -= 0.2 - self.mass = igl.massmatrix( - vertices, faces, igl.MASSMATRIX_TYPE_VORONOI) + self.mass = igl.massmatrix(vertices, faces, igl.MASSMATRIX_TYPE_VORONOI) self.mass = np.repeat(self.mass.diagonal(), 3) self.mass = scipy.sparse.diags(self.mass) @@ -55,31 +55,31 @@ def hessian(self, x): C.build(collision_mesh, vertices, dhat) # Create a barrier potential -B = ipctk.BarrierPotential(dhat, use_physical_barrier=True) +B = ipctk.BarrierPotential(dhat, kappa, use_physical_barrier=True) # Initial energy and gradient prev_energy = ( - inertia(vertices) - + kappa * B(C, collision_mesh, vertices) + inertia(vertices) + B(C, collision_mesh, vertices) # + ... ) grad = ( - inertia.gradient(vertices) - + kappa * B.gradient(C, collision_mesh, vertices) + inertia.gradient(vertices) + B.gradient(C, collision_mesh, vertices) # + ... ) iter = 0 while np.linalg.norm(grad) > 1e-3 and iter < 1000: - meshio.Mesh(vertices, {"triangle": faces}).write( - f"solver-step-{iter:03d}.ply") + meshio.Mesh(vertices, {"triangle": faces}).write(f"solver-step-{iter:03d}.ply") # Compute the Hessian hess = ( inertia.hessian(vertices) - + kappa * B.hessian( - C, collision_mesh, vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP) + + B.hessian( + C, + collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, + ) # + ... ) @@ -98,7 +98,8 @@ def hessian(self, x): # Initial step size for line search (ensure no collisions) alpha = candidates.compute_collision_free_stepsize( - collision_mesh, vertices, next_vertices) + collision_mesh, vertices, next_vertices + ) # Backtrack until the barrier potential decreases condition = True @@ -107,8 +108,7 @@ def hessian(self, x): next_vertices = vertices + alpha * dx.reshape(-1, 3, order="C") C.build(candidates, collision_mesh, next_vertices, dhat) curr_energy = ( - inertia(next_vertices) - + kappa * B(C, collision_mesh, next_vertices) + inertia(next_vertices) + B(C, collision_mesh, next_vertices) # + ... ) condition = curr_energy > prev_energy and ls_iter < 100 @@ -120,8 +120,7 @@ def hessian(self, x): prev_energy = curr_energy grad = ( - inertia.gradient(vertices) - + kappa * B.gradient(C, collision_mesh, vertices) + inertia.gradient(vertices) + B.gradient(C, collision_mesh, vertices) # + ... ) diff --git a/python/src/collisions/tangential/edge_edge.cpp b/python/src/collisions/tangential/edge_edge.cpp index e91a57402..8441f75aa 100644 --- a/python/src/collisions/tangential/edge_edge.cpp +++ b/python/src/collisions/tangential/edge_edge.cpp @@ -13,7 +13,6 @@ void define_edge_edge_tangential_collision(py::module_& m) .def( py::init< const EdgeEdgeNormalCollision&, Eigen::ConstRef, - const NormalPotential&, const double>(), - "collision"_a, "positions"_a, "normal_potential"_a, - "normal_stiffness"_a); + const NormalPotential&>(), + "collision"_a, "positions"_a, "normal_potential"_a); } diff --git a/python/src/collisions/tangential/edge_vertex.cpp b/python/src/collisions/tangential/edge_vertex.cpp index d36cfbcd5..265cb2fff 100644 --- a/python/src/collisions/tangential/edge_vertex.cpp +++ b/python/src/collisions/tangential/edge_vertex.cpp @@ -13,7 +13,6 @@ void define_edge_vertex_tangential_collision(py::module_& m) .def( py::init< const EdgeVertexNormalCollision&, Eigen::ConstRef, - const NormalPotential&, const double>(), - "collision"_a, "positions"_a, "normal_potential"_a, - "normal_stiffness"_a); + const NormalPotential&>(), + "collision"_a, "positions"_a, "normal_potential"_a); } diff --git a/python/src/collisions/tangential/face_vertex.cpp b/python/src/collisions/tangential/face_vertex.cpp index 9df8b4206..315460f1e 100644 --- a/python/src/collisions/tangential/face_vertex.cpp +++ b/python/src/collisions/tangential/face_vertex.cpp @@ -13,7 +13,6 @@ void define_face_vertex_tangential_collision(py::module_& m) .def( py::init< const FaceVertexNormalCollision&, Eigen::ConstRef, - const NormalPotential&, const double>(), - "collision"_a, "positions"_a, "normal_potential"_a, - "normal_stiffness"_a); + const NormalPotential&>(), + "collision"_a, "positions"_a, "normal_potential"_a); } diff --git a/python/src/collisions/tangential/tangential_collisions.cpp b/python/src/collisions/tangential/tangential_collisions.cpp index 15aa015b9..94c5cba23 100644 --- a/python/src/collisions/tangential/tangential_collisions.cpp +++ b/python/src/collisions/tangential/tangential_collisions.cpp @@ -12,44 +12,42 @@ void define_tangential_collisions(py::module_& m) "build", py::overload_cast< const CollisionMesh&, Eigen::ConstRef, - const NormalCollisions&, const NormalPotential&, double, - double>(&TangentialCollisions::build), + const NormalCollisions&, const NormalPotential&, double>( + &TangentialCollisions::build), "mesh"_a, "vertices"_a, "collisions"_a, "normal_potential"_a, - "normal_stiffness"_a, "mu"_a) + "mu"_a) .def( "build", py::overload_cast< const CollisionMesh&, Eigen::ConstRef, - const NormalCollisions&, const NormalPotential&, double, double, + const NormalCollisions&, const NormalPotential&, double, double>(&TangentialCollisions::build), "mesh"_a, "vertices"_a, "collisions"_a, "normal_potential"_a, - "normal_stiffness"_a, "mu_s"_a, "mu_k"_a) + "mu_s"_a, "mu_k"_a) .def( "build", [](TangentialCollisions& self, const CollisionMesh& mesh, Eigen::ConstRef vertices, const NormalCollisions& collisions, const NormalPotential& normal_potential, - const double normal_stiffness, Eigen::ConstRef mu_s, Eigen::ConstRef mu_k) { self.build( - mesh, vertices, collisions, normal_potential, - normal_stiffness, mu_s, mu_k); + mesh, vertices, collisions, normal_potential, mu_s, mu_k); }, "mesh"_a, "vertices"_a, "collisions"_a, "normal_potential"_a, - "normal_stiffness"_a, "mu_s"_a, "mu_k"_a) + "mu_s"_a, "mu_k"_a) .def( "build", py::overload_cast< const CollisionMesh&, Eigen::ConstRef, - const NormalCollisions&, const NormalPotential&, const double, + const NormalCollisions&, const NormalPotential&, Eigen::ConstRef, Eigen::ConstRef, const std::function&>( &TangentialCollisions::build), "mesh"_a, "vertices"_a, "collisions"_a, "normal_potential"_a, - "normal_stiffness"_a, "mu_s"_a, "mu_k"_a, "blend_mu"_a) + "mu_s"_a, "mu_k"_a, "blend_mu"_a) .def( "__len__", &TangentialCollisions::size, "Get the number of friction collisions.") diff --git a/python/src/collisions/tangential/vertex_vertex.cpp b/python/src/collisions/tangential/vertex_vertex.cpp index 468977e95..f6a9c5142 100644 --- a/python/src/collisions/tangential/vertex_vertex.cpp +++ b/python/src/collisions/tangential/vertex_vertex.cpp @@ -13,8 +13,6 @@ void define_vertex_vertex_tangential_collision(py::module_& m) .def( py::init< const VertexVertexNormalCollision&, - Eigen::ConstRef, const NormalPotential&, - const double>(), - "collision"_a, "positions"_a, "normal_potential"_a, - "normal_stiffness"_a); + Eigen::ConstRef, const NormalPotential&>(), + "collision"_a, "positions"_a, "normal_potential"_a); } diff --git a/python/src/potentials/barrier_potential.cpp b/python/src/potentials/barrier_potential.cpp index 70e027c5c..1d8f74561 100644 --- a/python/src/potentials/barrier_potential.cpp +++ b/python/src/potentials/barrier_potential.cpp @@ -9,25 +9,31 @@ void define_barrier_potential(py::module_& m) { py::class_(m, "BarrierPotential") .def( - py::init(), + py::init(), R"ipc_Qu8mg5v7( Construct a barrier potential. Parameters: dhat: The activation distance of the barrier. + stiffness: The stiffness of the barrier. + use_physical_barrier: Whether to use the physical barrier. )ipc_Qu8mg5v7", - "dhat"_a, "use_physical_barrier"_a = false) + "dhat"_a, "stiffness"_a, "use_physical_barrier"_a = false) .def( py::init< - const std::shared_ptr, const double, const bool>(), + const std::shared_ptr, const double, const double, + const bool>(), R"ipc_Qu8mg5v7( Construct a barrier potential. Parameters: barrier: The barrier function. dhat: The activation distance of the barrier. + stiffness: The stiffness of the barrier. + use_physical_barrier: Whether to use the physical barrier. )ipc_Qu8mg5v7", - "barrier"_a, "dhat"_a, "use_physical_barrier"_a = false) + "barrier"_a, "dhat"_a, "stiffness"_a, + "use_physical_barrier"_a = false) .def_property( "dhat", &BarrierPotential::dhat, &BarrierPotential::set_dhat, "Barrier activation distance.") diff --git a/python/src/potentials/normal_potential.cpp b/python/src/potentials/normal_potential.cpp index 46bbdd6cc..2d5189f5b 100644 --- a/python/src/potentials/normal_potential.cpp +++ b/python/src/potentials/normal_potential.cpp @@ -67,7 +67,7 @@ void define_normal_potential(py::module_& m) Returns: The force magnitude. )ipc_Qu8mg5v7", - "distance_squared"_a, "dmin"_a, "barrier_stiffness"_a) + "distance_squared"_a, "dmin"_a) .def( "force_magnitude_gradient", &NormalPotential::force_magnitude_gradient, @@ -78,11 +78,9 @@ void define_normal_potential(py::module_& m) distance_squared: The squared distance between elements. distance_squared_gradient: The gradient of the squared distance. dmin: The minimum distance offset to the barrier. - barrier_stiffness: The stiffness of the barrier. Returns: The gradient of the force. )ipc_Qu8mg5v7", - "distance_squared"_a, "distance_squared_gradient"_a, "dmin"_a, - "barrier_stiffness"_a); + "distance_squared"_a, "distance_squared_gradient"_a, "dmin"_a); } diff --git a/python/src/potentials/tangential_potential.cpp b/python/src/potentials/tangential_potential.cpp index 8e53c3dae..630f84be6 100644 --- a/python/src/potentials/tangential_potential.cpp +++ b/python/src/potentials/tangential_potential.cpp @@ -33,7 +33,7 @@ void define_tangential_potential(py::module_& m) Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, const NormalPotential&, - const double, const double, const bool>( + const double, const bool>( &TangentialPotential::force, py::const_), R"ipc_Qu8mg5v7( Compute the friction force from the given velocities. @@ -45,7 +45,6 @@ void define_tangential_potential(py::module_& m) lagged_displacements: Previous displacements of the vertices (rowwise). velocities: Current displacements of the vertices (rowwise). normal_potential: Normal potential (used for normal force magnitude). - normal_stiffness: Normal stiffness (used for normal force magnitude). dmin: Minimum distance (used for normal force magnitude). no_mu: whether to not multiply by mu @@ -54,7 +53,7 @@ void define_tangential_potential(py::module_& m) )ipc_Qu8mg5v7", "collisions"_a, "mesh"_a, "rest_positions"_a, "lagged_displacements"_a, "velocities"_a, "normal_potential"_a, - "normal_stiffness"_a, "dmin"_a = 0, "no_mu"_a = false) + "dmin"_a = 0, "no_mu"_a = false) .def( "force_jacobian", py::overload_cast< @@ -62,7 +61,7 @@ void define_tangential_potential(py::module_& m) Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, const NormalPotential&, - const double, const TangentialPotential::DiffWRT, const double>( + const TangentialPotential::DiffWRT, const double>( &TangentialPotential::force_jacobian, py::const_), R"ipc_Qu8mg5v7( Compute the Jacobian of the friction force wrt the velocities. @@ -74,7 +73,6 @@ void define_tangential_potential(py::module_& m) lagged_displacements: Previous displacements of the vertices (rowwise). velocities: Current displacements of the vertices (rowwise). normal_potential: Normal potential (used for normal force magnitude). - normal_stiffness: Normal stiffness (used for normal force magnitude). wrt: The variable to take the derivative with respect to. dmin: Minimum distance (used for normal force magnitude). @@ -83,13 +81,13 @@ void define_tangential_potential(py::module_& m) )ipc_Qu8mg5v7", "collisions"_a, "mesh"_a, "rest_positions"_a, "lagged_displacements"_a, "velocities"_a, "normal_potential"_a, - "normal_stiffness"_a, "wrt"_a, "dmin"_a = 0) + "wrt"_a, "dmin"_a = 0) .def( "force", py::overload_cast< const TangentialCollision&, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, - const NormalPotential&, const double, const double, const bool>( + const NormalPotential&, const double, const bool>( &TangentialPotential::force, py::const_), R"ipc_Qu8mg5v7( Compute the friction force. @@ -100,7 +98,6 @@ void define_tangential_potential(py::module_& m) lagged_displacements: Previous displacements of the vertices (rowwise). velocities: Current displacements of the vertices (rowwise). normal_potential: Normal potential (used for normal force magnitude). - normal_stiffness: Normal stiffness (used for normal force magnitude). dmin: Minimum distance (used for normal force magnitude). no_mu: Whether to not multiply by mu @@ -108,16 +105,15 @@ void define_tangential_potential(py::module_& m) Friction force )ipc_Qu8mg5v7", "collision"_a, "rest_positions"_a, "lagged_displacements"_a, - "velocities"_a, "normal_potential"_a, "normal_stiffness"_a, - "dmin"_a = 0, "no_mu"_a = false) + "velocities"_a, "normal_potential"_a, "dmin"_a = 0, + "no_mu"_a = false) .def( "force_jacobian", py::overload_cast< const TangentialCollision&, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, - const NormalPotential&, const double, - const TangentialPotential::DiffWRT, const double>( - &TangentialPotential::force_jacobian, py::const_), + const NormalPotential&, const TangentialPotential::DiffWRT, + const double>(&TangentialPotential::force_jacobian, py::const_), R"ipc_Qu8mg5v7( Compute the friction force Jacobian. @@ -127,7 +123,6 @@ void define_tangential_potential(py::module_& m) lagged_displacements: Previous displacements of the vertices (rowwise). velocities: Current displacements of the vertices (rowwise). normal_potential: Normal potential (used for normal force magnitude). - normal_stiffness: Normal stiffness (used for normal force magnitude). wrt: Variable to differentiate the friction force with respect to. dmin: Minimum distance (used for normal force magnitude). @@ -135,6 +130,5 @@ void define_tangential_potential(py::module_& m) Friction force Jacobian )ipc_Qu8mg5v7", "collision"_a, "rest_positions"_a, "lagged_displacements"_a, - "velocities"_a, "normal_potential"_a, "normal_stiffness"_a, "wrt"_a, - "dmin"_a = 0); + "velocities"_a, "normal_potential"_a, "wrt"_a, "dmin"_a = 0); } diff --git a/python/tests/test_ipc.py b/python/tests/test_ipc.py index 4e9b92482..f269c0c88 100644 --- a/python/tests/test_ipc.py +++ b/python/tests/test_ipc.py @@ -1,11 +1,11 @@ import numpy as np - -from find_ipctk import ipctk - import utils +from find_ipctk import ipctk -def check_ipc_derivatives(broad_phase, use_convergent_formulation, mesh_name, dhat, all_vertices_on_surface): +def check_ipc_derivatives( + broad_phase, use_convergent_formulation, mesh_name, dhat, all_vertices_on_surface +): vertices, edges, faces = utils.load_mesh(mesh_name) if all_vertices_on_surface: @@ -17,25 +17,31 @@ def check_ipc_derivatives(broad_phase, use_convergent_formulation, mesh_name, dh collisions = ipctk.NormalCollisions() collisions.use_area_weighting = use_convergent_formulation if use_convergent_formulation: - collisions.collision_set_type = ipctk.NormalCollisions.CollisionSetType.IMPROVED_MAX_APPROX + collisions.collision_set_type = ( + ipctk.NormalCollisions.CollisionSetType.IMPROVED_MAX_APPROX + ) else: collisions.collision_set_type = ipctk.NormalCollisions.CollisionSetType.IPC collisions.build(mesh, vertices, dhat, broad_phase=broad_phase) assert len(collisions) > 0 B = ipctk.BarrierPotential( - dhat, use_physical_barrier=use_convergent_formulation) + dhat, stiffness=1, use_physical_barrier=use_convergent_formulation + ) grad_b = B.gradient(collisions, mesh, vertices) fgrad_b = utils.finite_gradient( - vertices.flatten(), lambda x: B(collisions, mesh, x.reshape(vertices.shape))) + vertices.flatten(), lambda x: B(collisions, mesh, x.reshape(vertices.shape)) + ) assert np.linalg.norm(grad_b) > 1e-8 assert np.allclose(grad_b, fgrad_b) hess_b = B.hessian(collisions, mesh, vertices).toarray() fhess_b = utils.finite_jacobian( - vertices.flatten(), lambda x: B.gradient(collisions, mesh, x.reshape(vertices.shape))) + vertices.flatten(), + lambda x: B.gradient(collisions, mesh, x.reshape(vertices.shape)), + ) assert np.linalg.norm(hess_b) > 1e-8 assert np.allclose(hess_b, fhess_b, atol=1e-5) @@ -44,7 +50,35 @@ def check_ipc_derivatives(broad_phase, use_convergent_formulation, mesh_name, dh def test_ipc(): for method in utils.broad_phases(): for use_convergent_formulation in (True, False): - yield check_ipc_derivatives, method, use_convergent_formulation, "cube.ply", np.sqrt(2.0), True - yield check_ipc_derivatives, method, use_convergent_formulation, "two-cubes-far.ply", 1e-1, False - yield check_ipc_derivatives, method, use_convergent_formulation, "two-cubes-close.ply", 1e-1, False - yield check_ipc_derivatives, method, use_convergent_formulation, "bunny.ply", 5e-3, True + yield ( + check_ipc_derivatives, + method, + use_convergent_formulation, + "cube.ply", + np.sqrt(2.0), + True, + ) + yield ( + check_ipc_derivatives, + method, + use_convergent_formulation, + "two-cubes-far.ply", + 1e-1, + False, + ) + yield ( + check_ipc_derivatives, + method, + use_convergent_formulation, + "two-cubes-close.ply", + 1e-1, + False, + ) + yield ( + check_ipc_derivatives, + method, + use_convergent_formulation, + "bunny.ply", + 5e-3, + True, + ) diff --git a/src/ipc/collisions/tangential/edge_edge.cpp b/src/ipc/collisions/tangential/edge_edge.cpp index 56dfc9dd6..6c49bfc81 100644 --- a/src/ipc/collisions/tangential/edge_edge.cpp +++ b/src/ipc/collisions/tangential/edge_edge.cpp @@ -27,12 +27,10 @@ EdgeEdgeTangentialCollision::EdgeEdgeTangentialCollision( EdgeEdgeTangentialCollision::EdgeEdgeTangentialCollision( const EdgeEdgeNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness) + const NormalPotential& normal_potential) : EdgeEdgeTangentialCollision(collision) { - TangentialCollision::init( - collision, positions, normal_potential, normal_stiffness); + TangentialCollision::init(collision, positions, normal_potential); } // ============================================================================ diff --git a/src/ipc/collisions/tangential/edge_edge.hpp b/src/ipc/collisions/tangential/edge_edge.hpp index f032face1..88fbd8ba8 100644 --- a/src/ipc/collisions/tangential/edge_edge.hpp +++ b/src/ipc/collisions/tangential/edge_edge.hpp @@ -21,8 +21,7 @@ class EdgeEdgeTangentialCollision : public EdgeEdgeCandidate, EdgeEdgeTangentialCollision( const EdgeEdgeNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness); + const NormalPotential& normal_potential); protected: EdgeEdgeDistanceType known_dtype() const override diff --git a/src/ipc/collisions/tangential/edge_vertex.cpp b/src/ipc/collisions/tangential/edge_vertex.cpp index 97e520abf..602fa81dc 100644 --- a/src/ipc/collisions/tangential/edge_vertex.cpp +++ b/src/ipc/collisions/tangential/edge_vertex.cpp @@ -27,12 +27,10 @@ EdgeVertexTangentialCollision::EdgeVertexTangentialCollision( EdgeVertexTangentialCollision::EdgeVertexTangentialCollision( const EdgeVertexNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness) + const NormalPotential& normal_potential) : EdgeVertexTangentialCollision(collision) { - TangentialCollision::init( - collision, positions, normal_potential, normal_stiffness); + TangentialCollision::init(collision, positions, normal_potential); } // ============================================================================ diff --git a/src/ipc/collisions/tangential/edge_vertex.hpp b/src/ipc/collisions/tangential/edge_vertex.hpp index 70bd1798d..2f8e0d9f2 100644 --- a/src/ipc/collisions/tangential/edge_vertex.hpp +++ b/src/ipc/collisions/tangential/edge_vertex.hpp @@ -21,8 +21,7 @@ class EdgeVertexTangentialCollision : public EdgeVertexCandidate, EdgeVertexTangentialCollision( const EdgeVertexNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness); + const NormalPotential& normal_potential); protected: MatrixMax compute_tangent_basis( diff --git a/src/ipc/collisions/tangential/face_vertex.cpp b/src/ipc/collisions/tangential/face_vertex.cpp index 0f0c01cb3..82c935cdd 100644 --- a/src/ipc/collisions/tangential/face_vertex.cpp +++ b/src/ipc/collisions/tangential/face_vertex.cpp @@ -27,12 +27,10 @@ FaceVertexTangentialCollision::FaceVertexTangentialCollision( FaceVertexTangentialCollision::FaceVertexTangentialCollision( const FaceVertexNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness) + const NormalPotential& normal_potential) : FaceVertexTangentialCollision(collision) { - TangentialCollision::init( - collision, positions, normal_potential, normal_stiffness); + TangentialCollision::init(collision, positions, normal_potential); } // ============================================================================ diff --git a/src/ipc/collisions/tangential/face_vertex.hpp b/src/ipc/collisions/tangential/face_vertex.hpp index f2dd9d469..34f11e83a 100644 --- a/src/ipc/collisions/tangential/face_vertex.hpp +++ b/src/ipc/collisions/tangential/face_vertex.hpp @@ -21,8 +21,7 @@ class FaceVertexTangentialCollision : public FaceVertexCandidate, FaceVertexTangentialCollision( const FaceVertexNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness); + const NormalPotential& normal_potential); protected: MatrixMax compute_tangent_basis( diff --git a/src/ipc/collisions/tangential/tangential_collision.cpp b/src/ipc/collisions/tangential/tangential_collision.cpp index 8b840e96b..a38c445a6 100644 --- a/src/ipc/collisions/tangential/tangential_collision.cpp +++ b/src/ipc/collisions/tangential/tangential_collision.cpp @@ -2,20 +2,17 @@ #include -#include // std::out_of_range - namespace ipc { void TangentialCollision::init( const NormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness) + const NormalPotential& normal_potential) { init( collision, positions, normal_potential.force_magnitude( - compute_distance(positions), collision.dmin, normal_stiffness)); + compute_distance(positions), collision.dmin)); } void TangentialCollision::init( diff --git a/src/ipc/collisions/tangential/tangential_collision.hpp b/src/ipc/collisions/tangential/tangential_collision.hpp index 1b3d6e0da..8b34edecb 100644 --- a/src/ipc/collisions/tangential/tangential_collision.hpp +++ b/src/ipc/collisions/tangential/tangential_collision.hpp @@ -14,12 +14,10 @@ class TangentialCollision : virtual public CollisionStencil { /// @param collision NormalCollision stencil. /// @param positions Collision stencil's vertex positions. /// @param normal_potential Normal potential used for normal force. - /// @param normal_stiffness Normal potential stiffness. void init( const NormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness); + const NormalPotential& normal_potential); /// @brief Initialize the collision. /// @param collision NormalCollision stencil. diff --git a/src/ipc/collisions/tangential/tangential_collisions.cpp b/src/ipc/collisions/tangential/tangential_collisions.cpp index 2034cc174..0c44cdeb6 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.cpp +++ b/src/ipc/collisions/tangential/tangential_collisions.cpp @@ -16,7 +16,6 @@ void TangentialCollisions::build( Eigen::ConstRef vertices, const NormalCollisions& collisions, const NormalPotential& normal_potential, - const double normal_stiffness, Eigen::ConstRef mu_s, Eigen::ConstRef mu_k, const std::function& blend_mu) @@ -38,8 +37,7 @@ void TangentialCollisions::build( FC_vv.reserve(C_vv.size()); for (const auto& c_vv : C_vv) { FC_vv.emplace_back( - c_vv, c_vv.dof(vertices, edges, faces), normal_potential, - normal_stiffness); + c_vv, c_vv.dof(vertices, edges, faces), normal_potential); const auto& [v0i, v1i, _, __] = FC_vv.back().vertex_ids(edges, faces); FC_vv.back().mu_s = blend_mu(mu_s(v0i), mu_s(v1i)); @@ -49,8 +47,7 @@ void TangentialCollisions::build( FC_ev.reserve(C_ev.size()); for (const auto& c_ev : C_ev) { FC_ev.emplace_back( - c_ev, c_ev.dof(vertices, edges, faces), normal_potential, - normal_stiffness); + c_ev, c_ev.dof(vertices, edges, faces), normal_potential); const auto& [vi, e0i, e1i, _] = FC_ev.back().vertex_ids(edges, faces); const double edge_mu_s = @@ -75,8 +72,7 @@ void TangentialCollisions::build( } FC_ee.emplace_back( - c_ee, c_ee.dof(vertices, edges, faces), normal_potential, - normal_stiffness); + c_ee, c_ee.dof(vertices, edges, faces), normal_potential); double ea_mu_s = (mu_s(ea1i) - mu_s(ea0i)) * FC_ee.back().closest_point[0] @@ -98,8 +94,7 @@ void TangentialCollisions::build( FC_fv.reserve(C_fv.size()); for (const auto& c_fv : C_fv) { FC_fv.emplace_back( - c_fv, c_fv.dof(vertices, edges, faces), normal_potential, - normal_stiffness); + c_fv, c_fv.dof(vertices, edges, faces), normal_potential); const auto& [vi, f0i, f1i, f2i] = FC_fv.back().vertex_ids(edges, faces); double face_mu_s = mu_s(f0i) diff --git a/src/ipc/collisions/tangential/tangential_collisions.hpp b/src/ipc/collisions/tangential/tangential_collisions.hpp index 9f6c13196..5ba55125b 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.hpp +++ b/src/ipc/collisions/tangential/tangential_collisions.hpp @@ -28,19 +28,15 @@ class TangentialCollisions { /// @param vertices The vertices of the mesh. /// @param collisions The set of normal collisions. /// @param normal_potential The normal potential. - /// @param normal_stiffness Stiffness of the normal potential. /// @param mu The coefficient of friction. void build( const CollisionMesh& mesh, Eigen::ConstRef vertices, const NormalCollisions& collisions, const NormalPotential& normal_potential, - double normal_stiffness, double mu) { - this->build( - mesh, vertices, collisions, normal_potential, normal_stiffness, mu, - mu); + this->build(mesh, vertices, collisions, normal_potential, mu, mu); } /// @brief Build the tangential collisions. @@ -48,7 +44,6 @@ class TangentialCollisions { /// @param vertices The vertices of the mesh. /// @param collisions The set of normal collisions. /// @param normal_potential The normal potential. - /// @param normal_stiffness Stiffness of the normal potential. /// @param mu_s The static friction coefficient. /// @param mu_k The kinetic friction coefficient. void build( @@ -56,12 +51,11 @@ class TangentialCollisions { Eigen::ConstRef vertices, const NormalCollisions& collisions, const NormalPotential& normal_potential, - double normal_stiffness, double mu_s, double mu_k) { this->build( - mesh, vertices, collisions, normal_potential, normal_stiffness, + mesh, vertices, collisions, normal_potential, Eigen::VectorXd::Constant(vertices.rows(), mu_s), Eigen::VectorXd::Constant(vertices.rows(), mu_k)); } @@ -71,7 +65,6 @@ class TangentialCollisions { /// @param vertices The vertices of the mesh. /// @param collisions The set of normal collisions. /// @param normal_potential The normal potential. - /// @param normal_stiffness Stiffness of the normal potential. /// @param mu_k The kinetic friction coefficient per vertex. /// @param mu_s The static friction coefficient per vertex. /// @param blend_mu Function to blend vertex-based coefficients of friction. @@ -80,7 +73,6 @@ class TangentialCollisions { Eigen::ConstRef vertices, const NormalCollisions& collisions, const NormalPotential& normal_potential, - const double normal_stiffness, Eigen::ConstRef mu_s, Eigen::ConstRef mu_k, const std::function& blend_mu = diff --git a/src/ipc/collisions/tangential/vertex_vertex.cpp b/src/ipc/collisions/tangential/vertex_vertex.cpp index 2075627e7..975a05da8 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.cpp +++ b/src/ipc/collisions/tangential/vertex_vertex.cpp @@ -27,12 +27,10 @@ VertexVertexTangentialCollision::VertexVertexTangentialCollision( VertexVertexTangentialCollision::VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness) + const NormalPotential& normal_potential) : VertexVertexTangentialCollision(collision) { - TangentialCollision::init( - collision, positions, normal_potential, normal_stiffness); + TangentialCollision::init(collision, positions, normal_potential); } // ============================================================================ diff --git a/src/ipc/collisions/tangential/vertex_vertex.hpp b/src/ipc/collisions/tangential/vertex_vertex.hpp index 92e1f0ae3..92f7133bf 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.hpp +++ b/src/ipc/collisions/tangential/vertex_vertex.hpp @@ -22,8 +22,7 @@ class VertexVertexTangentialCollision : public VertexVertexCandidate, VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision, Eigen::ConstRef positions, - const NormalPotential& normal_potential, - const double normal_stiffness); + const NormalPotential& normal_potential); protected: MatrixMax compute_tangent_basis( diff --git a/src/ipc/potentials/barrier_potential.cpp b/src/ipc/potentials/barrier_potential.cpp index a9aeb1ddf..47bb32dbb 100644 --- a/src/ipc/potentials/barrier_potential.cpp +++ b/src/ipc/potentials/barrier_potential.cpp @@ -6,31 +6,35 @@ namespace ipc { BarrierPotential::BarrierPotential( - const double dhat, const bool use_physical_barrier) + const double dhat, const double stiffness, const bool use_physical_barrier) : BarrierPotential( - std::make_shared(), dhat, use_physical_barrier) + std::make_shared(), + dhat, + stiffness, + use_physical_barrier) { } BarrierPotential::BarrierPotential( std::shared_ptr barrier, const double dhat, + const double stiffness, const bool use_physical_barrier) : m_barrier(std::move(barrier)) , m_dhat(dhat) + , m_stiffness(stiffness) , m_use_physical_barrier(use_physical_barrier) { assert(dhat > 0); + assert(stiffness > 0); assert(m_barrier != nullptr); } double BarrierPotential::force_magnitude( - const double distance_squared, - const double dmin, - const double barrier_stiffness) const + const double distance_squared, const double dmin) const { double N = barrier_force_magnitude( - distance_squared, barrier(), dhat(), barrier_stiffness, dmin); + distance_squared, barrier(), dhat(), stiffness(), dmin); if (use_physical_barrier()) { N *= dhat() / barrier().units((2 * dmin + dhat()) * dhat()); @@ -42,12 +46,11 @@ double BarrierPotential::force_magnitude( VectorMax12d BarrierPotential::force_magnitude_gradient( const double distance_squared, Eigen::ConstRef distance_squared_gradient, - const double dmin, - const double barrier_stiffness) const + const double dmin) const { VectorMax12d grad_N = barrier_force_magnitude_gradient( distance_squared, distance_squared_gradient, barrier(), dhat(), - barrier_stiffness, dmin); + stiffness(), dmin); if (use_physical_barrier()) { grad_N *= dhat() / barrier().units((2 * dmin + dhat()) * dhat()); @@ -66,7 +69,7 @@ double BarrierPotential::operator()( b *= dhat() / barrier().units((2 * dmin + dhat()) * dhat()); } - return b; + return stiffness() * b; } double BarrierPotential::gradient( @@ -79,7 +82,7 @@ double BarrierPotential::gradient( db *= dhat() / barrier().units((2 * dmin + dhat()) * dhat()); } - return db; + return stiffness() * db; } double BarrierPotential::hessian( @@ -92,7 +95,7 @@ double BarrierPotential::hessian( d2b *= dhat() / barrier().units((2 * dmin + dhat()) * dhat()); } - return d2b; + return stiffness() * d2b; } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/potentials/barrier_potential.hpp b/src/ipc/potentials/barrier_potential.hpp index 325708d46..06611355f 100644 --- a/src/ipc/potentials/barrier_potential.hpp +++ b/src/ipc/potentials/barrier_potential.hpp @@ -11,20 +11,30 @@ namespace ipc { class BarrierPotential : public NormalPotential { using Super = NormalPotential; + // Delete the constructor that doesn't take stiffness to avoid + // accidentally using the default stiffness value of 0.0, which + // is likely not what we want. + BarrierPotential(const double, const bool) = delete; + public: /// @brief Construct a barrier potential. /// @param dhat The activation distance of the barrier. + /// @param stiffness The stiffness of the barrier. /// @param use_physical_barrier Whether to use the physical barrier. - explicit BarrierPotential( - const double dhat, const bool use_physical_barrier = false); + BarrierPotential( + const double dhat, + const double stiffness, + const bool use_physical_barrier = false); /// @brief Construct a barrier potential. /// @param barrier The barrier function. /// @param dhat The activation distance of the barrier. + /// @param stiffness The stiffness of the barrier. /// @param use_physical_barrier Whether to use the physical barrier. BarrierPotential( std::shared_ptr barrier, const double dhat, + const double stiffness, const bool use_physical_barrier = false); /// @brief Get the activation distance of the barrier. @@ -38,6 +48,17 @@ class BarrierPotential : public NormalPotential { m_dhat = dhat; } + /// @brief Get the stiffness of the barrier. + double stiffness() const { return m_stiffness; } + + /// @brief Set the stiffness of the barrier. + /// @param stiffness The stiffness of the barrier. + void set_stiffness(const double stiffness) + { + assert(stiffness > 0); + m_stiffness = stiffness; + } + /// @brief Get the barrier function used to compute the potential. const Barrier& barrier() const { @@ -60,24 +81,19 @@ class BarrierPotential : public NormalPotential { /// @brief Compute the force magnitude for a collision. /// @param distance_squared The squared distance between elements. /// @param dmin The minimum distance offset to the barrier. - /// @param barrier_stiffness The barrier stiffness. /// @return The force magnitude. double force_magnitude( - const double distance_squared, - const double dmin, - const double barrier_stiffness) const override; + const double distance_squared, const double dmin) const override; /// @brief Compute the gradient of the force magnitude for a collision. /// @param distance_squared The squared distance between elements. /// @param distance_squared_gradient The gradient of the squared distance. /// @param dmin The minimum distance offset to the barrier. - /// @param barrier_stiffness The stiffness of the barrier. /// @return The gradient of the force. VectorMax12d force_magnitude_gradient( const double distance_squared, Eigen::ConstRef distance_squared_gradient, - const double dmin, - const double barrier_stiffness) const override; + const double dmin) const override; /// @brief Get whether to use the physical barrier. /// @note When using the convergent formulation we want the barrier to @@ -125,6 +141,9 @@ class BarrierPotential : public NormalPotential { /// @brief The activation distance of the barrier. double m_dhat; + /// @brief The stiffness of the barrier. + double m_stiffness; + /// @brief Whether to use the physical barrier. /// @note When using the convergent formulation we want the barrier to /// have units of Pa⋅m, so κ gets units of Pa and the barrier function diff --git a/src/ipc/potentials/normal_adhesion_potential.cpp b/src/ipc/potentials/normal_adhesion_potential.cpp index a8c0b8e3d..7c1afb979 100644 --- a/src/ipc/potentials/normal_adhesion_potential.cpp +++ b/src/ipc/potentials/normal_adhesion_potential.cpp @@ -17,9 +17,7 @@ NormalAdhesionPotential::NormalAdhesionPotential( } double NormalAdhesionPotential::force_magnitude( - const double /*distance_squared*/, - const double dmin, - const double /*barrier_stiffness*/) const + const double /*distance_squared*/, const double dmin) const { const auto [arg_dhat_p, arg_dhat_a, a2] = normal_adhesion_potential_args(dmin); @@ -29,8 +27,7 @@ double NormalAdhesionPotential::force_magnitude( VectorMax12d NormalAdhesionPotential::force_magnitude_gradient( const double /*distance_squared*/, Eigen::ConstRef distance_squared_gradient, - const double /*dmin*/, - const double /*barrier_stiffness*/) const + const double /*dmin*/) const { // The force magnitude is constant wrt positions, so the gradient is zero. return VectorMax12d::Zero(distance_squared_gradient.size()); diff --git a/src/ipc/potentials/normal_adhesion_potential.hpp b/src/ipc/potentials/normal_adhesion_potential.hpp index 90eea0813..bb55aa649 100644 --- a/src/ipc/potentials/normal_adhesion_potential.hpp +++ b/src/ipc/potentials/normal_adhesion_potential.hpp @@ -22,24 +22,19 @@ class NormalAdhesionPotential : public NormalPotential { /// @brief Compute the force magnitude for a collision. /// @param distance_squared The squared distance between elements. /// @param dmin The minimum distance offset to the barrier. - /// @param barrier_stiffness The barrier stiffness. /// @return The force magnitude. double force_magnitude( - const double distance_squared, - const double dmin, - const double barrier_stiffness) const override; + const double distance_squared, const double dmin) const override; /// @brief Compute the gradient of the force magnitude for a collision. /// @param distance_squared The squared distance between elements. /// @param distance_squared_gradient The gradient of the squared distance. /// @param dmin The minimum distance offset to the barrier. - /// @param barrier_stiffness The stiffness of the barrier. /// @return The gradient of the force. VectorMax12d force_magnitude_gradient( const double distance_squared, Eigen::ConstRef distance_squared_gradient, - const double dmin, - const double barrier_stiffness) const override; + const double dmin) const override; /// @brief The distance of largest adhesion force (\f$\hat{d}_p\f$) (\f$0 < \hat{d}_p < \hat{d}_a\f$). double dhat_p; diff --git a/src/ipc/potentials/normal_potential.hpp b/src/ipc/potentials/normal_potential.hpp index a4eb8ee0c..bae76a514 100644 --- a/src/ipc/potentials/normal_potential.hpp +++ b/src/ipc/potentials/normal_potential.hpp @@ -79,10 +79,8 @@ class NormalPotential : public Potential { /// @param dmin The minimum distance offset to the barrier. /// @param barrier_stiffness The barrier stiffness. /// @return The force magnitude. - virtual double force_magnitude( - const double distance_squared, - const double dmin, - const double barrier_stiffness) const = 0; + virtual double + force_magnitude(const double distance_squared, const double dmin) const = 0; /// @brief Compute the gradient of the force magnitude for a collision. /// @param distance_squared The squared distance between elements. @@ -93,8 +91,7 @@ class NormalPotential : public Potential { virtual VectorMax12d force_magnitude_gradient( const double distance_squared, Eigen::ConstRef distance_squared_gradient, - const double dmin, - const double barrier_stiffness) const = 0; + const double dmin) const = 0; protected: /// @brief Compute the unmollified distance-based potential for a collisions. diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index f1fef39c3..f594db53b 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -17,7 +17,6 @@ Eigen::VectorXd TangentialPotential::force( Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, const NormalPotential& normal_potential, - const double normal_stiffness, const double dmin, const bool no_mu) const { @@ -43,7 +42,7 @@ Eigen::VectorXd TangentialPotential::force( collision, collision.dof(rest_positions, edges, faces), collision.dof(lagged_displacements, edges, faces), collision.dof(velocities, edges, faces), // - normal_potential, normal_stiffness, dmin, no_mu); + normal_potential, dmin, no_mu); const std::array vis = collision.vertex_ids(mesh.edges(), mesh.faces()); @@ -64,7 +63,6 @@ Eigen::SparseMatrix TangentialPotential::force_jacobian( Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, const NormalPotential& normal_potential, - const double normal_stiffness, const DiffWRT wrt, const double dmin) const { @@ -92,7 +90,7 @@ Eigen::SparseMatrix TangentialPotential::force_jacobian( collision, collision.dof(rest_positions, edges, faces), collision.dof(lagged_displacements, edges, faces), collision.dof(velocities, edges, faces), // - normal_potential, normal_stiffness, wrt, dmin); + normal_potential, wrt, dmin); const std::array vis = collision.vertex_ids(mesh.edges(), mesh.faces()); @@ -125,7 +123,7 @@ Eigen::SparseMatrix TangentialPotential::force_jacobian( collision, collision.dof(rest_positions, edges, faces), collision.dof(lagged_displacements, edges, faces), collision.dof(velocities, edges, faces), // - normal_potential, normal_stiffness, dmin); + normal_potential, dmin); assert(collision.weight != 0); local_force /= collision.weight; @@ -280,7 +278,6 @@ VectorMax12d TangentialPotential::force( Eigen::ConstRef lagged_displacements, // = u Eigen::ConstRef velocities, // = v const NormalPotential& normal_potential, - const double normal_stiffness, const double dmin, const bool no_mu) const { @@ -302,7 +299,7 @@ VectorMax12d TangentialPotential::force( // Compute N(x + u) const double N = normal_potential.force_magnitude( - collision.compute_distance(lagged_positions), dmin, normal_stiffness); + collision.compute_distance(lagged_positions), dmin); // Compute P const MatrixMax P = @@ -337,7 +334,6 @@ MatrixMax12d TangentialPotential::force_jacobian( Eigen::ConstRef lagged_displacements, // = u Eigen::ConstRef velocities, // = v const NormalPotential& normal_potential, - const double normal_stiffness, const DiffWRT wrt, const double dmin) const { @@ -365,7 +361,7 @@ MatrixMax12d TangentialPotential::force_jacobian( // Compute N const double N = normal_potential.force_magnitude( - collision.compute_distance(lagged_positions), dmin, normal_stiffness); + collision.compute_distance(lagged_positions), dmin); // Compute ∇N VectorMax12d grad_N; @@ -373,8 +369,7 @@ MatrixMax12d TangentialPotential::force_jacobian( // ∇ₓN = ∇ᵤN grad_N = normal_potential.force_magnitude_gradient( collision.compute_distance(lagged_positions), - collision.compute_distance_gradient(lagged_positions), dmin, - normal_stiffness); + collision.compute_distance_gradient(lagged_positions), dmin); assert(grad_N.array().isFinite().all()); } @@ -691,7 +686,7 @@ TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( // Compute N(x + u) // const double N = collision.compute_normal_force_magnitude( - // lagged_positions, barrier_potential, barrier_stiffness, dmin); + // lagged_positions, barrier_potential, dmin); const double N = collision.normal_force_magnitude; // Compute P diff --git a/src/ipc/potentials/tangential_potential.hpp b/src/ipc/potentials/tangential_potential.hpp index 4585b4ab5..bb4e0295c 100644 --- a/src/ipc/potentials/tangential_potential.hpp +++ b/src/ipc/potentials/tangential_potential.hpp @@ -37,7 +37,6 @@ class TangentialPotential : public Potential { /// @param lagged_displacements Previous displacements of the vertices (rowwise). /// @param velocities Current displacements of the vertices (rowwise). /// @param normal_potential Normal potential (used for normal force magnitude). - /// @param normal_stiffness Normal stiffness (used for normal force magnitude). /// @param dmin Minimum distance (used for normal force magnitude). /// @param no_mu whether to not multiply by mu /// @return The friction force. @@ -48,7 +47,6 @@ class TangentialPotential : public Potential { Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, const NormalPotential& normal_potential, - const double normal_stiffness, const double dmin = 0, const bool no_mu = false) const; @@ -59,7 +57,6 @@ class TangentialPotential : public Potential { /// @param lagged_displacements Previous displacements of the vertices (rowwise). /// @param velocities Current displacements of the vertices (rowwise). /// @param normal_potential Normal potential (used for normal force magnitude). - /// @param normal_stiffness Normal stiffness (used for normal force magnitude). /// @param wrt The variable to take the derivative with respect to. /// @param dmin Minimum distance (used for normal force magnitude). /// @return The Jacobian of the friction force wrt the velocities. @@ -70,7 +67,6 @@ class TangentialPotential : public Potential { Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, const NormalPotential& normal_potential, - const double normal_stiffness, const DiffWRT wrt, const double dmin = 0) const; @@ -129,7 +125,6 @@ class TangentialPotential : public Potential { /// @param lagged_displacements Previous displacements of the vertices (rowwise). /// @param velocities Current displacements of the vertices (rowwise). /// @param normal_potential Normal potential (used for normal force magnitude). - /// @param normal_stiffness Normal stiffness (used for normal force magnitude). /// @param dmin Minimum distance (used for normal force magnitude). /// @param no_mu Whether to not multiply by mu /// @return Friction force @@ -139,7 +134,6 @@ class TangentialPotential : public Potential { Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, const NormalPotential& normal_potential, - const double normal_stiffness, const double dmin = 0, const bool no_mu = false) const; //< whether to not multiply by mu @@ -149,7 +143,6 @@ class TangentialPotential : public Potential { /// @param lagged_displacements Previous displacements of the vertices (rowwise). /// @param velocities Current displacements of the vertices (rowwise). /// @param normal_potential Normal potential (used for normal force magnitude). - /// @param normal_stiffness Noraml stiffness (used for normal force magnitude). /// @param wrt Variable to differentiate the friction force with respect to. /// @param dmin Minimum distance (used for normal force magnitude). /// @return Friction force Jacobian @@ -159,7 +152,6 @@ class TangentialPotential : public Potential { Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, const NormalPotential& normal_potential, - const double normal_stiffness, const DiffWRT wrt, const double dmin = 0) const; diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index af86ee358..8e189c88c 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -401,8 +401,8 @@ TEST_CASE("Barrier derivatives", "[barrier]") double dhat = GENERATE_COPY(range(use_dist_sqr ? -2 : -5, 0)); dhat = pow(10, dhat); - double d = - GENERATE_COPY(take(10, random(dhat / 2, 0.9 * dhat))); // ∈ [0, d̂] + double step = (0.9 - 0.5) / 10.0 * dhat; + auto d = GENERATE_COPY(take(10, range(0.5 * dhat, 0.9 * dhat, step))); Eigen::Matrix d_vec; d_vec << d; @@ -438,13 +438,13 @@ TEST_CASE("Barrier derivatives", "[barrier]") fd::finite_gradient( d_vec, [&](const Eigen::VectorXd& _d) { return (*barrier)(_d[0], dhat); }, - fgrad); + fgrad, fd::AccuracyOrder::SECOND, 1e-10); Eigen::VectorXd grad(1); grad << barrier->first_derivative(d, dhat); CAPTURE(dhat, d, fgrad(0), grad(0), use_dist_sqr); - CHECK(fd::compare_gradient(fgrad, grad)); + CHECK(grad(0) == Catch::Approx(fgrad(0))); // Check second_derivative @@ -453,12 +453,12 @@ TEST_CASE("Barrier derivatives", "[barrier]") [&](const Eigen::VectorXd& _d) { return barrier->first_derivative(_d[0], dhat); }, - fgrad); + fgrad, fd::AccuracyOrder::SECOND, 1e-10); grad << barrier->second_derivative(d, dhat); CAPTURE(dhat, d, fgrad(0), grad(0), use_dist_sqr); - CHECK(fd::compare_gradient(fgrad, grad)); + CHECK(grad(0) == Catch::Approx(fgrad(0))); } TEST_CASE("Physical barrier", "[barrier]") diff --git a/tests/src/tests/collisions/test_normal_collisions.cpp b/tests/src/tests/collisions/test_normal_collisions.cpp index 0ee0c8455..3e30daa81 100644 --- a/tests/src/tests/collisions/test_normal_collisions.cpp +++ b/tests/src/tests/collisions/test_normal_collisions.cpp @@ -86,7 +86,7 @@ TEST_CASE("Codim. vertex-vertex collisions", "[collisions][codim]") CHECK(collisions.size() == 12); CHECK(collisions.vv_collisions.size() == 12); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + BarrierPotential barrier_potential(dhat, 1.0, use_physical_barrier); CHECK(barrier_potential(collisions, mesh, vertices) > 0.0); const Eigen::VectorXd grad = @@ -195,7 +195,7 @@ TEST_CASE("Codim. edge-vertex collisions", "[collisions][codim]") CHECK(collisions.fv_collisions.size() == 0); CHECK( - BarrierPotential(dhat, use_physical_barrier)( + BarrierPotential(dhat, 1.0, use_physical_barrier)( collisions, mesh, vertices) > 0.0); } diff --git a/tests/src/tests/friction/test_force_jacobian.cpp b/tests/src/tests/friction/test_force_jacobian.cpp index e765c7faa..2a775cb51 100644 --- a/tests/src/tests/friction/test_force_jacobian.cpp +++ b/tests/src/tests/friction/test_force_jacobian.cpp @@ -45,8 +45,8 @@ void check_friction_force_jacobian( TangentialCollisions tangential_collisions; tangential_collisions.build( - mesh, X + Ut, collisions, BarrierPotential(dhat), barrier_stiffness, - mu); + mesh, X + Ut, collisions, BarrierPotential(dhat, barrier_stiffness), + barrier_stiffness, mu); CHECK(!tangential_collisions.empty()); const FrictionPotential D(epsv_times_h); @@ -92,8 +92,9 @@ void check_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd JF_wrt_X = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, BarrierPotential(dhat), - barrier_stiffness, FrictionPotential::DiffWRT::REST_POSITIONS); + tangential_collisions, mesh, X, Ut, velocities, + BarrierPotential(dhat, barrier_stiffness), + FrictionPotential::DiffWRT::REST_POSITIONS); auto F_X = [&](const Eigen::VectorXd& x) { Eigen::MatrixXd fd_X = fd::unflatten(x, X.cols()); @@ -112,15 +113,15 @@ void check_friction_force_jacobian( fd_collisions.build(fd_mesh, fd_X + Ut, dhat); fd_friction_collisions.build( - fd_mesh, fd_X + Ut, fd_collisions, BarrierPotential(dhat), - barrier_stiffness, mu); + fd_mesh, fd_X + Ut, fd_collisions, + BarrierPotential(dhat, barrier_stiffness), mu); } else { fd_friction_collisions = tangential_collisions; } return D.force( fd_friction_collisions, fd_mesh, fd_X, Ut, velocities, - BarrierPotential(dhat), barrier_stiffness); + BarrierPotential(dhat, barrier_stiffness)); }; Eigen::MatrixXd fd_JF_wrt_X; fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); @@ -133,8 +134,9 @@ void check_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd JF_wrt_Ut = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, BarrierPotential(dhat), - barrier_stiffness, FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); + tangential_collisions, mesh, X, Ut, velocities, + BarrierPotential(dhat, barrier_stiffness), + FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); auto F_Ut = [&](const Eigen::VectorXd& ut) { Eigen::MatrixXd fd_Ut = fd::unflatten(ut, Ut.cols()); @@ -150,15 +152,15 @@ void check_friction_force_jacobian( fd_collisions.build(mesh, X + fd_Ut, dhat); fd_friction_collisions.build( - mesh, X + fd_Ut, fd_collisions, BarrierPotential(dhat), - barrier_stiffness, mu); + mesh, X + fd_Ut, fd_collisions, + BarrierPotential(dhat, barrier_stiffness), mu); } else { fd_friction_collisions = tangential_collisions; } return D.force( tangential_collisions, mesh, X, fd_Ut, velocities, - BarrierPotential(dhat), barrier_stiffness); + BarrierPotential(dhat, barrier_stiffness)); }; Eigen::MatrixXd fd_JF_wrt_Ut; fd::finite_jacobian(fd::flatten(Ut), F_Ut, fd_JF_wrt_Ut); @@ -171,14 +173,15 @@ void check_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd JF_wrt_V = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, BarrierPotential(dhat), - barrier_stiffness, FrictionPotential::DiffWRT::VELOCITIES); + tangential_collisions, mesh, X, Ut, velocities, + BarrierPotential(dhat, barrier_stiffness), + FrictionPotential::DiffWRT::VELOCITIES); auto F_V = [&](const Eigen::VectorXd& v) { return D.force( tangential_collisions, mesh, X, Ut, - fd::unflatten(v, velocities.cols()), BarrierPotential(dhat), - barrier_stiffness); + fd::unflatten(v, velocities.cols()), + BarrierPotential(dhat, barrier_stiffness)); }; Eigen::MatrixXd fd_JF_wrt_V; fd::finite_jacobian(fd::flatten(velocities), F_V, fd_JF_wrt_V); @@ -208,8 +211,8 @@ void check_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// const Eigen::VectorXd force = D.force( - tangential_collisions, mesh, X, Ut, velocities, BarrierPotential(dhat), - barrier_stiffness); + tangential_collisions, mesh, X, Ut, velocities, + BarrierPotential(dhat, barrier_stiffness)); const Eigen::VectorXd grad_D = D.gradient(tangential_collisions, mesh, velocities); CHECK(fd::compare_gradient(-force, grad_D)); @@ -217,8 +220,9 @@ void check_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd jac_force = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, BarrierPotential(dhat), - barrier_stiffness, FrictionPotential::DiffWRT::VELOCITIES); + tangential_collisions, mesh, X, Ut, velocities, + BarrierPotential(dhat, barrier_stiffness), + FrictionPotential::DiffWRT::VELOCITIES); CHECK(fd::compare_jacobian(-jac_force, hess_D)); } diff --git a/tests/src/tests/friction/test_friction.cpp b/tests/src/tests/friction/test_friction.cpp index 8a82f57dd..54926584d 100644 --- a/tests/src/tests/friction/test_friction.cpp +++ b/tests/src/tests/friction/test_friction.cpp @@ -197,7 +197,7 @@ TEST_CASE( TangentialCollisions tangential_collisions; tangential_collisions.build( - mesh, V_lagged, collisions, BarrierPotential(dhat), barrier_stiffness, + mesh, V_lagged, collisions, BarrierPotential(dhat, barrier_stiffness), mu); REQUIRE(tangential_collisions.size() == collisions.size()); REQUIRE( diff --git a/tests/src/tests/potential/test_adhesion_potentials.cpp b/tests/src/tests/potential/test_adhesion_potentials.cpp index da4cc254b..5dbf11435 100644 --- a/tests/src/tests/potential/test_adhesion_potentials.cpp +++ b/tests/src/tests/potential/test_adhesion_potentials.cpp @@ -83,7 +83,7 @@ TEST_CASE("Normal adhesion potential", "[potential][adhesion]") // --- Check the potential gradients --------------------------------------- - CHECK(BarrierPotential(dhat_a)(collisions, mesh, vertices) > 0); + CHECK(BarrierPotential(dhat_a, 1.0)(collisions, mesh, vertices) > 0); NormalAdhesionPotential potential(dhat_p, dhat_a, Y, eps_c); CHECK(potential(collisions, mesh, vertices) < 0); @@ -125,12 +125,12 @@ TEST_CASE("Normal adhesion potential", "[potential][adhesion]") // --- Maximum normal adhesion force magnitude ----------------------------- CHECK( - potential.force_magnitude(1e-3, 0, 1) + potential.force_magnitude(1e-3, 0) == max_normal_adhesion_force_magnitude( dhat_p * dhat_p, dhat_a * dhat_a, Y * eps_c / (4 * (dhat_p) * (dhat_p * dhat_p - dhat_a * dhat_a)))); - CHECK(potential.force_magnitude_gradient(1e-3, VectorMax12d::Zero(12), 0, 1) + CHECK(potential.force_magnitude_gradient(1e-3, VectorMax12d::Zero(12), 0) .isZero()); } diff --git a/tests/src/tests/potential/test_barrier_potential.cpp b/tests/src/tests/potential/test_barrier_potential.cpp index df361f379..e67bb581c 100644 --- a/tests/src/tests/potential/test_barrier_potential.cpp +++ b/tests/src/tests/potential/test_barrier_potential.cpp @@ -71,7 +71,8 @@ TEST_CASE( CAPTURE(dhat, broad_phase->name(), use_area_weighting, collision_set_type); CHECK(!collisions.empty()); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + double kappa = 1.0; + BarrierPotential barrier_potential(dhat, kappa, use_physical_barrier); // ------------------------------------------------------------------------- // Gradient @@ -220,7 +221,7 @@ TEST_CASE( collisions.build(mesh, vertices, dhat); CHECK(!collisions.empty()); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + BarrierPotential barrier_potential(dhat, 1.0, use_physical_barrier); const Eigen::VectorXd grad_b = barrier_potential.gradient(collisions, mesh, vertices); @@ -297,7 +298,7 @@ TEST_CASE( collisions.build(candidates, mesh, vertices, dhat); REQUIRE(!collisions.ee_collisions.empty()); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + BarrierPotential barrier_potential(dhat, 1.0, use_physical_barrier); for (int i = 0; i < collisions.size(); i++) { std::vector> triplets; @@ -436,7 +437,7 @@ TEST_CASE( collisions.set_enable_shape_derivatives(true); collisions.build(mesh, vertices, dhat); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + BarrierPotential barrier_potential(dhat, 1.0, use_physical_barrier); const Eigen::MatrixXd JF_wrt_X = barrier_potential.shape_derivative(collisions, mesh, vertices); @@ -502,7 +503,7 @@ TEST_CASE( CAPTURE(mesh_name, dhat); CHECK(!collisions.empty()); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + BarrierPotential barrier_potential(dhat, 1.0, use_physical_barrier); BENCHMARK("Compute barrier potential") { @@ -568,7 +569,7 @@ TEST_CASE( collisions.set_enable_shape_derivatives(true); collisions.build(mesh, vertices, dhat); - BarrierPotential barrier_potential(dhat, use_physical_barrier); + BarrierPotential barrier_potential(dhat, 1.0, use_physical_barrier); Eigen::SparseMatrix JF_wrt_X; diff --git a/tests/src/tests/potential/test_friction_potential.cpp b/tests/src/tests/potential/test_friction_potential.cpp index 9f8516051..f428adde4 100644 --- a/tests/src/tests/potential/test_friction_potential.cpp +++ b/tests/src/tests/potential/test_friction_potential.cpp @@ -24,7 +24,7 @@ TEST_CASE("Friction gradient and hessian", "[friction][gradient][hessian]") TangentialCollisions tangential_collisions; tangential_collisions.build( - mesh, V0, collisions, BarrierPotential(dhat), barrier_stiffness, mu); + mesh, V0, collisions, BarrierPotential(dhat, barrier_stiffness), mu); const FrictionPotential D(epsv_times_h); From abf94f08ae0bd71148874966116919981ee576f6 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 17 Feb 2026 14:19:41 -0500 Subject: [PATCH 03/16] Remove assert(false) if the meshes has non-manifold edges --- src/ipc/collision_mesh.cpp | 8 +++++--- tests/src/tests/geometry/test_angle.cpp | 21 ++++++++++++++++++++- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 2691d0f18..e37133783 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -135,8 +135,7 @@ void CollisionMesh::init_edges_to_faces() return; } - m_edges_to_faces.setOnes(num_edges(), 2); - m_edges_to_faces *= -1; + m_edges_to_faces.setConstant(num_edges(), 2, -1); for (int f = 0; f < m_faces_to_edges.rows(); f++) { for (int le = 0; le < 3; le++) { if (m_edges_to_faces(m_faces_to_edges(f, le), 0) < 0) { @@ -144,7 +143,10 @@ void CollisionMesh::init_edges_to_faces() } else if (m_edges_to_faces(m_faces_to_edges(f, le), 1) < 0) { m_edges_to_faces(m_faces_to_edges(f, le), 1) = f; } else { - assert(false); + logger().warn( + "Edge {} of face {} is shared by more than 2 faces. " + "This may cause issues with the Geometric Contact Potential (smooth contact).", + m_faces_to_edges(f, le), f); } } } diff --git a/tests/src/tests/geometry/test_angle.cpp b/tests/src/tests/geometry/test_angle.cpp index 72d9398cc..009cc4258 100644 --- a/tests/src/tests/geometry/test_angle.cpp +++ b/tests/src/tests/geometry/test_angle.cpp @@ -1,4 +1,6 @@ +#include #include +#include #include #include @@ -70,4 +72,21 @@ TEST_CASE("Dihedral angle and gradient", "[angle][dihedral]") std::cout << " Gradient:\n" << grad.transpose() << std::endl; std::cout << "FD Gradient:\n" << fd_grad.transpose() << std::endl; } -} \ No newline at end of file +} + +TEST_CASE( + "Benchmark dihedral angle and gradient", "[!benchmark][angle][dihedral]") +{ + Eigen::Vector3d x0, x1, x2, x3; + x0 << -0.015247385606936873, 1.1187166216183693, -0.09508569727171673; + x1 << -0.017971426013627917, 1.1229485012592226, -0.0934495693604115; + x2 << -0.021023579437439658, 1.1190719774332136, -0.09490934871219975; + x3 << -0.014473605843359506, 1.1216871870663812, -0.09395608203835042; + + BENCHMARK("Dihedral angle") { return dihedral_angle(x0, x1, x2, x3); }; + + BENCHMARK("Dihedral angle gradient") + { + return dihedral_angle_gradient(x0, x1, x2, x3); + }; +} From c68b5aa04a4c5de3bba60fec779ea051295e3eeb Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 20 Feb 2026 11:55:30 -0500 Subject: [PATCH 04/16] Add alias for Eigen::last = Eigen::indexing::last --- src/ipc/utils/eigen_ext.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index ede4d60a1..140d5a071 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -262,7 +262,8 @@ static const Eigen::IOFormat OBJ_VERTEX_FORMAT = Eigen::IOFormat( #if EIGEN_VERSION_AT_LEAST(3, 4, 90) namespace Eigen { // Move all back to the root namespace for compatibility with older versions -static const Eigen::internal::all_t all = indexing::all; // NOLINT +static const auto all = indexing::all; // NOLINT +static const auto last = indexing::last; // NOLINT } // namespace Eigen #endif From 128473a220384978211bc0f6be0885197e6b025f Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 20 Feb 2026 14:54:54 -0500 Subject: [PATCH 05/16] Apply EIGEN_DONT_VECTORIZE to ipc_toolkit Disable Eigen vectorization to avoid alignment issues. Place EIGEN_DONT_VECTORIZE on the ipc_toolkit target as PUBLIC because setting it on the Eigen target or making it PRIVATE caused crashes. --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fe7e932c0..2dc83ba96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -265,7 +265,8 @@ if(IPC_TOOLKIT_WITH_SIMD) target_link_libraries(ipc_toolkit PRIVATE xsimd::xsimd) # Disable vectorization in Eigen since I've found it to have alignment issues. - target_compile_definitions(Eigen3_Eigen INTERFACE EIGEN_DONT_VECTORIZE=1) + # NOTE: I don't know why this needs to be public, but it crashes if I make it private. + target_compile_definitions(ipc_toolkit PUBLIC EIGEN_DONT_VECTORIZE=1) endif() # For MSVC, do not use the min and max macros. From 4e45a9b146e5df2480471e02e1ede68865ed1b1a Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 21 Feb 2026 23:44:08 -0500 Subject: [PATCH 06/16] Fix friction force jacobian tests --- .../tests/friction/test_force_jacobian.cpp | 83 ++++++++----------- .../potential/test_barrier_potential.cpp | 12 +-- 2 files changed, 42 insertions(+), 53 deletions(-) diff --git a/tests/src/tests/friction/test_force_jacobian.cpp b/tests/src/tests/friction/test_force_jacobian.cpp index 2a775cb51..048913619 100644 --- a/tests/src/tests/friction/test_force_jacobian.cpp +++ b/tests/src/tests/friction/test_force_jacobian.cpp @@ -20,7 +20,8 @@ void check_friction_force_jacobian( const Eigen::MatrixXd& Ut, const Eigen::MatrixXd& U, const NormalCollisions& collisions, - const double mu, + const double mu_s, + const double mu_k, const double epsv_times_h, const double dhat, const double barrier_stiffness, @@ -28,6 +29,9 @@ void check_friction_force_jacobian( { REQUIRE(collisions.enable_shape_derivatives()); + const BarrierPotential B(dhat, barrier_stiffness); + const FrictionPotential D(epsv_times_h); + const Eigen::MatrixXd& X = mesh.rest_positions(); double distance_t0 = collisions.compute_minimum_distance(mesh, X + Ut); double distance_t1 = collisions.compute_minimum_distance(mesh, X + U); @@ -36,21 +40,18 @@ void check_friction_force_jacobian( return; } + // V = (X + U) - (X + Ut) = U - Ut const Eigen::MatrixXd velocities = U - Ut; CAPTURE( - mu, epsv_times_h, dhat, barrier_stiffness, + mu_s, mu_k, epsv_times_h, dhat, barrier_stiffness, collisions.vv_collisions.size(), collisions.ev_collisions.size(), collisions.ee_collisions.size(), collisions.fv_collisions.size()); TangentialCollisions tangential_collisions; - tangential_collisions.build( - mesh, X + Ut, collisions, BarrierPotential(dhat, barrier_stiffness), - barrier_stiffness, mu); + tangential_collisions.build(mesh, X + Ut, collisions, B, mu_s, mu_k); CHECK(!tangential_collisions.empty()); - const FrictionPotential D(epsv_times_h); - /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd JPA_wrt_X(mesh.num_vertices(), mesh.ndof()); @@ -65,8 +66,8 @@ void check_friction_force_jacobian( Eigen::MatrixXd fd_JPA_wrt_X; fd::finite_jacobian(fd::flatten(X), PA_X, fd_JPA_wrt_X); - CHECKED_ELSE(fd::compare_jacobian(JPA_wrt_X, fd_JPA_wrt_X)) - { + CHECK(fd::compare_jacobian(JPA_wrt_X, fd_JPA_wrt_X)); + if (!fd::compare_jacobian(JPA_wrt_X, fd_JPA_wrt_X)) { tests::print_compare_nonzero(JPA_wrt_X, fd_JPA_wrt_X); } @@ -84,16 +85,15 @@ void check_friction_force_jacobian( Eigen::MatrixXd fd_JEA_wrt_X; fd::finite_jacobian(fd::flatten(X), EA_X, fd_JEA_wrt_X); - CHECKED_ELSE(fd::compare_jacobian(JEA_wrt_X, fd_JEA_wrt_X)) - { + CHECK(fd::compare_jacobian(JEA_wrt_X, fd_JEA_wrt_X)); + if (!fd::compare_jacobian(JEA_wrt_X, fd_JEA_wrt_X)) { tests::print_compare_nonzero(JEA_wrt_X, fd_JEA_wrt_X); } /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd JF_wrt_X = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, - BarrierPotential(dhat, barrier_stiffness), + tangential_collisions, mesh, X, Ut, velocities, B, FrictionPotential::DiffWRT::REST_POSITIONS); auto F_X = [&](const Eigen::VectorXd& x) { @@ -113,29 +113,25 @@ void check_friction_force_jacobian( fd_collisions.build(fd_mesh, fd_X + Ut, dhat); fd_friction_collisions.build( - fd_mesh, fd_X + Ut, fd_collisions, - BarrierPotential(dhat, barrier_stiffness), mu); + fd_mesh, fd_X + Ut, fd_collisions, B, mu_s, mu_k); } else { fd_friction_collisions = tangential_collisions; } return D.force( - fd_friction_collisions, fd_mesh, fd_X, Ut, velocities, - BarrierPotential(dhat, barrier_stiffness)); + fd_friction_collisions, fd_mesh, fd_X, Ut, velocities, B); }; Eigen::MatrixXd fd_JF_wrt_X; fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); - CHECKED_ELSE(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) - { + CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); } /////////////////////////////////////////////////////////////////////////// - Eigen::MatrixXd JF_wrt_Ut = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, - BarrierPotential(dhat, barrier_stiffness), + tangential_collisions, mesh, X, Ut, velocities, B, FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); auto F_Ut = [&](const Eigen::VectorXd& ut) { @@ -152,42 +148,37 @@ void check_friction_force_jacobian( fd_collisions.build(mesh, X + fd_Ut, dhat); fd_friction_collisions.build( - mesh, X + fd_Ut, fd_collisions, - BarrierPotential(dhat, barrier_stiffness), mu); + mesh, X + fd_Ut, fd_collisions, B, mu_s, mu_k); } else { fd_friction_collisions = tangential_collisions; } - return D.force( - tangential_collisions, mesh, X, fd_Ut, velocities, - BarrierPotential(dhat, barrier_stiffness)); + return D.force(tangential_collisions, mesh, X, fd_Ut, velocities, B); }; Eigen::MatrixXd fd_JF_wrt_Ut; fd::finite_jacobian(fd::flatten(Ut), F_Ut, fd_JF_wrt_Ut); - CHECKED_ELSE(fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)) - { + CHECK(fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)); + if (!fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)) { tests::print_compare_nonzero(JF_wrt_Ut, fd_JF_wrt_Ut); } /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd JF_wrt_V = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, - BarrierPotential(dhat, barrier_stiffness), + tangential_collisions, mesh, X, Ut, velocities, B, FrictionPotential::DiffWRT::VELOCITIES); auto F_V = [&](const Eigen::VectorXd& v) { return D.force( tangential_collisions, mesh, X, Ut, - fd::unflatten(v, velocities.cols()), - BarrierPotential(dhat, barrier_stiffness)); + fd::unflatten(v, velocities.cols()), B); }; Eigen::MatrixXd fd_JF_wrt_V; fd::finite_jacobian(fd::flatten(velocities), F_V, fd_JF_wrt_V); - CHECKED_ELSE(fd::compare_jacobian(JF_wrt_V, fd_JF_wrt_V)) - { + CHECK(fd::compare_jacobian(JF_wrt_V, fd_JF_wrt_V)); + if (!fd::compare_jacobian(JF_wrt_V, fd_JF_wrt_V)) { tests::print_compare_nonzero(JF_wrt_V, fd_JF_wrt_V); } @@ -203,16 +194,15 @@ void check_friction_force_jacobian( Eigen::MatrixXd fd_hessian; fd::finite_jacobian(fd::flatten(velocities), grad, fd_hessian); - CHECKED_ELSE(fd::compare_jacobian(hess_D, fd_hessian)) - { + CHECK(fd::compare_jacobian(hess_D, fd_hessian)); + if (!fd::compare_jacobian(hess_D, fd_hessian)) { tests::print_compare_nonzero(hess_D, fd_hessian); } /////////////////////////////////////////////////////////////////////////// - const Eigen::VectorXd force = D.force( - tangential_collisions, mesh, X, Ut, velocities, - BarrierPotential(dhat, barrier_stiffness)); + const Eigen::VectorXd force = + D.force(tangential_collisions, mesh, X, Ut, velocities, B); const Eigen::VectorXd grad_D = D.gradient(tangential_collisions, mesh, velocities); CHECK(fd::compare_gradient(-force, grad_D)); @@ -220,8 +210,7 @@ void check_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// Eigen::MatrixXd jac_force = D.force_jacobian( - tangential_collisions, mesh, X, Ut, velocities, - BarrierPotential(dhat, barrier_stiffness), + tangential_collisions, mesh, X, Ut, velocities, B, FrictionPotential::DiffWRT::VELOCITIES); CHECK(fd::compare_jacobian(-jac_force, hess_D)); } @@ -251,8 +240,8 @@ TEST_CASE("Friction force jacobian", "[friction][force-jacobian]") mesh.init_area_jacobians(); check_friction_force_jacobian( - mesh, Ut, U, collisions, mu, epsv_times_h, dhat, barrier_stiffness, - false); + mesh, Ut, U, collisions, 0.5 * mu, mu, epsv_times_h, dhat, + barrier_stiffness, false); } TEST_CASE( @@ -331,10 +320,10 @@ TEST_CASE( X = mesh.vertices(X); if (Ut.rows() != X.rows()) { - Ut = mesh.vertices(Ut); + Ut = mesh.map_displacements(Ut); } if (U.rows() != X.rows()) { - U = mesh.vertices(U); + U = mesh.map_displacements(U); } NormalCollisions collisions; @@ -349,7 +338,7 @@ TEST_CASE( CHECK(collisions.compute_minimum_distance(mesh, X + U) != 0); check_friction_force_jacobian( - mesh, Ut, U, collisions, mu, epsv_dt, dhat, kappa, true); + mesh, Ut, U, collisions, 0.5 * mu, mu, epsv_dt, dhat, kappa, true); } void check_smooth_friction_force_jacobian( diff --git a/tests/src/tests/potential/test_barrier_potential.cpp b/tests/src/tests/potential/test_barrier_potential.cpp index e67bb581c..55d344ac2 100644 --- a/tests/src/tests/potential/test_barrier_potential.cpp +++ b/tests/src/tests/potential/test_barrier_potential.cpp @@ -348,8 +348,8 @@ TEST_CASE( Eigen::MatrixXd fd_JF_wrt_X; fd::finite_jacobian(fd::flatten(rest_positions), F_X, fd_JF_wrt_X); - CHECKED_ELSE(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) - { + CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); } } @@ -390,8 +390,8 @@ TEST_CASE( Eigen::MatrixXd fd_JF_wrt_X; fd::finite_jacobian(fd::flatten(rest_positions), F_X, fd_JF_wrt_X); - CHECKED_ELSE(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) - { + CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); } } @@ -458,8 +458,8 @@ TEST_CASE( Eigen::MatrixXd fd_JF_wrt_X; fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); - CHECKED_ELSE(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) - { + CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); } } From d3d99065389b2e51d159787728eac96846d169ba Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 22 Feb 2026 16:23:31 -0500 Subject: [PATCH 07/16] Handle single-node LBVH in broad-phase queries Detect lbvh.size() == 1 and treat the root as a leaf in both single and batched query traversals, performing intersection tests and adding candidate collisions as appropriate --- src/ipc/broad_phase/lbvh.cpp | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp index ce6179cb5..c5db0accc 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -411,6 +411,15 @@ namespace { do { const LBVH::Node& node = lbvh[node_idx]; + if (lbvh.size() == 1) { // Single node case (only root) + assert(node.is_leaf()); // Only one node, so it must be a leaf + if (node.intersects(query)) { + attempt_add_candidate( + query, node, can_collide, candidates); + } + break; + } + // Check left and right are valid pointers assert(node.is_inner()); @@ -507,6 +516,28 @@ namespace { do { const LBVH::Node& node = lbvh[node_idx]; + if (lbvh.size() == 1) { // Single node case (only root) + assert(node.is_leaf()); // Only one node, so it must be a leaf + // Check intersection with all queries simultaneously + const xs::batch_bool intersects = + (node.aabb_min.x() <= q_max_x) + & (node.aabb_min.y() <= q_max_y) + & (node.aabb_min.z() <= q_max_z) + & (q_min_x <= node.aabb_max.x()) + & (q_min_y <= node.aabb_max.y()) + & (q_min_z <= node.aabb_max.z()); + if (xs::any(intersects)) { + for (int k = 0; k < n_queries; ++k) { + if (intersects.get(k)) { + attempt_add_candidate< + Candidate, swap_order, triangular>( + queries[k], node, can_collide, candidates); + } + } + } + break; + } + // Check left and right are valid pointers assert(node.is_inner()); From 476e2dd16850618728f99c4cc81376dce19dba9b Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 26 Feb 2026 10:36:53 -0500 Subject: [PATCH 08/16] Add as cast helpers to CollisionStencil --- src/ipc/candidates/collision_stencil.hpp | 25 ++++++++++++++++++++++++ src/ipc/tangent/closest_point.cpp | 2 +- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index cec3bdd6f..18b00f121 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -18,6 +18,31 @@ class CollisionStencil { virtual ~CollisionStencil() = default; + /// @brief Get this as a child collision type. + /// @tparam CollisionStencilType The child collision type to get this as. + /// @return This as the child collision type. + /// @throws std::bad_cast if this is not of type CollisionStencilType. + template CollisionStencilType& as() + { + static_assert( + std::is_base_of::value, + "Template argument CollisionStencilType must inherit from CollisionStencil"); + return dynamic_cast(*this); + } + + /// @brief Get this as a child collision type. + /// @tparam CollisionStencilType The child collision type to get this as. + /// @return This as the child collision type. + /// @throws std::bad_cast if this is not of type CollisionStencilType. + template + const CollisionStencilType& as() const + { + static_assert( + std::is_base_of::value, + "Template argument CollisionStencilType must inherit from CollisionStencil"); + return dynamic_cast(*this); + } + /// @brief Get the number of vertices in the collision stencil. virtual int num_vertices() const = 0; diff --git a/src/ipc/tangent/closest_point.cpp b/src/ipc/tangent/closest_point.cpp index 55f4a4ecd..441ed4e18 100644 --- a/src/ipc/tangent/closest_point.cpp +++ b/src/ipc/tangent/closest_point.cpp @@ -74,7 +74,7 @@ Eigen::Vector2d edge_edge_closest_point( const Eigen::Vector3d ea = ea1 - ea0; const Eigen::Vector3d eb = eb1 - eb0; - Eigen::Matrix A; + Eigen::Matrix2d A; A(0, 0) = ea.squaredNorm(); A(0, 1) = A(1, 0) = -eb.dot(ea); A(1, 1) = eb.squaredNorm(); From 3999c9e807ebb3d03d83aca982a98ebffb4698d1 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 12:24:17 -0500 Subject: [PATCH 09/16] Add analytic plane-vertex collision support (#216) * Integrate analytic plane-vertex primitives into the collision pipeline. * Add PlaneVertexCandidate, normal and tangential plane-vertex collisions, and pv handling in builders. * Add CollisionMesh::planes and Python bindings (Hyperplane, PlaneVertex*). * Remove old implicits module and update CMake, tests, and small API/format fixes. * Refactor conservative rescaling initialization in CCD classes and update documentation --- cmake/ipc_toolkit/ipc_toolkit_warnings.cmake | 1 + cmake/recipes/eigen.cmake | 4 + python/src/CMakeLists.txt | 1 - python/src/bindings.cpp | 5 +- python/src/bindings.hpp | 1 - python/src/candidates/CMakeLists.txt | 1 + python/src/candidates/bindings.hpp | 1 + python/src/candidates/candidates.cpp | 3 +- python/src/candidates/plane_vertex.cpp | 18 + python/src/collision_mesh.cpp | 31 +- python/src/collisions/normal/plane_vertex.cpp | 18 +- .../src/collisions/tangential/CMakeLists.txt | 1 + python/src/collisions/tangential/bindings.hpp | 1 + .../collisions/tangential/plane_vertex.cpp | 18 + python/src/implicits/CMakeLists.txt | 5 - python/src/implicits/bindings.hpp | 5 - python/src/implicits/plane.cpp | 193 ------ src/ipc/CMakeLists.txt | 1 - src/ipc/candidates/CMakeLists.txt | 4 +- src/ipc/candidates/candidates.cpp | 35 +- src/ipc/candidates/candidates.hpp | 2 + src/ipc/candidates/edge_edge.hpp | 2 +- src/ipc/candidates/edge_vertex.hpp | 2 +- src/ipc/candidates/face_vertex.hpp | 2 +- src/ipc/candidates/plane_vertex.cpp | 95 +++ src/ipc/candidates/plane_vertex.hpp | 96 +++ src/ipc/candidates/vertex_vertex.hpp | 2 +- src/ipc/ccd/additive_ccd.cpp | 2 +- src/ipc/ccd/additive_ccd.hpp | 5 +- src/ipc/ccd/inexact_ccd.cpp | 4 +- src/ipc/ccd/inexact_ccd.hpp | 5 +- src/ipc/ccd/narrow_phase_ccd.hpp | 15 + src/ipc/ccd/point_static_plane.cpp | 2 +- src/ipc/ccd/tight_inclusion_ccd.cpp | 2 +- src/ipc/ccd/tight_inclusion_ccd.hpp | 5 +- src/ipc/collision_mesh.hpp | 5 + src/ipc/collisions/normal/CMakeLists.txt | 1 - .../collisions/normal/normal_collisions.cpp | 18 + src/ipc/collisions/normal/plane_vertex.cpp | 73 --- src/ipc/collisions/normal/plane_vertex.hpp | 92 +-- src/ipc/collisions/tangential/CMakeLists.txt | 2 + .../collisions/tangential/plane_vertex.cpp | 96 +++ .../collisions/tangential/plane_vertex.hpp | 52 ++ .../tangential/tangential_collisions.cpp | 28 +- .../tangential/tangential_collisions.hpp | 3 + src/ipc/implicits/CMakeLists.txt | 6 - src/ipc/implicits/plane.cpp | 131 ---- src/ipc/implicits/plane.hpp | 80 --- src/ipc/potentials/potential.cpp | 2 +- src/ipc/smooth_contact/primitives/edge.cpp | 2 +- src/ipc/smooth_contact/primitives/edge.hpp | 2 +- src/ipc/smooth_contact/primitives/edge2.cpp | 2 +- src/ipc/smooth_contact/primitives/edge2.hpp | 2 +- src/ipc/smooth_contact/primitives/edge3.cpp | 2 +- src/ipc/smooth_contact/primitives/edge3.hpp | 2 +- src/ipc/smooth_contact/primitives/face.cpp | 2 +- src/ipc/smooth_contact/primitives/face.hpp | 2 +- src/ipc/smooth_contact/primitives/point2.cpp | 2 +- src/ipc/smooth_contact/primitives/point2.hpp | 2 +- src/ipc/smooth_contact/primitives/point3.cpp | 2 +- src/ipc/smooth_contact/primitives/point3.hpp | 2 +- src/ipc/utils/CMakeLists.txt | 5 +- src/ipc/utils/eigen_ext.hpp | 2 +- .../src/tests/candidates/test_candidates.cpp | 30 +- .../tests/candidates/test_coefficients.cpp | 2 +- tests/src/tests/candidates/test_normals.cpp | 2 +- tests/src/tests/collisions/CMakeLists.txt | 1 + .../collisions/test_normal_collisions.cpp | 8 +- .../test_plane_vertex_collisions.cpp | 580 ++++++++++++++++++ tests/src/tests/distance/test_edge_edge.cpp | 2 +- tests/src/tests/distance/test_point_plane.cpp | 2 +- 71 files changed, 1183 insertions(+), 652 deletions(-) create mode 100644 python/src/candidates/plane_vertex.cpp create mode 100644 python/src/collisions/tangential/plane_vertex.cpp delete mode 100644 python/src/implicits/CMakeLists.txt delete mode 100644 python/src/implicits/bindings.hpp delete mode 100644 python/src/implicits/plane.cpp create mode 100644 src/ipc/candidates/plane_vertex.cpp create mode 100644 src/ipc/candidates/plane_vertex.hpp delete mode 100644 src/ipc/collisions/normal/plane_vertex.cpp create mode 100644 src/ipc/collisions/tangential/plane_vertex.cpp create mode 100644 src/ipc/collisions/tangential/plane_vertex.hpp delete mode 100644 src/ipc/implicits/CMakeLists.txt delete mode 100644 src/ipc/implicits/plane.cpp delete mode 100644 src/ipc/implicits/plane.hpp create mode 100644 tests/src/tests/collisions/test_plane_vertex_collisions.cpp diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index 3b50f2abc..fb32358e1 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -42,6 +42,7 @@ else() -Werror=nonnull -Werror=init-self -Werror=main + -Werror=extra-semi -Werror=missing-braces -Werror=sequence-point -Werror=return-type diff --git a/cmake/recipes/eigen.cmake b/cmake/recipes/eigen.cmake index 9a43f2808..884746b2a 100644 --- a/cmake/recipes/eigen.cmake +++ b/cmake/recipes/eigen.cmake @@ -26,6 +26,10 @@ target_include_directories(Eigen3_Eigen SYSTEM INTERFACE $ ) +set_target_properties(Eigen3_Eigen PROPERTIES + INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_SOURCE_DIR}" +) + if(EIGEN_DONT_VECTORIZE) target_compile_definitions(Eigen3_Eigen INTERFACE EIGEN_DONT_VECTORIZE=1) endif() diff --git a/python/src/CMakeLists.txt b/python/src/CMakeLists.txt index 04f24a120..b48bd0ffe 100644 --- a/python/src/CMakeLists.txt +++ b/python/src/CMakeLists.txt @@ -19,7 +19,6 @@ add_subdirectory(collisions) add_subdirectory(distance) add_subdirectory(friction) add_subdirectory(geometry) -add_subdirectory(implicits) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potentials) diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 0a31510ae..d20970c2e 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -40,6 +40,7 @@ PYBIND11_MODULE(ipctk, m) define_edge_vertex_candidate(m); define_face_face_candidate(m); define_face_vertex_candidate(m); + define_plane_vertex_candidate(m); define_vertex_vertex_candidate(m); // ccd @@ -72,6 +73,7 @@ PYBIND11_MODULE(ipctk, m) define_edge_edge_tangential_collision(m); define_edge_vertex_tangential_collision(m); define_face_vertex_tangential_collision(m); + define_plane_vertex_tangential_collision(m); define_vertex_vertex_tangential_collision(m); // distance @@ -97,9 +99,6 @@ PYBIND11_MODULE(ipctk, m) define_intersection(m); define_normal(m); - // implicits - define_plane_implicit(m); - // math define_interval(m); diff --git a/python/src/bindings.hpp b/python/src/bindings.hpp index bc2d389a7..fcd7775fa 100644 --- a/python/src/bindings.hpp +++ b/python/src/bindings.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/candidates/CMakeLists.txt b/python/src/candidates/CMakeLists.txt index 7c821f354..01a0aba40 100644 --- a/python/src/candidates/CMakeLists.txt +++ b/python/src/candidates/CMakeLists.txt @@ -5,6 +5,7 @@ set(SOURCES edge_vertex.cpp face_face.cpp face_vertex.cpp + plane_vertex.cpp vertex_vertex.cpp candidates.cpp ) diff --git a/python/src/candidates/bindings.hpp b/python/src/candidates/bindings.hpp index 2d7bd09f3..dac007648 100644 --- a/python/src/candidates/bindings.hpp +++ b/python/src/candidates/bindings.hpp @@ -10,4 +10,5 @@ void define_edge_face_candidate(py::module_& m); void define_edge_vertex_candidate(py::module_& m); void define_face_face_candidate(py::module_& m); void define_face_vertex_candidate(py::module_& m); +void define_plane_vertex_candidate(py::module_& m); void define_vertex_vertex_candidate(py::module_& m); \ No newline at end of file diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index 92ab19f0c..be82f62cc 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -267,5 +267,6 @@ void define_candidates(py::module_& m) .def_readwrite("vv_candidates", &Candidates::vv_candidates) .def_readwrite("ev_candidates", &Candidates::ev_candidates) .def_readwrite("ee_candidates", &Candidates::ee_candidates) - .def_readwrite("fv_candidates", &Candidates::fv_candidates); + .def_readwrite("fv_candidates", &Candidates::fv_candidates) + .def_readwrite("pv_candidates", &Candidates::pv_candidates); } diff --git a/python/src/candidates/plane_vertex.cpp b/python/src/candidates/plane_vertex.cpp new file mode 100644 index 000000000..07f6eb30f --- /dev/null +++ b/python/src/candidates/plane_vertex.cpp @@ -0,0 +1,18 @@ +#include + +#include + +using namespace ipc; + +void define_plane_vertex_candidate(py::module_& m) +{ + py::class_( + m, "PlaneVertexCandidate") + .def( + py::init, index_t>(), "plane"_a, + "vertex_id"_a) + .def_readwrite( + "plane", &PlaneVertexCandidate::plane, "Plane of the candidate") + .def_readwrite( + "vertex_id", &PlaneVertexCandidate::vertex_id, "ID of the vertex"); +} \ No newline at end of file diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp index e8df18395..ae88b1981 100644 --- a/python/src/collision_mesh.cpp +++ b/python/src/collision_mesh.cpp @@ -135,7 +135,25 @@ void define_collision_mesh(py::module_& m) )ipc_Qu8mg5v7", "i"_a, "j"_a); - py::class_(m, "CollisionMesh") + py::class_>(m, "Hyperplane") + .def(py::init<>()) + .def(py::init()) + .def( + "normal", + [](const Eigen::Hyperplane& self) { + return self.normal(); + }) + .def( + "offset", + [](const Eigen::Hyperplane& self) { + return self.offset(); + }) + .def("origin", [](const Eigen::Hyperplane& self) { + return -self.offset() * self.normal(); + }); + + py::class_>( + m, "CollisionMesh") .def( py::init< Eigen::ConstRef, @@ -489,5 +507,16 @@ void define_collision_mesh(py::module_& m) A function that takes two vertex IDs and returns true if the vertices (and faces or edges containing the vertices) can collide. By default all primitives can collide with all other primitives. + )ipc_Qu8mg5v7") + .def_readwrite( + "planes", &CollisionMesh::planes, + R"ipc_Qu8mg5v7( + A vector of planes in the collision mesh. + + Each plane is represented as a `Hyperplane` object (wrapping `Eigen::Hyperplane`). + In Python, a `Hyperplane` can be constructed from either: + * a normal and a point on the plane: `Hyperplane(normal, point)`, or + * a normal and an offset: `Hyperplane(normal, offset)`, + where `normal` and `point` are 3D vectors and `offset` is a scalar. )ipc_Qu8mg5v7"); } diff --git a/python/src/collisions/normal/plane_vertex.cpp b/python/src/collisions/normal/plane_vertex.cpp index 07f727c32..abfe60ed0 100644 --- a/python/src/collisions/normal/plane_vertex.cpp +++ b/python/src/collisions/normal/plane_vertex.cpp @@ -6,20 +6,10 @@ using namespace ipc; void define_plane_vertex_normal_collision(py::module_& m) { - py::class_( + py::class_< + PlaneVertexNormalCollision, PlaneVertexCandidate, NormalCollision>( m, "PlaneVertexNormalCollision") .def( - py::init< - Eigen::ConstRef, Eigen::ConstRef, - const index_t>(), - "plane_origin"_a, "plane_normal"_a, "vertex_id"_a) - .def_readwrite( - "plane_origin", &PlaneVertexNormalCollision::plane_origin, - "The plane's origin.") - .def_readwrite( - "plane_normal", &PlaneVertexNormalCollision::plane_normal, - "The plane's normal.") - .def_readwrite( - "vertex_id", &PlaneVertexNormalCollision::vertex_id, - "The vertex's id."); + py::init, index_t>(), "plane"_a, + "vertex_id"_a); } diff --git a/python/src/collisions/tangential/CMakeLists.txt b/python/src/collisions/tangential/CMakeLists.txt index 652d6fe65..9c20f7ca8 100644 --- a/python/src/collisions/tangential/CMakeLists.txt +++ b/python/src/collisions/tangential/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES edge_edge.cpp edge_vertex.cpp face_vertex.cpp + plane_vertex.cpp tangential_collision.cpp tangential_collisions.cpp vertex_vertex.cpp diff --git a/python/src/collisions/tangential/bindings.hpp b/python/src/collisions/tangential/bindings.hpp index 491d4966f..1283fb75c 100644 --- a/python/src/collisions/tangential/bindings.hpp +++ b/python/src/collisions/tangential/bindings.hpp @@ -7,4 +7,5 @@ void define_tangential_collisions(py::module_& m); void define_edge_edge_tangential_collision(py::module_& m); void define_edge_vertex_tangential_collision(py::module_& m); void define_face_vertex_tangential_collision(py::module_& m); +void define_plane_vertex_tangential_collision(py::module_& m); void define_vertex_vertex_tangential_collision(py::module_& m); \ No newline at end of file diff --git a/python/src/collisions/tangential/plane_vertex.cpp b/python/src/collisions/tangential/plane_vertex.cpp new file mode 100644 index 000000000..1a6ffe363 --- /dev/null +++ b/python/src/collisions/tangential/plane_vertex.cpp @@ -0,0 +1,18 @@ +#include + +#include + +using namespace ipc; + +void define_plane_vertex_tangential_collision(py::module_& m) +{ + py::class_< + PlaneVertexTangentialCollision, PlaneVertexCandidate, + TangentialCollision>(m, "PlaneVertexTangentialCollision") + .def(py::init(), "collision"_a) + .def( + py::init< + const PlaneVertexNormalCollision&, + Eigen::ConstRef, const NormalPotential&>(), + "collision"_a, "positions"_a, "normal_potential"_a); +} \ No newline at end of file diff --git a/python/src/implicits/CMakeLists.txt b/python/src/implicits/CMakeLists.txt deleted file mode 100644 index fb4526ff5..000000000 --- a/python/src/implicits/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -set(SOURCES - plane.cpp -) - -target_sources(ipctk PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/python/src/implicits/bindings.hpp b/python/src/implicits/bindings.hpp deleted file mode 100644 index df6ab2e47..000000000 --- a/python/src/implicits/bindings.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include - -void define_plane_implicit(py::module_& m); \ No newline at end of file diff --git a/python/src/implicits/plane.cpp b/python/src/implicits/plane.cpp deleted file mode 100644 index ef3c02839..000000000 --- a/python/src/implicits/plane.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include - -#include - -using namespace ipc; - -void define_plane_implicit(py::module_& m) -{ - m.def( - "construct_point_plane_collisions", - [](Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, const double dhat, - const double dmin = 0) { - std::vector pv_collisions; - construct_point_plane_collisions( - points, plane_origins, plane_normals, dhat, pv_collisions, - dmin); - return pv_collisions; - }, - R"ipc_Qu8mg5v7( - Construct a set of point-plane distance collisions used to compute - - Note: - The given pv_collisions will be cleared. - - the barrier potential. - - Parameters: - points: Points as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - dhat: The activation distance of the barrier. - dmin: Minimum distance. - - Returns: - The constructed set of collisions. - )ipc_Qu8mg5v7", - "points"_a, "plane_origins"_a, "plane_normals"_a, "dhat"_a, - "dmin"_a = 0); - - m.def( - "construct_point_plane_collisions", - [](Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, const double dhat, - const double dmin, - const std::function& can_collide) { - std::vector pv_collisions; - construct_point_plane_collisions( - points, plane_origins, plane_normals, dhat, pv_collisions, dmin, - can_collide); - return pv_collisions; - }, - R"ipc_Qu8mg5v7( - Construct a set of point-plane distance collisions used to compute - - Note: - The given pv_collisions will be cleared. - - the barrier potential. - - Parameters: - points: Points as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - dhat: The activation distance of the barrier. - dmin: Minimum distance. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - The constructed set of collisions. - )ipc_Qu8mg5v7", - "points"_a, "plane_origins"_a, "plane_normals"_a, "dhat"_a, "dmin"_a, - "can_collide"_a); - - m.def( - "is_step_point_plane_collision_free", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals) { - return is_step_point_plane_collision_free( - points_t0, points_t1, plane_origins, plane_normals); - }, - R"ipc_Qu8mg5v7( - Determine if the step is collision free. - - Note: - Assumes the trajectory is linear. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - - Returns: - True if any collisions occur. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a); - - m.def( - "is_step_point_plane_collision_free", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) { - return is_step_point_plane_collision_free( - points_t0, points_t1, plane_origins, plane_normals, - can_collide); - }, - R"ipc_Qu8mg5v7( - Determine if the step is collision free. - - Note: - Assumes the trajectory is linear. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - True if any collisions occur. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a, - "can_collide"_a); - - m.def( - "compute_point_plane_collision_free_stepsize", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals) { - return compute_point_plane_collision_free_stepsize( - points_t0, points_t1, plane_origins, plane_normals); - }, - R"ipc_Qu8mg5v7( - Computes a maximal step size that is collision free. - - Notes: - Assumes points_t0 is intersection free. - Assumes the trajectory is linear. - A value of 1.0 if a full step and 0.0 is no step. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - A step-size $\in [0, 1]$ that is collision free. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a); - - m.def( - "compute_point_plane_collision_free_stepsize", - [](Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) { - return compute_point_plane_collision_free_stepsize( - points_t0, points_t1, plane_origins, plane_normals, - can_collide); - }, - R"ipc_Qu8mg5v7( - Computes a maximal step size that is collision free. - - Notes: - Assumes points_t0 is intersection free. - Assumes the trajectory is linear. - A value of 1.0 if a full step and 0.0 is no step. - - Parameters: - points_t0: Points at start as rows of a matrix. - points_t1: Points at end as rows of a matrix. - plane_origins: Plane origins as rows of a matrix. - plane_normals: Plane normals as rows of a matrix. - can_collide: A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. - - Returns: - A step-size $\in [0, 1]$ that is collision free. - )ipc_Qu8mg5v7", - "points_t0"_a, "points_t1"_a, "plane_origins"_a, "plane_normals"_a, - "can_collide"_a); -} diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index a4dc25987..aa1ae83de 100644 --- a/src/ipc/CMakeLists.txt +++ b/src/ipc/CMakeLists.txt @@ -20,7 +20,6 @@ add_subdirectory(collisions) add_subdirectory(distance) add_subdirectory(friction) add_subdirectory(geometry) -add_subdirectory(implicits) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potentials) diff --git a/src/ipc/candidates/CMakeLists.txt b/src/ipc/candidates/CMakeLists.txt index f7efceb30..be0fc3139 100644 --- a/src/ipc/candidates/CMakeLists.txt +++ b/src/ipc/candidates/CMakeLists.txt @@ -13,8 +13,8 @@ set(SOURCES face_face.hpp face_vertex.cpp face_vertex.hpp - # plane_vertex.cpp - # plane_vertex.hpp + plane_vertex.cpp + plane_vertex.hpp vertex_vertex.cpp vertex_vertex.hpp ) diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index f12028d84..8f23e102a 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -111,6 +112,15 @@ void Candidates::build( vi = mesh.codim_vertices()[vi]; // Map back to vertices } } + + // Planes to vertices: + for (const auto& plane : mesh.planes) { + for (index_t vi = 0; vi < mesh.num_vertices(); ++vi) { + if (plane.signedDistance(vertices.row(vi)) < inflation_radius) { + pv_candidates.emplace_back(plane, vi); + } + } + } } void Candidates::build( @@ -195,6 +205,17 @@ void Candidates::build( vi = mesh.codim_vertices()[vi]; // Map back to vertices } } + + // Planes to vertices: + for (const auto& plane : mesh.planes) { + for (index_t vi = 0; vi < mesh.num_vertices(); ++vi) { + const double d_t0 = plane.signedDistance(vertices_t0.row(vi)); + const double d_t1 = plane.signedDistance(vertices_t1.row(vi)); + if (d_t0 < inflation_radius || d_t1 < inflation_radius) { + pv_candidates.emplace_back(plane, vi); + } + } + } } bool Candidates::is_step_collision_free( @@ -467,13 +488,14 @@ Eigen::VectorXd Candidates::compute_per_vertex_safe_distances( size_t Candidates::size() const { return vv_candidates.size() + ev_candidates.size() + ee_candidates.size() - + fv_candidates.size(); + + fv_candidates.size() + pv_candidates.size(); } bool Candidates::empty() const { return vv_candidates.empty() && ev_candidates.empty() - && ee_candidates.empty() && fv_candidates.empty(); + && ee_candidates.empty() && fv_candidates.empty() + && pv_candidates.empty(); } void Candidates::clear() @@ -482,6 +504,7 @@ void Candidates::clear() ev_candidates.clear(); ee_candidates.clear(); fv_candidates.clear(); + pv_candidates.clear(); } CollisionStencil& Candidates::operator[](size_t i) @@ -501,6 +524,10 @@ CollisionStencil& Candidates::operator[](size_t i) if (i < fv_candidates.size()) { return fv_candidates[i]; } + i -= fv_candidates.size(); + if (i < pv_candidates.size()) { + return pv_candidates[i]; + } throw std::out_of_range("Candidate index is out of range!"); } @@ -521,6 +548,10 @@ const CollisionStencil& Candidates::operator[](size_t i) const if (i < fv_candidates.size()) { return fv_candidates[i]; } + i -= fv_candidates.size(); + if (i < pv_candidates.size()) { + return pv_candidates[i]; + } throw std::out_of_range("Candidate index is out of range!"); } diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index c9ce8afcd..5ac2ca12e 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -215,6 +216,7 @@ class Candidates { std::vector ev_candidates; std::vector ee_candidates; std::vector fv_candidates; + std::vector pv_candidates; private: static bool default_is_active(double candidate) { return true; } diff --git a/src/ipc/candidates/edge_edge.hpp b/src/ipc/candidates/edge_edge.hpp index c1837f163..0e174fb14 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -17,7 +17,7 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 4; }; + int num_vertices() const override { return 4; } /// @brief Get the vertex IDs for the edge-edge pair /// @param edges The edge connectivity matrix diff --git a/src/ipc/candidates/edge_vertex.hpp b/src/ipc/candidates/edge_vertex.hpp index 866d6637a..b489fbe49 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -17,7 +17,7 @@ class EdgeVertexCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 3; }; + int num_vertices() const override { return 3; } /// @brief Get the vertex IDs for the edge-vertex pair /// @param edges The edge connectivity matrix diff --git a/src/ipc/candidates/face_vertex.hpp b/src/ipc/candidates/face_vertex.hpp index 9f9fc8bf9..ef8cd755f 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -17,7 +17,7 @@ class FaceVertexCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 4; }; + int num_vertices() const override { return 4; } /// @brief Get the vertex IDs for the face-vertex pair /// @param edges The edge connectivity matrix diff --git a/src/ipc/candidates/plane_vertex.cpp b/src/ipc/candidates/plane_vertex.cpp new file mode 100644 index 000000000..be4c8205e --- /dev/null +++ b/src/ipc/candidates/plane_vertex.cpp @@ -0,0 +1,95 @@ +#include "plane_vertex.hpp" + +#include +#include + +namespace ipc { + +PlaneVertexCandidate::PlaneVertexCandidate( + const Eigen::Hyperplane& _plane, const index_t _vertex_id) + : plane(_plane) + , vertex_id(_vertex_id) +{ +} + +double PlaneVertexCandidate::compute_distance( + Eigen::ConstRef point) const +{ + assert(point.size() == 3); + return point_plane_distance( + point, -plane.offset() * plane.normal(), plane.normal()); +} + +VectorMax12d PlaneVertexCandidate::compute_distance_gradient( + Eigen::ConstRef point) const +{ + assert(point.size() == 3); + return point_plane_distance_gradient( + point, -plane.offset() * plane.normal(), plane.normal()); +} + +MatrixMax12d PlaneVertexCandidate::compute_distance_hessian( + Eigen::ConstRef point) const +{ + assert(point.size() == 3); + return point_plane_distance_hessian( + point, -plane.offset() * plane.normal(), plane.normal()); +} + +VectorMax4d PlaneVertexCandidate::compute_coefficients( + Eigen::ConstRef positions) const +{ + VectorMax4d coeffs(1); + coeffs << 1.0; + return coeffs; +} + +VectorMax3d PlaneVertexCandidate::compute_unnormalized_normal( + Eigen::ConstRef positions) const +{ + return plane.normal(); +} + +MatrixMax +PlaneVertexCandidate::compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const +{ + return MatrixMax::Zero(positions.size(), positions.size()); +} + +bool PlaneVertexCandidate::ccd( + Eigen::ConstRef vertices_t0, + Eigen::ConstRef vertices_t1, + double& toi, + const double min_distance, + const double tmax, + const NarrowPhaseCCD& narrow_phase_ccd) const +{ + assert(min_distance == 0 && "Not implemented"); + assert(vertices_t0.size() == 3 && vertices_t1.size() == 3); + return point_static_plane_ccd( + vertices_t0, vertices_t1, -plane.offset() * plane.normal(), + plane.normal(), toi, narrow_phase_ccd.conservative_rescaling); +} + +bool PlaneVertexCandidate::operator==(const PlaneVertexCandidate& other) const +{ + return this->vertex_id == other.vertex_id + && this->plane.normal().isApprox(other.plane.normal()) + && this->plane.offset() == other.plane.offset(); +} + +bool PlaneVertexCandidate::operator!=(const PlaneVertexCandidate& other) const +{ + return !(*this == other); +} + +bool PlaneVertexCandidate::operator<(const PlaneVertexCandidate& other) const +{ + if (this->vertex_id == other.vertex_id) { + return this->plane.offset() < other.plane.offset(); + } + return this->vertex_id < other.vertex_id; +} + +} // namespace ipc diff --git a/src/ipc/candidates/plane_vertex.hpp b/src/ipc/candidates/plane_vertex.hpp new file mode 100644 index 000000000..349186134 --- /dev/null +++ b/src/ipc/candidates/plane_vertex.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include +#include + +namespace ipc { + +class PlaneVertexCandidate : virtual public CollisionStencil { +public: + PlaneVertexCandidate( + const Eigen::Hyperplane& plane, const index_t vertex_id); + + int num_vertices() const override { return 1; } + + std::array vertex_ids( + Eigen::ConstRef edges, + Eigen::ConstRef faces) const override + { + return { { vertex_id, -1, -1, -1 } }; + } + + using CollisionStencil::compute_coefficients; + using CollisionStencil::compute_distance; + using CollisionStencil::compute_distance_gradient; + using CollisionStencil::compute_distance_hessian; + + /// @brief Compute the distance between the point and plane. + /// @param point Point's position. + /// @return Distance of the stencil. + double compute_distance(Eigen::ConstRef point) const override; + + /// @brief Compute the gradient of the distance w.r.t. the point's positions. + /// @param point Point's position. + /// @return Distance gradient w.r.t. the point's positions. + VectorMax12d compute_distance_gradient( + Eigen::ConstRef point) const override; + + /// @brief Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions. + /// @param point Point's position. + /// @return Distance Hessian w.r.t. the point's positions. + MatrixMax12d compute_distance_hessian( + Eigen::ConstRef point) const override; + + /// @brief Compute the coefficients of the stencil. + /// @param positions Vertex positions. + /// @return Coefficients of the stencil. + VectorMax4d compute_coefficients( + Eigen::ConstRef positions) const override; + + /// @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. + /// @param[out] toi Computed time of impact (normalized). + /// @param[in] min_distance Minimum separation distance between primitives. + /// @param[in] tmax Maximum time (normalized) to look for collisions. + /// @param[in] narrow_phase_ccd The narrow phase CCD algorithm to use. + /// @return If the candidate had a collision over the time interval. + bool + ccd(Eigen::ConstRef vertices_t0, + Eigen::ConstRef vertices_t1, + double& toi, + const double min_distance = 0.0, + const double tmax = 1.0, + const NarrowPhaseCCD& narrow_phase_ccd = + DEFAULT_NARROW_PHASE_CCD) const override; + + /// @brief The plane of the candidate. + Eigen::Hyperplane plane; + + /// @brief The vertex's id. + index_t vertex_id; + + /// @brief Compare two PlaneVertexCandidates for equality. + bool operator==(const PlaneVertexCandidate& other) const; + + /// @brief Compare two PlaneVertexCandidates for inequality. + bool operator!=(const PlaneVertexCandidate& other) const; + + /// @brief Compare two PlaneVertexCandidates for less than. + bool operator<(const PlaneVertexCandidate& other) const; + +protected: + /// @brief Compute the normal vector of the stencil. + /// @param positions Vertex positions. + /// @return Normal vector of the stencil. + VectorMax3d compute_unnormalized_normal( + Eigen::ConstRef positions) const override; + + /// @brief Compute the Jacobian of the normal vector of the stencil. + /// @param positions Vertex positions. + /// @return Jacobian of the normal vector of the stencil. + MatrixMax compute_unnormalized_normal_jacobian( + Eigen::ConstRef positions) const override; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/candidates/vertex_vertex.hpp b/src/ipc/candidates/vertex_vertex.hpp index 943a87ef5..a2a440b6f 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -17,7 +17,7 @@ class VertexVertexCandidate : virtual public CollisionStencil { // ------------------------------------------------------------------------ // CollisionStencil - int num_vertices() const override { return 2; }; + int num_vertices() const override { return 2; } /// @brief Get the indices of the vertices /// @param edges edge matrix of mesh diff --git a/src/ipc/ccd/additive_ccd.cpp b/src/ipc/ccd/additive_ccd.cpp index 645298209..b0e536ccb 100644 --- a/src/ipc/ccd/additive_ccd.cpp +++ b/src/ipc/ccd/additive_ccd.cpp @@ -64,8 +64,8 @@ namespace { AdditiveCCD::AdditiveCCD( const long _max_iterations, const double _conservative_rescaling) : max_iterations(_max_iterations) - , conservative_rescaling(_conservative_rescaling) { + conservative_rescaling = _conservative_rescaling; } bool AdditiveCCD::additive_ccd( diff --git a/src/ipc/ccd/additive_ccd.hpp b/src/ipc/ccd/additive_ccd.hpp index 123477c16..875544c22 100644 --- a/src/ipc/ccd/additive_ccd.hpp +++ b/src/ipc/ccd/additive_ccd.hpp @@ -27,7 +27,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @brief Construct a new AdditiveCCD object. /// @param max_iterations The maximum number of iterations to use for the CCD algorithm. If set to UNLIMITTED_ITERATIONS, the algorithm will run until it converges. - /// @param conservative_rescaling The conservative rescaling of the time of impact. + /// @param conservative_rescaling The conservative rescaling value used to avoid taking steps exactly to impact. explicit AdditiveCCD( const long max_iterations = UNLIMITTED_ITERATIONS, const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); @@ -127,9 +127,6 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @brief Maximum number of iterations. long max_iterations; - /// @brief The conservative rescaling value used to avoid taking steps exactly to impact. - double conservative_rescaling; - private: /// @brief Computes the time of impact between two objects using additive continuous collision detection. /// @param x Initial positions diff --git a/src/ipc/ccd/inexact_ccd.cpp b/src/ipc/ccd/inexact_ccd.cpp index 233664836..09df70b82 100644 --- a/src/ipc/ccd/inexact_ccd.cpp +++ b/src/ipc/ccd/inexact_ccd.cpp @@ -12,9 +12,9 @@ namespace ipc { -InexactCCD::InexactCCD(const double conservative_rescaling) - : conservative_rescaling(conservative_rescaling) +InexactCCD::InexactCCD(const double _conservative_rescaling) { + conservative_rescaling = _conservative_rescaling; } bool InexactCCD::ccd_strategy( diff --git a/src/ipc/ccd/inexact_ccd.hpp b/src/ipc/ccd/inexact_ccd.hpp index 6e0f085d3..7f36964b3 100644 --- a/src/ipc/ccd/inexact_ccd.hpp +++ b/src/ipc/ccd/inexact_ccd.hpp @@ -19,7 +19,7 @@ class InexactCCD : public NarrowPhaseCCD { static constexpr double SMALL_TOI = 1e-6; /// @brief Construct a new AdditiveCCD object. - /// @param conservative_rescaling The conservative rescaling of the time of impact. + /// @param conservative_rescaling The conservative rescaling value used to avoid taking steps exactly to impact. InexactCCD( const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); @@ -115,9 +115,6 @@ class InexactCCD : public NarrowPhaseCCD { const double min_distance = 0.0, const double tmax = 1.0) const override; - /// @brief Conservative rescaling of the time of impact. - double conservative_rescaling; - private: /// @brief Computes the time of impact between two points in 3D using continuous collision detection. /// @param[in] p0_t0 The initial position of the first point. diff --git a/src/ipc/ccd/narrow_phase_ccd.hpp b/src/ipc/ccd/narrow_phase_ccd.hpp index 3911f9954..cc6d67625 100644 --- a/src/ipc/ccd/narrow_phase_ccd.hpp +++ b/src/ipc/ccd/narrow_phase_ccd.hpp @@ -102,6 +102,21 @@ class NarrowPhaseCCD { double& toi, const double min_distance = 0.0, const double tmax = 1.0) const = 0; + + /// @brief The conservative rescaling value used to avoid taking steps exactly to impact. + /// + /// This is used to creat a effective minimum distance between objects, + /// which can help prevent numerical issues when objects are very close to + /// each other. + /// + /// I.e., min_effective_distance = (1.0 - conservative_rescaling) * + /// (initial_distance - min_distance) + /// + /// If the time of impact is small (e.g. 1e-6), then the CCD will be rerun + /// without a effective minimum distance to try to get a more accurate time + /// of impact. In this case, the conservative rescaling will be used to + /// scale the time of impact to avoid taking steps exactly to impact. + double conservative_rescaling; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/ccd/point_static_plane.cpp b/src/ipc/ccd/point_static_plane.cpp index 0158fc659..9418c2b1b 100644 --- a/src/ipc/ccd/point_static_plane.cpp +++ b/src/ipc/ccd/point_static_plane.cpp @@ -6,7 +6,7 @@ namespace ipc { -inline bool is_in_01(double x) { return 0 <= x && x <= 1; }; +inline bool is_in_01(double x) { return 0 <= x && x <= 1; } bool point_static_plane_ccd( Eigen::ConstRef p_t0, diff --git a/src/ipc/ccd/tight_inclusion_ccd.cpp b/src/ipc/ccd/tight_inclusion_ccd.cpp index e7a5441a3..e13563d2e 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.cpp +++ b/src/ipc/ccd/tight_inclusion_ccd.cpp @@ -26,8 +26,8 @@ TightInclusionCCD::TightInclusionCCD( const double _conservative_rescaling) : tolerance(_tolerance) , max_iterations(_max_iterations) - , conservative_rescaling(_conservative_rescaling) { + conservative_rescaling = _conservative_rescaling; } bool TightInclusionCCD::ccd_strategy( diff --git a/src/ipc/ccd/tight_inclusion_ccd.hpp b/src/ipc/ccd/tight_inclusion_ccd.hpp index 5f9637701..08b434a0d 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.hpp +++ b/src/ipc/ccd/tight_inclusion_ccd.hpp @@ -21,7 +21,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @brief Construct a new AdditiveCCD object. /// @param tolerance The tolerance used for the CCD algorithm. /// @param max_iterations The maximum number of iterations for the CCD algorithm. - /// @param conservative_rescaling The conservative rescaling of the time of impact. + /// @param conservative_rescaling The conservative rescaling value used to avoid taking steps exactly to impact. explicit TightInclusionCCD( const double tolerance = DEFAULT_TOLERANCE, const long max_iterations = DEFAULT_MAX_ITERATIONS, @@ -125,9 +125,6 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @brief Maximum number of iterations. long max_iterations; - /// @brief Conservative rescaling of the time of impact. - double conservative_rescaling; - private: /// @brief Computes the time of impact between two points in 3D using continuous collision detection. /// @param[in] p0_t0 The initial position of the first point. diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index c267bcc8c..ae1a215a6 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -327,6 +327,11 @@ class CollisionMesh { /// primitives can collide with all other primitives. std::function can_collide = default_can_collide; + /// @brief Analytic planes in the scene that can be collided with. + /// This is useful for representing infinite planes (e.g., the ground plane) + /// or planes that are not part of the collision mesh. + std::vector> planes; + protected: // ----------------------------------------------------------------------- // Helper initialization functions diff --git a/src/ipc/collisions/normal/CMakeLists.txt b/src/ipc/collisions/normal/CMakeLists.txt index 98ea4c0fe..28266c59e 100644 --- a/src/ipc/collisions/normal/CMakeLists.txt +++ b/src/ipc/collisions/normal/CMakeLists.txt @@ -9,7 +9,6 @@ set(SOURCES normal_collisions_builder.hpp normal_collisions.cpp normal_collisions.hpp - plane_vertex.cpp plane_vertex.hpp vertex_vertex.hpp ) diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 6b5611f48..92fdc2cfe 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -146,6 +146,24 @@ void NormalCollisions::build( NormalCollisionsBuilder::merge(storage, *this); + for (const auto& [plane, vertex_id] : candidates.pv_candidates) { + const double d = plane.signedDistance(vertices.row(vertex_id)); + if (d < dhat + dmin) { + const double weight = + use_area_weighting() ? mesh.vertex_area(vertex_id) : 1; + + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives()) { + weight_gradient = use_area_weighting() + ? mesh.vertex_area_gradient(vertex_id) + : Eigen::SparseVector(vertices.size()); + } + + pv_collisions.emplace_back( + plane, vertex_id, weight, weight_gradient); + } + } + // logger().debug(to_string(mesh, vertices)); for (size_t ci = 0; ci < size(); ci++) { diff --git a/src/ipc/collisions/normal/plane_vertex.cpp b/src/ipc/collisions/normal/plane_vertex.cpp deleted file mode 100644 index 974b74205..000000000 --- a/src/ipc/collisions/normal/plane_vertex.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "plane_vertex.hpp" - -#include -#include - -namespace ipc { - -PlaneVertexNormalCollision::PlaneVertexNormalCollision( - Eigen::ConstRef _plane_origin, - Eigen::ConstRef _plane_normal, - const index_t _vertex_id) - : plane_origin(_plane_origin) - , plane_normal(_plane_normal) - , vertex_id(_vertex_id) -{ -} - -double PlaneVertexNormalCollision::compute_distance( - Eigen::ConstRef point) const -{ - assert(point.size() == plane_origin.size()); - return point_plane_distance(point, plane_origin, plane_normal); -} - -VectorMax12d PlaneVertexNormalCollision::compute_distance_gradient( - Eigen::ConstRef point) const -{ - assert(point.size() == plane_origin.size()); - return point_plane_distance_gradient(point, plane_origin, plane_normal); -} - -MatrixMax12d PlaneVertexNormalCollision::compute_distance_hessian( - Eigen::ConstRef point) const -{ - assert(point.size() == plane_origin.size()); - return point_plane_distance_hessian(point, plane_origin, plane_normal); -} - -VectorMax4d PlaneVertexNormalCollision::compute_coefficients( - Eigen::ConstRef positions) const -{ - VectorMax4d coeffs(1); - coeffs << 1.0; - return coeffs; -} - -VectorMax3d PlaneVertexNormalCollision::compute_unnormalized_normal( - Eigen::ConstRef positions) const -{ - return plane_normal; -} - -MatrixMax -PlaneVertexNormalCollision::compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const -{ - return MatrixMax::Zero(positions.size(), positions.size()); -} - -bool PlaneVertexNormalCollision::ccd( - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - double& toi, - const double min_distance, - const double tmax, - const NarrowPhaseCCD& narrow_phase_ccd) const -{ - assert(min_distance == 0 && "Not implemented"); - return point_static_plane_ccd( - vertices_t0, vertices_t1, plane_origin, plane_normal, toi); -} - -} // namespace ipc diff --git a/src/ipc/collisions/normal/plane_vertex.hpp b/src/ipc/collisions/normal/plane_vertex.hpp index 34901afb2..1caed7e2b 100644 --- a/src/ipc/collisions/normal/plane_vertex.hpp +++ b/src/ipc/collisions/normal/plane_vertex.hpp @@ -1,92 +1,30 @@ #pragma once +#include #include #include namespace ipc { -class PlaneVertexNormalCollision : public NormalCollision { +class PlaneVertexNormalCollision : public PlaneVertexCandidate, + public NormalCollision { public: - PlaneVertexNormalCollision( - Eigen::ConstRef plane_origin, - Eigen::ConstRef plane_normal, - const index_t vertex_id); - - int num_vertices() const override { return 1; } + using PlaneVertexCandidate::PlaneVertexCandidate; - std::array vertex_ids( - Eigen::ConstRef edges, - Eigen::ConstRef faces) const override + PlaneVertexNormalCollision(const PlaneVertexCandidate& candidate) + : PlaneVertexCandidate(candidate) { - return { { vertex_id, -1, -1, -1 } }; } - using CollisionStencil::compute_coefficients; - using CollisionStencil::compute_distance; - using CollisionStencil::compute_distance_gradient; - using CollisionStencil::compute_distance_hessian; - - /// @brief Compute the distance between the point and plane. - /// @param point Point's position. - /// @return Distance of the stencil. - double compute_distance(Eigen::ConstRef point) const override; - - /// @brief Compute the gradient of the distance w.r.t. the point's positions. - /// @param point Point's position. - /// @return Distance gradient w.r.t. the point's positions. - VectorMax12d compute_distance_gradient( - Eigen::ConstRef point) const override; - - /// @brief Compute the distance Hessian of the stencil w.r.t. the stencil's vertex positions. - /// @param point Point's position. - /// @return Distance Hessian w.r.t. the point's positions. - MatrixMax12d compute_distance_hessian( - Eigen::ConstRef point) const override; - - /// @brief Compute the coefficients of the stencil. - /// @param positions Vertex positions. - /// @return Coefficients of the stencil. - VectorMax4d compute_coefficients( - Eigen::ConstRef positions) const override; - - /// @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. - /// @param[out] toi Computed time of impact (normalized). - /// @param[in] min_distance Minimum separation distance between primitives. - /// @param[in] tmax Maximum time (normalized) to look for collisions. - /// @param[in] narrow_phase_ccd The narrow phase CCD algorithm to use. - /// @return If the candidate had a collision over the time interval. - bool - ccd(Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - double& toi, - const double min_distance = 0.0, - const double tmax = 1.0, - const NarrowPhaseCCD& narrow_phase_ccd = - DEFAULT_NARROW_PHASE_CCD) const override; - - /// @brief The plane's origin. - VectorMax3d plane_origin; - - /// @brief The plane's normal. - VectorMax3d plane_normal; - - /// @brief The vertex's id. - index_t vertex_id; - -protected: - /// @brief Compute the normal vector of the stencil. - /// @param positions Vertex positions. - /// @return Normal vector of the stencil. - VectorMax3d compute_unnormalized_normal( - Eigen::ConstRef positions) const override; - - /// @brief Compute the Jacobian of the normal vector of the stencil. - /// @param positions Vertex positions. - /// @return Jacobian of the normal vector of the stencil. - MatrixMax compute_unnormalized_normal_jacobian( - Eigen::ConstRef positions) const override; + PlaneVertexNormalCollision( + const Eigen::Hyperplane& _plane, + const index_t _vertex_id, + const double _weight, + const Eigen::SparseVector& _weight_gradient) + : PlaneVertexCandidate(_plane, _vertex_id) + , NormalCollision(_weight, _weight_gradient) + { + } }; } // namespace ipc diff --git a/src/ipc/collisions/tangential/CMakeLists.txt b/src/ipc/collisions/tangential/CMakeLists.txt index 3f5ac3b88..251503f3a 100644 --- a/src/ipc/collisions/tangential/CMakeLists.txt +++ b/src/ipc/collisions/tangential/CMakeLists.txt @@ -5,6 +5,8 @@ set(SOURCES edge_vertex.hpp face_vertex.cpp face_vertex.hpp + plane_vertex.cpp + plane_vertex.hpp tangential_collision.cpp tangential_collision.hpp tangential_collisions.cpp diff --git a/src/ipc/collisions/tangential/plane_vertex.cpp b/src/ipc/collisions/tangential/plane_vertex.cpp new file mode 100644 index 000000000..09f08ba91 --- /dev/null +++ b/src/ipc/collisions/tangential/plane_vertex.cpp @@ -0,0 +1,96 @@ +#include "plane_vertex.hpp" + +#include +#include +#include +#include + +namespace ipc { + +PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision) + : PlaneVertexCandidate(collision) +{ + this->weight = collision.weight; + this->weight_gradient = collision.weight_gradient; +} + +PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) + : PlaneVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_force); +} + +PlaneVertexTangentialCollision::PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const NormalPotential& normal_potential) + : PlaneVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_potential); +} + +// ============================================================================ + +MatrixMax PlaneVertexTangentialCollision::compute_tangent_basis( + Eigen::ConstRef positions) const +{ + assert(positions.size() == ndof()); + Eigen::Vector3d p0 = -plane.offset() * plane.normal(); + Eigen::Vector3d p1 = p0 + plane.normal(); + return point_point_tangent_basis( + p0.head(positions.size()), p1.head(positions.size())); +} + +MatrixMax +PlaneVertexTangentialCollision::compute_tangent_basis_jacobian( + Eigen::ConstRef positions) const +{ + assert(positions.size() == ndof()); + return MatrixMax::Zero( + ndof() * ndof(), positions.size() == 2 ? 1 : 2); +} + +// ============================================================================ + +VectorMax2d PlaneVertexTangentialCollision::compute_closest_point( + Eigen::ConstRef positions) const +{ + return VectorMax2d(); +} + +MatrixMax +PlaneVertexTangentialCollision::compute_closest_point_jacobian( + Eigen::ConstRef positions) const +{ + return MatrixMax(); +} + +// ============================================================================ + +VectorMax3d PlaneVertexTangentialCollision::relative_velocity( + Eigen::ConstRef velocities) const +{ + assert(velocities.size() <= 3); + return velocities; +} + +MatrixMax +PlaneVertexTangentialCollision::relative_velocity_matrix( + Eigen::ConstRef _closest_point) const +{ + return MatrixMax::Identity(ndof(), ndof()); +} + +MatrixMax +PlaneVertexTangentialCollision::relative_velocity_matrix_jacobian( + Eigen::ConstRef _closest_point) const +{ + return MatrixMax::Zero( + _closest_point.size() * ndof(), ndof()); +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/tangential/plane_vertex.hpp b/src/ipc/collisions/tangential/plane_vertex.hpp new file mode 100644 index 000000000..20f2a6ef3 --- /dev/null +++ b/src/ipc/collisions/tangential/plane_vertex.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include + +namespace ipc { + +class PlaneVertexTangentialCollision : public PlaneVertexCandidate, + public TangentialCollision { +public: + using PlaneVertexCandidate::PlaneVertexCandidate; + + PlaneVertexTangentialCollision(const PlaneVertexNormalCollision& collision); + + PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + + PlaneVertexTangentialCollision( + const PlaneVertexNormalCollision& collision, + Eigen::ConstRef positions, + const NormalPotential& normal_potential); + +protected: + MatrixMax compute_tangent_basis( + Eigen::ConstRef positions) const override; + + MatrixMax compute_tangent_basis_jacobian( + Eigen::ConstRef positions) const override; + + VectorMax2d compute_closest_point( + Eigen::ConstRef positions) const override; + + MatrixMax compute_closest_point_jacobian( + Eigen::ConstRef positions) const override; + + VectorMax3d + relative_velocity(Eigen::ConstRef velocities) const override; + + using TangentialCollision::relative_velocity_matrix; + + MatrixMax relative_velocity_matrix( + Eigen::ConstRef closest_point) const override; + + MatrixMax relative_velocity_matrix_jacobian( + Eigen::ConstRef closest_point) const override; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/tangential/tangential_collisions.cpp b/src/ipc/collisions/tangential/tangential_collisions.cpp index 0c44cdeb6..9424ab2b7 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.cpp +++ b/src/ipc/collisions/tangential/tangential_collisions.cpp @@ -32,7 +32,8 @@ void TangentialCollisions::build( const auto& C_ev = collisions.ev_collisions; const auto& C_ee = collisions.ee_collisions; const auto& C_fv = collisions.fv_collisions; - auto& [FC_vv, FC_ev, FC_ee, FC_fv] = *this; + const auto& C_pv = collisions.pv_collisions; + auto& [FC_vv, FC_ev, FC_ee, FC_fv, FC_pv] = *this; FC_vv.reserve(C_vv.size()); for (const auto& c_vv : C_vv) { @@ -107,6 +108,15 @@ void TangentialCollisions::build( + FC_fv.back().closest_point[1] * (mu_k(f2i) - mu_k(f0i)); FC_fv.back().mu_k = blend_mu(face_mu_k, mu_k(vi)); } + + FC_pv.reserve(C_pv.size()); + for (const auto& c_pv : C_pv) { + FC_pv.emplace_back( + c_pv, c_pv.dof(vertices, edges, faces), normal_potential); + const auto& [vi, _0, _1, _2] = FC_pv.back().vertex_ids(edges, faces); + FC_pv.back().mu_s = mu_s(vi); + FC_pv.back().mu_k = mu_k(vi); + } } void TangentialCollisions::build( @@ -127,7 +137,7 @@ void TangentialCollisions::build( clear(); - auto& [FC_vv, FC_ev, FC_ee, FC_fv] = *this; + auto& [FC_vv, FC_ev, FC_ee, FC_fv, FC_pv] = *this; // FC_vv.reserve(C_vv.size()); for (size_t i = 0; i < collisions.size(); i++) { @@ -301,13 +311,14 @@ void TangentialCollisions::build( size_t TangentialCollisions::size() const { return vv_collisions.size() + ev_collisions.size() + ee_collisions.size() - + fv_collisions.size(); + + fv_collisions.size() + pv_collisions.size(); } bool TangentialCollisions::empty() const { return vv_collisions.empty() && ev_collisions.empty() - && ee_collisions.empty() && fv_collisions.empty(); + && ee_collisions.empty() && fv_collisions.empty() + && pv_collisions.empty(); } void TangentialCollisions::clear() @@ -316,6 +327,7 @@ void TangentialCollisions::clear() ev_collisions.clear(); ee_collisions.clear(); fv_collisions.clear(); + pv_collisions.clear(); } TangentialCollision& TangentialCollisions::operator[](size_t i) @@ -335,6 +347,10 @@ TangentialCollision& TangentialCollisions::operator[](size_t i) if (i < fv_collisions.size()) { return fv_collisions[i]; } + i -= fv_collisions.size(); + if (i < pv_collisions.size()) { + return pv_collisions[i]; + } throw std::out_of_range("Friction collision index is out of range!"); } @@ -355,6 +371,10 @@ const TangentialCollision& TangentialCollisions::operator[](size_t i) const if (i < fv_collisions.size()) { return fv_collisions[i]; } + i -= fv_collisions.size(); + if (i < pv_collisions.size()) { + return pv_collisions[i]; + } throw std::out_of_range("Friction collision index is out of range!"); } diff --git a/src/ipc/collisions/tangential/tangential_collisions.hpp b/src/ipc/collisions/tangential/tangential_collisions.hpp index 5ba55125b..71c202aca 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.hpp +++ b/src/ipc/collisions/tangential/tangential_collisions.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -136,6 +137,8 @@ class TangentialCollisions { std::vector ee_collisions; /// @brief Face-vertex tangential collisions. std::vector fv_collisions; + /// @brief Plane-vertex tangential collisions. + std::vector pv_collisions; }; } // namespace ipc diff --git a/src/ipc/implicits/CMakeLists.txt b/src/ipc/implicits/CMakeLists.txt deleted file mode 100644 index c210df385..000000000 --- a/src/ipc/implicits/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -set(SOURCES - plane.cpp - plane.hpp -) - -target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/implicits/plane.cpp b/src/ipc/implicits/plane.cpp deleted file mode 100644 index 5dbe8451d..000000000 --- a/src/ipc/implicits/plane.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "plane.hpp" - -#include -#include - -#include -#include - -namespace ipc { - -void construct_point_plane_collisions( - Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const double dhat, - std::vector& pv_collisions, - const double dmin, - const std::function& can_collide) -{ - pv_collisions.clear(); - - double dhat_squared = dhat * dhat; - double dmin_squared = dmin * dmin; - - // Cull the candidates by measuring the distance and dropping those that are - // greater than dhat. - - size_t n_planes = plane_origins.rows(); - assert(plane_normals.rows() == n_planes); - - for (size_t vi = 0; vi < points.rows(); vi++) { - for (size_t pi = 0; pi < n_planes; pi++) { - if (!can_collide(vi, pi)) { - continue; - } - - const auto& plane_origin = plane_origins.row(pi); - const auto& plane_normal = plane_normals.row(pi); - - double distance_sqr = point_plane_distance( - points.row(vi), plane_origin, plane_normal); - - if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) { - pv_collisions.emplace_back(plane_origin, plane_normal, vi); - pv_collisions.back().dmin = dmin; - } - } - } -} - -// ============================================================================ - -bool is_step_point_plane_collision_free( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) -{ - size_t n_planes = plane_origins.rows(); - assert(plane_normals.rows() == n_planes); - assert(points_t0.rows() == points_t1.rows()); - - for (size_t vi = 0; vi < points_t0.rows(); vi++) { - for (size_t pi = 0; pi < n_planes; pi++) { - if (!can_collide(vi, pi)) { - continue; - } - - const auto& plane_origin = plane_origins.row(pi); - const auto& plane_normal = plane_normals.row(pi); - - double toi; - bool is_collision = point_static_plane_ccd( - points_t0.row(vi), points_t1.row(vi), plane_origin, - plane_normal, toi); - - if (is_collision) { - return false; - } - } - } - - return true; -} - -// ============================================================================ - -double compute_point_plane_collision_free_stepsize( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide) -{ - size_t n_planes = plane_origins.rows(); - assert(plane_normals.rows() == n_planes); - assert(points_t0.rows() == points_t1.rows()); - - const double earliest_toi = tbb::parallel_reduce( - tbb::blocked_range(0, points_t0.rows()), - /*inital_step_size=*/1.0, - [&](tbb::blocked_range r, double current_toi) { - for (size_t vi = r.begin(); vi < r.end(); vi++) { - for (size_t pi = 0; pi < n_planes; pi++) { - if (!can_collide(vi, pi)) { - continue; - } - - const auto& plane_origin = plane_origins.row(pi); - const auto& plane_normal = plane_normals.row(pi); - - double toi; - bool are_colliding = point_static_plane_ccd( - points_t0.row(vi), points_t1.row(vi), plane_origin, - plane_normal, toi); - - if (are_colliding) { - current_toi = std::min(current_toi, toi); - } - } - } - return current_toi; - }, - [&](double a, double b) { return std::min(a, b); }); - - assert(earliest_toi >= 0 && earliest_toi <= 1.0); - return earliest_toi; -} - -} // namespace ipc diff --git a/src/ipc/implicits/plane.hpp b/src/ipc/implicits/plane.hpp deleted file mode 100644 index 7b3bed163..000000000 --- a/src/ipc/implicits/plane.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include - -#include - -#include - -namespace ipc { - -inline bool -default_can_point_plane_collide(size_t /*unused*/, size_t /*unused*/) -{ - return true; -} - -/// @brief Construct a set of point-plane distance collisions used to compute -/// the barrier potential. -/// -/// @note The given pv_collisions will be cleared. -/// -/// @param[in] points Points as rows of a matrix. -/// @param[in] plane_origins Plane origins as rows of a matrix. -/// @param[in] plane_normals Plane normals as rows of a matrix. -/// @param[in] dhat The activation distance of the barrier. -/// @param[out] pv_collisions The constructed set of collisions. -/// @param[in] dmin Minimum distance. -/// @param[in] can_collide A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. -void construct_point_plane_collisions( - Eigen::ConstRef points, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const double dhat, - std::vector& pv_collisions, - const double dmin = 0, - const std::function& can_collide = - default_can_point_plane_collide); - -// ============================================================================ -// Collision detection - -/// @brief Determine if the step is collision free. -/// -/// @note Assumes the trajectory is linear. -/// -/// @param[in] points_t0 Points at start as rows of a matrix. -/// @param[in] points_t1 Points at end as rows of a matrix. -/// @param[in] plane_origins Plane origins as rows of a matrix. -/// @param[in] plane_normals Plane normals as rows of a matrix. -/// @param[in] can_collide A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. -/// @returns True if any collisions occur. -bool is_step_point_plane_collision_free( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide = - default_can_point_plane_collide); - -/// @brief Computes a maximal step size that is collision free. -/// -/// @note Assumes points_t0 is intersection free. -/// @note Assumes the trajectory is linear. -/// @note A value of 1.0 if a full step and 0.0 is no step. -/// -/// @param points_t0 Points at start as rows of a matrix. -/// @param points_t1 Points at end as rows of a matrix. -/// @param plane_origins Plane origins as rows of a matrix. -/// @param plane_normals Plane normals as rows of a matrix. -/// @param can_collide A function that takes a vertex ID (row numbers in points) and a plane ID (row number in plane_origins) then returns true if the vertex can collide with the plane. By default all points can collide with all planes. -/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. -double compute_point_plane_collision_free_stepsize( - Eigen::ConstRef points_t0, - Eigen::ConstRef points_t1, - Eigen::ConstRef plane_origins, - Eigen::ConstRef plane_normals, - const std::function& can_collide = - default_can_point_plane_collide); - -} // namespace ipc diff --git a/src/ipc/potentials/potential.cpp b/src/ipc/potentials/potential.cpp index 2eb1b2062..c4da5d2b2 100644 --- a/src/ipc/potentials/potential.cpp +++ b/src/ipc/potentials/potential.cpp @@ -51,7 +51,7 @@ double Potential::operator()( } return partial_sum; }, - [](double a, double b) { return a + b; }); + std::plus()); } template diff --git a/src/ipc/smooth_contact/primitives/edge.cpp b/src/ipc/smooth_contact/primitives/edge.cpp index 15d50d354..24a0db29a 100644 --- a/src/ipc/smooth_contact/primitives/edge.cpp +++ b/src/ipc/smooth_contact/primitives/edge.cpp @@ -9,7 +9,7 @@ Edge::Edge( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/edge.hpp b/src/ipc/smooth_contact/primitives/edge.hpp index e0711c7ed..d1016a948 100644 --- a/src/ipc/smooth_contact/primitives/edge.hpp +++ b/src/ipc/smooth_contact/primitives/edge.hpp @@ -20,7 +20,7 @@ template class Edge : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/edge2.cpp b/src/ipc/smooth_contact/primitives/edge2.cpp index c009fdd56..fd3870e02 100644 --- a/src/ipc/smooth_contact/primitives/edge2.cpp +++ b/src/ipc/smooth_contact/primitives/edge2.cpp @@ -7,7 +7,7 @@ Edge2::Edge2( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/edge2.hpp b/src/ipc/smooth_contact/primitives/edge2.hpp index b52c13b43..d9bfe5efa 100644 --- a/src/ipc/smooth_contact/primitives/edge2.hpp +++ b/src/ipc/smooth_contact/primitives/edge2.hpp @@ -15,7 +15,7 @@ class Edge2 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/edge3.cpp b/src/ipc/smooth_contact/primitives/edge3.cpp index 67ec74c7a..4390fafab 100644 --- a/src/ipc/smooth_contact/primitives/edge3.cpp +++ b/src/ipc/smooth_contact/primitives/edge3.cpp @@ -384,7 +384,7 @@ Edge3::Edge3( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/edge3.hpp b/src/ipc/smooth_contact/primitives/edge3.hpp index 32ce6f307..eb1193480 100644 --- a/src/ipc/smooth_contact/primitives/edge3.hpp +++ b/src/ipc/smooth_contact/primitives/edge3.hpp @@ -15,7 +15,7 @@ class Edge3 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/face.cpp b/src/ipc/smooth_contact/primitives/face.cpp index 2d1254e85..9621b8f7e 100644 --- a/src/ipc/smooth_contact/primitives/face.cpp +++ b/src/ipc/smooth_contact/primitives/face.cpp @@ -24,7 +24,7 @@ Face::Face( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/face.hpp b/src/ipc/smooth_contact/primitives/face.hpp index 962457acd..db061b3f6 100644 --- a/src/ipc/smooth_contact/primitives/face.hpp +++ b/src/ipc/smooth_contact/primitives/face.hpp @@ -15,7 +15,7 @@ class Face : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); int n_vertices() const override; diff --git a/src/ipc/smooth_contact/primitives/point2.cpp b/src/ipc/smooth_contact/primitives/point2.cpp index 52f415dfa..9d7e0ba79 100644 --- a/src/ipc/smooth_contact/primitives/point2.cpp +++ b/src/ipc/smooth_contact/primitives/point2.cpp @@ -107,7 +107,7 @@ Point2::Point2( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/point2.hpp b/src/ipc/smooth_contact/primitives/point2.hpp index 120468a73..a72ac2ea2 100644 --- a/src/ipc/smooth_contact/primitives/point2.hpp +++ b/src/ipc/smooth_contact/primitives/point2.hpp @@ -15,7 +15,7 @@ class Point2 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); Point2( diff --git a/src/ipc/smooth_contact/primitives/point3.cpp b/src/ipc/smooth_contact/primitives/point3.cpp index e82fd3a65..b2455acdf 100644 --- a/src/ipc/smooth_contact/primitives/point3.cpp +++ b/src/ipc/smooth_contact/primitives/point3.cpp @@ -10,7 +10,7 @@ Point3::Point3( const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params) : Primitive(id, params) { diff --git a/src/ipc/smooth_contact/primitives/point3.hpp b/src/ipc/smooth_contact/primitives/point3.hpp index 32407d7cd..c55ec437d 100644 --- a/src/ipc/smooth_contact/primitives/point3.hpp +++ b/src/ipc/smooth_contact/primitives/point3.hpp @@ -15,7 +15,7 @@ class Point3 : public Primitive { const index_t id, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const VectorMax3d& d, + Eigen::ConstRef d, const SmoothContactParameters& params); Point3( diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index e81869efd..f0086228e 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -6,6 +6,8 @@ set(SOURCES local_to_global.hpp logger.cpp logger.hpp + matrix_cache.cpp + matrix_cache.hpp merge_thread_local.hpp profiler.cpp profiler.hpp @@ -16,9 +18,6 @@ set(SOURCES vertex_to_min_edge.cpp vertex_to_min_edge.hpp world_bbox_diagonal_length.hpp - autodiff_types.hpp - matrix_cache.hpp - matrix_cache.cpp ) target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 140d5a071..2dd00f79b 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -9,7 +9,7 @@ // NOTE: Avoid error about abs casting double to int. Eigen does this // internally but seemingly only if EIGEN_DONT_VECTORIZE is not defined. // TODO: We should always use std::abs to avoid this issue. -EIGEN_USING_STD(abs); // using std::abs; +EIGEN_USING_STD(abs) // using std::abs; #endif namespace Eigen { diff --git a/tests/src/tests/candidates/test_candidates.cpp b/tests/src/tests/candidates/test_candidates.cpp index 350d1f67f..aa1b5e55b 100644 --- a/tests/src/tests/candidates/test_candidates.cpp +++ b/tests/src/tests/candidates/test_candidates.cpp @@ -3,6 +3,7 @@ #include #include #include +#include using namespace ipc; @@ -13,8 +14,12 @@ TEST_CASE("Candidates", "[candidates]") candidates.ev_candidates = { { 3, 4 }, { 5, 6 } }; candidates.ee_candidates = { { 6, 7 }, { 8, 9 } }; candidates.fv_candidates = { { 10, 11 }, { 12, 13 } }; + candidates.pv_candidates.emplace_back( + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 14); + candidates.pv_candidates.emplace_back( + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 15); - CHECK(candidates.size() == 8); + CHECK(candidates.size() == 10); CHECK( dynamic_cast(candidates[0]) @@ -28,11 +33,13 @@ TEST_CASE("Candidates", "[candidates]") CHECK( dynamic_cast(candidates[6]) == candidates.fv_candidates[0]); + CHECK(&(candidates[8]) == &(candidates.pv_candidates[0])); CHECK(&(candidates[0]) == &(candidates.vv_candidates[0])); CHECK(&(candidates[2]) == &(candidates.ev_candidates[0])); CHECK(&(candidates[4]) == &(candidates.ee_candidates[0])); CHECK(&(candidates[6]) == &(candidates.fv_candidates[0])); + CHECK(&(candidates[8]) == &(candidates.pv_candidates[0])); try { candidates[candidates.size()]; @@ -46,22 +53,26 @@ TEST_CASE("Candidates", "[candidates]") const Candidates& const_candidates = candidates; CHECK( - dynamic_cast(const_candidates[0]) + const_candidates[0].as() == candidates.vv_candidates[0]); CHECK( - dynamic_cast(const_candidates[2]) + const_candidates[2].as() == candidates.ev_candidates[0]); CHECK( - dynamic_cast(const_candidates[4]) + const_candidates[4].as() == candidates.ee_candidates[0]); CHECK( - dynamic_cast(const_candidates[6]) + const_candidates[6].as() == candidates.fv_candidates[0]); + CHECK( + const_candidates[8].as() + == candidates.pv_candidates[0]); CHECK(&(const_candidates[0]) == &(candidates.vv_candidates[0])); CHECK(&(const_candidates[2]) == &(candidates.ev_candidates[0])); CHECK(&(const_candidates[4]) == &(candidates.ee_candidates[0])); CHECK(&(const_candidates[6]) == &(candidates.fv_candidates[0])); + CHECK(&(const_candidates[8]) == &(candidates.pv_candidates[0])); try { const_candidates[candidates.size()]; @@ -139,3 +150,12 @@ TEST_CASE("Face-Face Candidate", "[candidates][face-face]") CHECK(FaceFaceCandidate(0, 1) < FaceFaceCandidate(2, 0)); CHECK(!(FaceFaceCandidate(1, 1) < FaceFaceCandidate(0, 2))); } + +TEST_CASE("Plane-Vertex Candidate", "[candidates][plane-vertex]") +{ + Eigen::Hyperplane plane(Eigen::Vector3d::UnitY(), 0); + CHECK(PlaneVertexCandidate(plane, 1) == PlaneVertexCandidate(plane, 1)); + CHECK(PlaneVertexCandidate(plane, 1) != PlaneVertexCandidate(plane, 2)); + CHECK(PlaneVertexCandidate(plane, 1) < PlaneVertexCandidate(plane, 2)); + CHECK(!(PlaneVertexCandidate(plane, 2) < PlaneVertexCandidate(plane, 1))); +} \ No newline at end of file diff --git a/tests/src/tests/candidates/test_coefficients.cpp b/tests/src/tests/candidates/test_coefficients.cpp index 4e8dd9125..f401eefe5 100644 --- a/tests/src/tests/candidates/test_coefficients.cpp +++ b/tests/src/tests/candidates/test_coefficients.cpp @@ -145,7 +145,7 @@ TEST_CASE("Plane-vertex collision stencil coeffs.", "[pv][stencil][coeffs]") Eigen::MatrixXi E, F; - PlaneVertexNormalCollision pv(o, n, 0); + PlaneVertexNormalCollision pv(Eigen::Hyperplane(n, o), 0); VectorMax4d coeffs = pv.compute_coefficients(V, E, F); diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index 9fca19f64..b47b340bd 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -363,7 +363,7 @@ TEST_CASE("Plane-vertex collision normal", "[pv][normal]") Eigen::MatrixXi E, F; - PlaneVertexNormalCollision pv(o, n, 0); + PlaneVertexNormalCollision pv(Eigen::Hyperplane(n, o), 0); Eigen::Vector3d normal = pv.compute_normal(V, E, F); Eigen::MatrixXd jacobian = pv.compute_normal_jacobian(V, E, F); diff --git a/tests/src/tests/collisions/CMakeLists.txt b/tests/src/tests/collisions/CMakeLists.txt index 56b054e6f..8fa263aa4 100644 --- a/tests/src/tests/collisions/CMakeLists.txt +++ b/tests/src/tests/collisions/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES # Tests test_normal_collisions.cpp + test_plane_vertex_collisions.cpp # Benchmarks diff --git a/tests/src/tests/collisions/test_normal_collisions.cpp b/tests/src/tests/collisions/test_normal_collisions.cpp index 3e30daa81..5f62a924e 100644 --- a/tests/src/tests/collisions/test_normal_collisions.cpp +++ b/tests/src/tests/collisions/test_normal_collisions.cpp @@ -281,13 +281,13 @@ TEST_CASE("Plane-Vertex NormalCollision", "[collision][plane-vertex]") { Eigen::MatrixXi edges, faces; const Eigen::Vector3d n(0, 1, 0), o(0, 0, 0); - const PlaneVertexNormalCollision c(o, n, 0); + const PlaneVertexNormalCollision c(Eigen::Hyperplane(n, o), 0); CHECK(c.num_vertices() == 1); CHECK( c.vertex_ids(edges, faces) == std::array { { 0, -1, -1, -1 } }); - CHECK(c.plane_origin == o); - CHECK(c.plane_normal == n); + CHECK(-c.plane.offset() * c.plane.normal() == o); + CHECK(c.plane.normal() == n); CHECK(c.vertex_id == 0); CHECK(c.compute_distance(Eigen::Vector3d(0, -2, 0)) == 4.0); @@ -308,7 +308,7 @@ TEST_CASE("NormalCollisions::is_*", "[collisions]") collisions.ee_collisions.emplace_back(0, 1, 0.0); collisions.fv_collisions.emplace_back(0, 1); collisions.pv_collisions.emplace_back( - Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 1, 0), 0); + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 0); for (int i = 0; i < collisions.size(); i++) { CHECK(collisions.is_vertex_vertex(i) == (i == 0)); diff --git a/tests/src/tests/collisions/test_plane_vertex_collisions.cpp b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp new file mode 100644 index 000000000..3cd826b1d --- /dev/null +++ b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp @@ -0,0 +1,580 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace ipc; + +// ============================================================================= +// PlaneVertexCandidate tests +// ============================================================================= + +TEST_CASE("PlaneVertexCandidate", "[candidates][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Hyperplane plane(n, o); + + PlaneVertexCandidate pvc(plane, 0); + + CHECK(pvc.num_vertices() == 1); + + Eigen::MatrixXi edges, faces; + auto ids = pvc.vertex_ids(edges, faces); + CHECK(ids == std::array { { 0, -1, -1, -1 } }); + + SECTION("Distance computation") + { + Eigen::Vector3d point(1.0, 2.0, 3.0); + double dist = pvc.compute_distance(point); + CHECK(dist == Catch::Approx(4.0)); // squared distance = (2)^2 + + Eigen::VectorXd grad = pvc.compute_distance_gradient(point); + CHECK(grad.size() == 3); + CHECK(grad.isApprox(Eigen::Vector3d(0, 4, 0))); + + Eigen::MatrixXd hess = pvc.compute_distance_hessian(point); + CHECK(hess.isApprox(2 * n * n.transpose())); + } + + SECTION("Coefficients") + { + Eigen::Vector3d point(0.5, 1.0, 0.5); + VectorMax4d coeffs = pvc.compute_coefficients(point); + CHECK(coeffs.size() == 1); + CHECK(coeffs(0) == Catch::Approx(1.0)); + } + + SECTION("Normal (exercises unnormalized normal internally)") + { + Eigen::Vector3d point(0, 1, 0); + // compute_normal is public and calls compute_unnormalized_normal + VectorMax3d normal = pvc.compute_normal(point); + CHECK(normal.isApprox(n)); + } + + SECTION("CCD") + { + AdditiveCCD ccd; + ccd.conservative_rescaling = 1.0; // No rescaling for testing + + // Vertex starts above the plane and moves below + Eigen::Vector3d v_t0(0, 1, 0); + Eigen::Vector3d v_t1(0, -1, 0); + double toi; + bool hit = + pvc.ccd(v_t0, v_t1, toi, /*min_distance=*/0, /*tmax=*/1.0, ccd); + CHECK(hit); + CHECK(toi == Catch::Approx(0.5)); + + // Vertex starts and stays above the plane + Eigen::Vector3d v_t0_above(0, 2, 0); + Eigen::Vector3d v_t1_above(0, 1, 0); + bool no_hit = pvc.ccd( + v_t0_above, v_t1_above, toi, /*min_distance=*/0, /*tmax=*/1.0, ccd); + CHECK_FALSE(no_hit); + + // Vertex starts below and moves further below — already past plane + Eigen::Vector3d v_t0_below(0, -0.5, 0); + Eigen::Vector3d v_t1_below(0, -2, 0); + bool below_hit = pvc.ccd( + v_t0_below, v_t1_below, toi, /*min_distance=*/0, /*tmax=*/1.0, ccd); + CHECK_FALSE(below_hit); + } +} + +// ============================================================================= +// PlaneVertexNormalCollision constructor tests +// ============================================================================= + +TEST_CASE( + "PlaneVertexNormalCollision constructors", "[collision][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Hyperplane plane(n, o); + + SECTION("From PlaneVertexCandidate") + { + PlaneVertexCandidate candidate(plane, 5); + PlaneVertexNormalCollision collision(candidate); + CHECK(collision.vertex_id == 5); + CHECK(collision.plane.normal().isApprox(n)); + CHECK(collision.num_vertices() == 1); + } + + SECTION("From plane, vertex_id, weight, weight_gradient") + { + const double weight = 2.5; + Eigen::SparseVector weight_gradient(9); + weight_gradient.insert(3) = 1.0; + + PlaneVertexNormalCollision collision(plane, 3, weight, weight_gradient); + CHECK(collision.vertex_id == 3); + CHECK(collision.plane.normal().isApprox(n)); + CHECK(collision.weight == Catch::Approx(weight)); + CHECK(collision.weight_gradient.nonZeros() == 1); + } +} + +// ============================================================================= +// PlaneVertexTangentialCollision tests +// ============================================================================= + +TEST_CASE( + "PlaneVertexTangentialCollision", "[collision][tangential][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Hyperplane plane(n, o); + + PlaneVertexNormalCollision normal_collision( + plane, 0, 1.0, Eigen::SparseVector(9)); + + SECTION("Constructor from NormalCollision") + { + PlaneVertexTangentialCollision tc(normal_collision); + CHECK(tc.vertex_id == 0); + CHECK(tc.weight == Catch::Approx(1.0)); + } + + SECTION("Constructor with positions and normal_force") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + CHECK(tc.vertex_id == 0); + CHECK(tc.normal_force_magnitude == Catch::Approx(normal_force)); + } + + SECTION("Constructor with positions and NormalPotential") + { + Eigen::Vector3d pos(0, 0.5, 0); + BarrierPotential bp(1.0, 1.0); + PlaneVertexTangentialCollision tc(normal_collision, pos, bp); + CHECK(tc.vertex_id == 0); + } + + SECTION("Tangent basis") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + // Call through base class reference (public in TangentialCollision) + TangentialCollision& tc_base = tc; + auto basis = tc_base.compute_tangent_basis(pos); + // For a Y-normal plane, tangent basis should be in XZ plane + CHECK(basis.rows() == 3); + CHECK(basis.cols() == 2); + // The tangent basis vectors should be orthogonal to the normal + CHECK(std::abs(n.dot(basis.col(0))) < 1e-10); + CHECK(std::abs(n.dot(basis.col(1))) < 1e-10); + } + + SECTION("Tangent basis jacobian") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto jac = tc_base.compute_tangent_basis_jacobian(pos); + CHECK(jac.isZero()); + } + + SECTION("Closest point") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto cp = tc_base.compute_closest_point(pos); + CHECK(cp.size() == 0); + } + + SECTION("Closest point jacobian") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto jac = tc_base.compute_closest_point_jacobian(pos); + CHECK(jac.rows() == 0); + } + + SECTION("Relative velocity") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + Eigen::Vector3d velocity(1.0, 2.0, 3.0); + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto rv = tc_base.relative_velocity(velocity); + CHECK(rv.isApprox(velocity)); + } + + SECTION("Relative velocity matrix") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + VectorMax2d cp; // empty closest point for pv + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto mat = tc_base.relative_velocity_matrix(cp); + CHECK(mat.isApprox(MatrixMax::Identity(3, 3))); + } + + SECTION("Relative velocity matrix jacobian") + { + Eigen::Vector3d pos(0, 0.5, 0); + const double normal_force = 10.0; + VectorMax2d cp; // empty closest point for pv + PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); + TangentialCollision& tc_base = tc; + auto jac = tc_base.relative_velocity_matrix_jacobian(cp); + CHECK(jac.isZero()); + } +} + +// ============================================================================= +// Integration: Candidates::build with planes +// ============================================================================= + +TEST_CASE( + "Candidates build with ground plane", "[candidates][plane-vertex][build]") +{ + // 4 vertices: two above plane, two below + Eigen::MatrixXd vertices(4, 3); + vertices << 0, 0.5, 0, // above, close + 1, 2.0, 0, // above, far + -1, 0.1, 0, // above, very close + 0, -0.5, 0; // below + + CollisionMesh mesh(vertices); + mesh.planes.emplace_back(Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero()); + + const double inflation_radius = 1.0; + + SECTION("Static build") + { + Candidates candidates; + candidates.build(mesh, vertices, inflation_radius); + + // Vertices 0 (d=0.5), 2 (d=0.1), 3 (d=-0.5) are within + // inflation_radius of the plane + CHECK(candidates.pv_candidates.size() >= 2); + CHECK(!candidates.empty()); + + // Verify pv_candidates are included in size() + size_t expected_size = candidates.vv_candidates.size() + + candidates.ev_candidates.size() + candidates.ee_candidates.size() + + candidates.fv_candidates.size() + candidates.pv_candidates.size(); + CHECK(candidates.size() == expected_size); + + // Verify operator[] can reach pv_candidates + if (!candidates.pv_candidates.empty()) { + size_t pv_start = candidates.vv_candidates.size() + + candidates.ev_candidates.size() + + candidates.ee_candidates.size() + + candidates.fv_candidates.size(); + auto& pv_ref = candidates[pv_start]; + CHECK(&pv_ref == &candidates.pv_candidates[0]); + + // const version + const Candidates& const_cand = candidates; + auto& pv_const_ref = const_cand[pv_start]; + CHECK(&pv_const_ref == &candidates.pv_candidates[0]); + } + } + + SECTION("CCD build") + { + // Move vertices downward + Eigen::MatrixXd vertices_t1 = vertices; + vertices_t1.col(1).array() -= 1.0; + + Candidates candidates; + candidates.build(mesh, vertices, vertices_t1, inflation_radius); + + // All vertices moving toward or past the plane should be candidates + CHECK(candidates.pv_candidates.size() >= 2); + } + + SECTION("Clear resets pv_candidates") + { + Candidates candidates; + candidates.build(mesh, vertices, inflation_radius); + CHECK(!candidates.pv_candidates.empty()); + candidates.clear(); + CHECK(candidates.pv_candidates.empty()); + CHECK(candidates.empty()); + } +} + +// ============================================================================= +// Integration: NormalCollisions::build with planes +// ============================================================================= + +TEST_CASE( + "NormalCollisions build with ground plane", + "[collisions][normal][plane-vertex][build]") +{ + // Vertex sitting just above the ground plane + Eigen::MatrixXd vertices(2, 3); + vertices << 0, 0.05, 0, // close to ground + 5, 5, 5; // far away + + CollisionMesh mesh(vertices); + mesh.init_area_jacobians(); + mesh.planes.emplace_back(Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero()); + + const double dhat = 0.2; + + SECTION("Basic build") + { + NormalCollisions collisions; + collisions.build(mesh, vertices, dhat); + + // Vertex 0 is at distance 0.05 from the plane (< dhat) + CHECK(!collisions.pv_collisions.empty()); + CHECK(collisions.pv_collisions.size() >= 1); + CHECK(!collisions.empty()); + CHECK(collisions.size() >= 1); + + // Verify is_plane_vertex + size_t pv_idx = collisions.vv_collisions.size() + + collisions.ev_collisions.size() + collisions.ee_collisions.size() + + collisions.fv_collisions.size(); + CHECK(collisions.is_plane_vertex(pv_idx)); + CHECK(!collisions.is_vertex_vertex(pv_idx)); + CHECK(!collisions.is_edge_vertex(pv_idx)); + CHECK(!collisions.is_edge_edge(pv_idx)); + CHECK(!collisions.is_face_vertex(pv_idx)); + + // Verify operator[] reaches pv_collisions + auto& collision_ref = collisions[pv_idx]; + CHECK( + &collision_ref + == static_cast(&collisions.pv_collisions[0])); + + const NormalCollisions& const_collisions = collisions; + auto& const_ref = const_collisions[pv_idx]; + CHECK( + &const_ref + == static_cast( + &collisions.pv_collisions[0])); + } + + SECTION("With area weighting") + { + NormalCollisions collisions; + collisions.set_use_area_weighting(true); + collisions.build(mesh, vertices, dhat); + + if (!collisions.pv_collisions.empty()) { + CHECK(collisions.pv_collisions[0].weight > 0); + } + } + + SECTION("With shape derivatives") + { + NormalCollisions collisions; + collisions.set_use_area_weighting(true); + collisions.set_enable_shape_derivatives(true); + collisions.build(mesh, vertices, dhat); + + if (!collisions.pv_collisions.empty()) { + CHECK(collisions.pv_collisions[0].weight > 0); + // weight_gradient should be set + } + } + + SECTION("Clear resets pv_collisions") + { + NormalCollisions collisions; + collisions.build(mesh, vertices, dhat); + CHECK(!collisions.pv_collisions.empty()); + collisions.clear(); + CHECK(collisions.pv_collisions.empty()); + CHECK(collisions.empty()); + } + + SECTION("Barrier potential with plane-vertex") + { + NormalCollisions collisions; + collisions.build(mesh, vertices, dhat); + REQUIRE(!collisions.pv_collisions.empty()); + + BarrierPotential bp(dhat, 1.0); + double val = bp(collisions, mesh, vertices); + CHECK(val > 0.0); + } +} + +// ============================================================================= +// Integration: TangentialCollisions::build with planes +// ============================================================================= + +TEST_CASE( + "TangentialCollisions build with ground plane", + "[collisions][tangential][plane-vertex][build]") +{ + Eigen::MatrixXd vertices(2, 3); + vertices << 0, 0.05, 0, // close to ground + 5, 5, 5; // far away + + CollisionMesh mesh(vertices); + mesh.init_area_jacobians(); + mesh.planes.emplace_back(Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero()); + + const double dhat = 0.2; + const double mu = 0.5; + + NormalCollisions normal_collisions; + normal_collisions.build(mesh, vertices, dhat); + REQUIRE(!normal_collisions.pv_collisions.empty()); + + BarrierPotential bp(dhat, 1.0); + + SECTION("Basic build") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + + CHECK(!tangential_collisions.pv_collisions.empty()); + CHECK(tangential_collisions.pv_collisions.size() >= 1); + CHECK(!tangential_collisions.empty()); + CHECK(tangential_collisions.size() >= 1); + + // Verify friction coefficient is set + CHECK(tangential_collisions.pv_collisions[0].mu_s == Catch::Approx(mu)); + CHECK(tangential_collisions.pv_collisions[0].mu_k == Catch::Approx(mu)); + } + + SECTION("Per-vertex friction") + { + Eigen::VectorXd mu_s = Eigen::VectorXd::Constant(vertices.rows(), 0.3); + Eigen::VectorXd mu_k = Eigen::VectorXd::Constant(vertices.rows(), 0.2); + mu_s(0) = 0.8; // vertex 0 has different friction + mu_k(0) = 0.6; + + TangentialCollisions tangential_collisions; + tangential_collisions.build( + mesh, vertices, normal_collisions, bp, mu_s, mu_k); + + CHECK(!tangential_collisions.pv_collisions.empty()); + // For pv collision, mu_s/mu_k should come from just the vertex + CHECK( + tangential_collisions.pv_collisions[0].mu_s == Catch::Approx(0.8)); + CHECK( + tangential_collisions.pv_collisions[0].mu_k == Catch::Approx(0.6)); + } + + SECTION("operator[] reaches pv_collisions") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + + size_t pv_idx = tangential_collisions.vv_collisions.size() + + tangential_collisions.ev_collisions.size() + + tangential_collisions.ee_collisions.size() + + tangential_collisions.fv_collisions.size(); + + CHECK( + &tangential_collisions[pv_idx] + == static_cast( + &tangential_collisions.pv_collisions[0])); + + const TangentialCollisions& const_tc = tangential_collisions; + CHECK( + &const_tc[pv_idx] + == static_cast( + &tangential_collisions.pv_collisions[0])); + } + + SECTION("Clear resets pv_collisions") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + CHECK(!tangential_collisions.pv_collisions.empty()); + tangential_collisions.clear(); + CHECK(tangential_collisions.pv_collisions.empty()); + CHECK(tangential_collisions.empty()); + } + + SECTION("Friction potential with plane-vertex") + { + TangentialCollisions tangential_collisions; + tangential_collisions.build(mesh, vertices, normal_collisions, bp, mu); + REQUIRE(!tangential_collisions.pv_collisions.empty()); + + Eigen::MatrixXd V1 = vertices; + V1(0, 0) += 0.01; // Small lateral displacement + Eigen::MatrixXd U = V1 - vertices; + + FrictionPotential fp(1e-3); + double val = fp(tangential_collisions, mesh, U); + CHECK(val >= 0.0); + } +} + +// ============================================================================= +// Out-of-range tests for operator[] +// ============================================================================= + +TEST_CASE( + "NormalCollisions operator[] out of range with pv", + "[collisions][normal][plane-vertex]") +{ + NormalCollisions collisions; + collisions.pv_collisions.emplace_back( + Eigen::Hyperplane(Eigen::Vector3d::UnitY(), 0), 0); + + CHECK(collisions.size() == 1); + CHECK( + &collisions[0] + == static_cast(&collisions.pv_collisions[0])); + + try { + collisions[1]; + FAIL("Should have thrown an exception"); + } catch (const std::out_of_range& e) { + SUCCEED("Exception thrown"); + } +} + +TEST_CASE( + "TangentialCollisions operator[] out of range with pv", + "[collisions][tangential][plane-vertex]") +{ + const Eigen::Vector3d n = Eigen::Vector3d::UnitY(); + const Eigen::Hyperplane plane(n, Eigen::Vector3d::Zero()); + + PlaneVertexNormalCollision nc( + plane, 0, 1.0, Eigen::SparseVector(9)); + TangentialCollisions tangential_collisions; + tangential_collisions.pv_collisions.emplace_back(nc); + + CHECK(tangential_collisions.size() == 1); + CHECK(!tangential_collisions.empty()); + + CHECK( + &tangential_collisions[0] + == static_cast( + &tangential_collisions.pv_collisions[0])); + + try { + tangential_collisions[1]; + FAIL("Should have thrown an exception"); + } catch (const std::out_of_range& e) { + SUCCEED("Exception thrown"); + } +} diff --git a/tests/src/tests/distance/test_edge_edge.cpp b/tests/src/tests/distance/test_edge_edge.cpp index 9c2d5c7a6..9b75d41e1 100644 --- a/tests/src/tests/distance/test_edge_edge.cpp +++ b/tests/src/tests/distance/test_edge_edge.cpp @@ -22,7 +22,7 @@ double edge_edge_distance_stacked(const Eigen::VectorXd& x) assert(x.size() == 12); return edge_edge_distance( x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); -}; +} } // namespace TEST_CASE("Edge-edge distance", "[distance][edge-edge]") diff --git a/tests/src/tests/distance/test_point_plane.cpp b/tests/src/tests/distance/test_point_plane.cpp index 6bf1691c8..29959df40 100644 --- a/tests/src/tests/distance/test_point_plane.cpp +++ b/tests/src/tests/distance/test_point_plane.cpp @@ -15,7 +15,7 @@ double point_plane_distance_stacked(const Eigen::VectorXd& x) assert(x.size() == 12); return point_plane_distance( x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); -}; +} } // namespace TEST_CASE( From fd12ceb5272e767b3469cba2468d54389429ac1c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 15:02:02 -0500 Subject: [PATCH 10/16] Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/ipc/utils/local_to_global.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ipc/utils/local_to_global.hpp b/src/ipc/utils/local_to_global.hpp index b6710e8ec..de3cc16b1 100644 --- a/src/ipc/utils/local_to_global.hpp +++ b/src/ipc/utils/local_to_global.hpp @@ -76,7 +76,7 @@ void local_gradient_to_global_gradient( /// can be efficiently assembled into a sparse matrix later. /// /// @tparam IDContainer A container type that supports operator[] and size() to access vertex ids. Can be a std::vector, Eigen::VectorXi, etc. -/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @tparam GlobalOrder The storage order of the global Hessian. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global Hessian is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global Hessian is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. /// @param local_hessian The local Hessian to be added to the global Hessian. Must be of size (n_verts * dim) x (n_verts * dim), where n_verts is the number of vertices in the local element and dim is the dimension of the problem. /// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). @@ -121,7 +121,7 @@ void local_hessian_to_global_triplets( /// @tparam Derived The derived type of the local Jacobian matrix. Must be a matrix expression that can be evaluated to an Eigen::MatrixXd. The local Jacobian must be of size (n_rows * dim) x (n_cols * dim), where n_rows and n_cols are the number of vertices in the local element for the row and column respectively, and dim is the dimension of the problem. /// @tparam IDContainer1 A container type that supports operator[] and size() to access vertex ids for the row indices. Can be a std::vector, Eigen::VectorXi, etc. Must be of size at least n_rows, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @tparam IDContainer2 A container type that supports operator[] and size() to access vertex ids for the column indices. Can be a std::vector, Eigen::VectorXi, etc. Must be of size at least n_cols, but can be larger (e.g., if the local element has fewer vertices than the number of ids). -/// @tparam GlobalOrder The storage order of the global gradient. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global gradient is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global gradient is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. +/// @tparam GlobalOrder The storage order of the global Jacobian. Must be either Eigen::RowMajor or Eigen::ColMajor. If RowMajor, the global Jacobian is assumed to be ordered as [x0, y0, z0, x1, y1, z1, ...]. If ColMajor, the global Jacobian is assumed to be ordered as [x0, x1, ..., y0, y1, ..., z0, z1, ...]. /// @param local_jacobian The local Jacobian to be added to the global Jacobian. Must be of size (n_rows * dim) x (n_cols * dim), where n_rows and n_cols are the number of vertices in the local element for the row and column respectively, and dim is the dimension of the problem. /// @param row_ids The vertex ids corresponding to the row indices of the local Jacobian. Must be of size at least n_rows, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param col_ids The vertex ids corresponding to the column indices of the local Jacobian. Must be of size at least n_cols, but can be larger (e.g., if the local element has fewer vertices than the number of ids). From 5ebe9e8b09ae992f44ad4912c80b63c612adf318 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 15:24:59 -0500 Subject: [PATCH 11/16] Handle ColMajor global ordering in local_to_global Use total global vertex/row/col counts when computing column-major indices and validate buffer sizes. Add optional n_total_verts, n_total_rows and n_total_cols parameters (default -1) that are required for ColMajor storage and throw an error if omitted. Include logger header, assert grad size divisibility, and update MatrixCache/triplet indexing accordingly. Add test_local_to_global.cpp to tests CMakeLists. --- src/ipc/utils/local_to_global.hpp | 56 +- tests/src/tests/utils/CMakeLists.txt | 1 + .../src/tests/utils/test_local_to_global.cpp | 594 ++++++++++++++++++ 3 files changed, 640 insertions(+), 11 deletions(-) create mode 100644 tests/src/tests/utils/test_local_to_global.cpp diff --git a/src/ipc/utils/local_to_global.hpp b/src/ipc/utils/local_to_global.hpp index de3cc16b1..ffd0f4360 100644 --- a/src/ipc/utils/local_to_global.hpp +++ b/src/ipc/utils/local_to_global.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -16,7 +17,7 @@ namespace ipc { /// @param local_grad The local gradient to be added to the global gradient. Must be of size n_verts * dim, where n_verts is the number of vertices in the local element and dim is the dimension of the problem. /// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). -/// @param grad The global gradient to which the local gradient will be added. Must be of size at least max(ids) * dim, but can be larger. +/// @param grad The global gradient to which the local gradient will be added. template void local_gradient_to_global_gradient( Eigen::ConstRef local_grad, @@ -29,12 +30,14 @@ void local_gradient_to_global_gradient( assert(local_grad.size() % dim == 0); const int n_verts = local_grad.size() / dim; assert(ids.size() >= n_verts); // Can be extra ids + 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++) { if constexpr (GlobalOrder == Eigen::RowMajor) { grad.segment(dim * ids[i], dim) += local_grad.segment(dim * i, dim); } else { for (int d = 0; d < dim; d++) { - grad[n_verts * d + ids[i]] += local_grad[dim * i + d]; + grad[n_total_verts * d + ids[i]] += local_grad[dim * i + d]; } } } @@ -46,7 +49,7 @@ void local_gradient_to_global_gradient( /// @param local_grad The local gradient to be added to the global gradient. Must be of size n_verts * dim, where n_verts is the number of vertices in the local element and dim is the dimension of the problem. /// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). -/// @param grad The global gradient to which the local gradient will be added. Must be of size at least max(ids) * dim, but can be larger. +/// @param grad The global gradient to which the local gradient will be added. template void local_gradient_to_global_gradient( Eigen::ConstRef local_grad, @@ -59,12 +62,15 @@ void local_gradient_to_global_gradient( assert(local_grad.size() % dim == 0); const int n_verts = local_grad.size() / dim; assert(ids.size() >= n_verts); // Can be extra ids + 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++) { for (int d = 0; d < dim; d++) { if constexpr (GlobalOrder == Eigen::RowMajor) { grad.coeffRef(dim * ids[i] + d) += local_grad(dim * i + d); } else { - grad.coeffRef(n_verts * d + ids[i]) += local_grad(dim * i + d); + grad.coeffRef(n_total_verts * d + ids[i]) += + local_grad(dim * i + d); } } } @@ -81,12 +87,14 @@ void local_gradient_to_global_gradient( /// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). /// @param triplets The vector of triplets to which the local Hessian will be added. +/// @param n_total_verts The total number of vertices in the global mesh. Only required when GlobalOrder is ColMajor. Ignored when GlobalOrder is RowMajor. template void local_hessian_to_global_triplets( Eigen::ConstRef local_hessian, const IDContainer& ids, const int dim, - std::vector>& triplets) + std::vector>& triplets, + const int n_total_verts = -1) { static_assert( GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); @@ -94,6 +102,12 @@ void local_hessian_to_global_triplets( assert(local_hessian.rows() % dim == 0); const int n_verts = local_hessian.rows() / dim; assert(ids.size() >= n_verts); // Can be extra ids + if constexpr (GlobalOrder == Eigen::ColMajor) { + if (n_total_verts <= 0) { + log_and_throw_error( + "Total number of vertices must be provided for ColMajor storage order."); + } + } for (int i = 0; i < n_verts; i++) { for (int j = 0; j < n_verts; j++) { for (int k = 0; k < dim; k++) { @@ -104,7 +118,8 @@ void local_hessian_to_global_triplets( local_hessian(dim * i + k, dim * j + l)); } else { triplets.emplace_back( - n_verts * k + ids[i], n_verts * l + ids[j], + n_total_verts * k + ids[i], + n_total_verts * l + ids[j], local_hessian(dim * i + k, dim * j + l)); } } @@ -127,6 +142,8 @@ void local_hessian_to_global_triplets( /// @param col_ids The vertex ids corresponding to the column indices of the local Jacobian. Must be of size at least n_cols, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). /// @param triplets The vector of triplets to which the local Hessian will be added. +/// @param n_total_rows The total number of row vertices in the global mesh. Only required when GlobalOrder is ColMajor. Ignored when GlobalOrder is RowMajor. +/// @param n_total_cols The total number of column vertices in the global mesh. Only required when GlobalOrder is ColMajor. Ignored when GlobalOrder is RowMajor. template < typename Derived, typename IDContainer1, @@ -137,7 +154,9 @@ void local_jacobian_to_global_triplets( const IDContainer1& row_ids, const IDContainer2& col_ids, int dim, - std::vector>& triplets) + std::vector>& triplets, + const int n_total_rows = -1, + const int n_total_cols = -1) { static_assert( GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); @@ -148,6 +167,12 @@ void local_jacobian_to_global_triplets( const int n_cols = local_jacobian.cols() / dim; assert(row_ids.size() >= n_rows); // Can be extra ids assert(col_ids.size() >= n_cols); // Can be extra ids + if constexpr (GlobalOrder == Eigen::ColMajor) { + if (n_total_rows <= 0 || n_total_cols <= 0) { + log_and_throw_error( + "Total number of rows and columns must be provided for ColMajor storage order."); + } + } for (int i = 0; i < n_rows; i++) { for (int j = 0; j < n_cols; j++) { for (int k = 0; k < dim; k++) { @@ -158,7 +183,8 @@ void local_jacobian_to_global_triplets( local_jacobian(dim * i + k, dim * j + l)); } else { triplets.emplace_back( - n_rows * k + row_ids[i], n_cols * l + col_ids[j], + n_total_rows * k + row_ids[i], + n_total_cols * l + col_ids[j], local_jacobian(dim * i + k, dim * j + l)); } } @@ -227,6 +253,7 @@ struct LocalThreadMatStorage { /// @param ids The vertex ids corresponding to the local gradient. Must be of size at least n_verts, but can be larger (e.g., if the local element has fewer vertices than the number of ids). /// @param dim The dimension of the problem (e.g., 2 for 2D, 3 for 3D). /// @param triplets The MatrixCache to which the local Hessian will be added. The triplets will be added to the first matrix in the cache (i.e., matrix index 0). The cache should be initialized with the correct number of rows and columns for the global Hessian, and should have enough reserved space for the number of triplets that will be added. +/// @param n_total_verts The total number of vertices in the global mesh. Only required when GlobalOrder is ColMajor. Ignored when GlobalOrder is RowMajor. template < typename Derived, typename IDContainer, @@ -235,7 +262,8 @@ void local_hessian_to_global_triplets( const Eigen::MatrixBase& local_hessian, const IDContainer& ids, int dim, - MatrixCache& triplets) + MatrixCache& triplets, + const int n_total_verts = -1) { static_assert( GlobalOrder == Eigen::ColMajor || GlobalOrder == Eigen::RowMajor); @@ -243,6 +271,12 @@ void local_hessian_to_global_triplets( assert(local_hessian.rows() % dim == 0); const int n_verts = local_hessian.rows() / dim; assert(ids.size() >= n_verts); // Can be extra ids + if constexpr (GlobalOrder == Eigen::ColMajor) { + if (n_total_verts <= 0) { + log_and_throw_error( + "Total number of vertices must be provided for ColMajor storage order."); + } + } for (int i = 0; i < n_verts; i++) { for (int j = 0; j < n_verts; j++) { for (int k = 0; k < dim; k++) { @@ -254,8 +288,8 @@ void local_hessian_to_global_triplets( 0, dim * ids[i] + k, dim * ids[j] + l, val); } else { triplets.add_value( - 0, n_verts * k + ids[i], n_verts * l + ids[j], - val); + 0, n_total_verts * k + ids[i], + n_total_verts * l + ids[j], val); } } } diff --git a/tests/src/tests/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index 2a547cbd7..e04072a47 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_utils.cpp test_matrixcache.cpp + test_local_to_global.cpp # Benchmarks diff --git a/tests/src/tests/utils/test_local_to_global.cpp b/tests/src/tests/utils/test_local_to_global.cpp new file mode 100644 index 000000000..597e5ca7c --- /dev/null +++ b/tests/src/tests/utils/test_local_to_global.cpp @@ -0,0 +1,594 @@ +#include + +#include +#include +#include + +#include + +using namespace ipc; + +// ─── helpers ──────────────────────────────────────────────────────────────── + +/// Convert a RowMajor-ordered dense vector to ColMajor ordering. +/// RowMajor: [x0 y0 z0 x1 y1 z1 …] +/// ColMajor: [x0 x1 … y0 y1 … z0 z1 …] +static Eigen::VectorXd +row_to_col(const Eigen::VectorXd& v, int n_verts, int dim) +{ + return v.reshaped(n_verts, dim) + .reshaped(); +} + +/// Convert a RowMajor-ordered sparse matrix to ColMajor DOF ordering. +/// RowMajor rows/cols indexed as (dim*v + d) +/// ColMajor rows/cols indexed as (n_verts*d + v) +static Eigen::MatrixXd +row_to_col_mat(const Eigen::MatrixXd& M, int n_verts, int dim) +{ + const int N = n_verts * dim; + assert(M.rows() == N && M.cols() == N); + Eigen::MatrixXd out = Eigen::MatrixXd::Zero(N, N); + for (int vi = 0; vi < n_verts; ++vi) { + for (int di = 0; di < dim; ++di) { + for (int vj = 0; vj < n_verts; ++vj) { + for (int dj = 0; dj < dim; ++dj) { + out(n_verts * di + vi, n_verts * dj + vj) = + M(dim * vi + di, dim * vj + dj); + } + } + } + } + return out; +} + +/// Same as row_to_col_mat but for a non-square Jacobian where row and column +/// vertex counts may differ. +static Eigen::MatrixXd row_to_col_jac( + const Eigen::MatrixXd& M, int n_row_verts, int n_col_verts, int dim) +{ + const int Nr = n_row_verts * dim; + const int Nc = n_col_verts * dim; + assert(M.rows() == Nr && M.cols() == Nc); + Eigen::MatrixXd out = Eigen::MatrixXd::Zero(Nr, Nc); + for (int vi = 0; vi < n_row_verts; ++vi) { + for (int di = 0; di < dim; ++di) { + for (int vj = 0; vj < n_col_verts; ++vj) { + for (int dj = 0; dj < dim; ++dj) { + out(n_row_verts * di + vi, n_col_verts * dj + vj) = + M(dim * vi + di, dim * vj + dj); + } + } + } + } + return out; +} + +/// Assemble a dense matrix from a vector of triplets. +static Eigen::MatrixXd triplets_to_dense( + const std::vector>& trips, int rows, int cols) +{ + Eigen::SparseMatrix sp(rows, cols); + sp.setFromTriplets(trips.begin(), trips.end()); + return Eigen::MatrixXd(sp); +} + +// ─── Test parameters ──────────────────────────────────────────────────────── + +// Global mesh: 5 vertices in 3D → 15 DOFs. +// Local element touches vertices {1, 3} (2 verts, 6 local DOFs). +static constexpr int DIM = 3; +static constexpr int N_TOTAL_VERTS = 5; +static constexpr int N_DOF = N_TOTAL_VERTS * DIM; + +static const std::vector ELEM_IDS = { { 1, 3 } }; +static constexpr int N_LOCAL_VERTS = 2; +static constexpr int N_LOCAL_DOF = N_LOCAL_VERTS * DIM; + +/// Deterministic local gradient (6 entries). +static Eigen::VectorXd make_local_grad() +{ + Eigen::VectorXd g(N_LOCAL_DOF); + // RowMajor local ordering: [x1 y1 z1 x3 y3 z3] + g << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + return g; +} + +/// Deterministic local hessian (6×6). +static Eigen::MatrixXd make_local_hessian() +{ + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(N_LOCAL_DOF, N_LOCAL_DOF); + for (int i = 0; i < N_LOCAL_DOF; ++i) { + for (int j = 0; j < N_LOCAL_DOF; ++j) { + H(i, j) = 10.0 * (i + 1) + (j + 1); // nonzero everywhere + } + } + return H; +} + +// ═══════════════════════════════════════════════════════════════════════════ +// 1. local_gradient_to_global_gradient – dense Eigen::VectorXd +// ═══════════════════════════════════════════════════════════════════════════ + +TEST_CASE( + "local_gradient_to_global_gradient dense RowMajor vs ColMajor", + "[utils][local_to_global][gradient][dense]") +{ + const Eigen::VectorXd local_grad = make_local_grad(); + + // --- RowMajor assembly --- + Eigen::VectorXd grad_row = Eigen::VectorXd::Zero(N_DOF); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_grad, ELEM_IDS, DIM, grad_row); + + // Verify a few values manually. + // Vertex 1, dim 0 → index 3*1+0 = 3 + CHECK(grad_row[3] == 1.0); + // Vertex 1, dim 1 → index 3*1+1 = 4 + CHECK(grad_row[4] == 2.0); + // Vertex 3, dim 2 → index 3*3+2 = 11 + CHECK(grad_row[11] == 6.0); + + // --- ColMajor assembly --- + Eigen::VectorXd grad_col = Eigen::VectorXd::Zero(N_DOF); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_grad, ELEM_IDS, DIM, grad_col); + + // ColMajor: vertex v, dim d → index N_TOTAL_VERTS*d + v + // Vertex 1, dim 0 → index 5*0+1 = 1 + CHECK(grad_col[1] == 1.0); + // Vertex 1, dim 1 → index 5*1+1 = 6 + CHECK(grad_col[6] == 2.0); + // Vertex 3, dim 2 → index 5*2+3 = 13 + CHECK(grad_col[13] == 6.0); + + // Structural check: both vectors should be the permutation of each other. + Eigen::VectorXd expected_col = row_to_col(grad_row, N_TOTAL_VERTS, DIM); + CHECK((grad_col.array() == expected_col.array()).all()); +} + +TEST_CASE( + "local_gradient_to_global_gradient dense accumulates from two elements", + "[utils][local_to_global][gradient][dense]") +{ + // Two elements that share vertex 3. + const std::vector ids_a = { 0, 3 }; + const std::vector ids_b = { 3, 4 }; + + Eigen::VectorXd local_a(N_LOCAL_DOF), local_b(N_LOCAL_DOF); + local_a << 1, 2, 3, 10, 20, 30; + local_b << 100, 200, 300, 7, 8, 9; + + Eigen::VectorXd grad_row = Eigen::VectorXd::Zero(N_DOF); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_a, ids_a, DIM, grad_row); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_b, ids_b, DIM, grad_row); + + Eigen::VectorXd grad_col = Eigen::VectorXd::Zero(N_DOF); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_a, ids_a, DIM, grad_col); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_b, ids_b, DIM, grad_col); + + // Vertex 3 receives contributions from both elements. + // RowMajor vertex 3 dim 0 → index 9: 10 + 100 = 110 + CHECK(grad_row[9] == 110.0); + // ColMajor vertex 3 dim 0 → index 5*0+3 = 3: should also be 110 + CHECK(grad_col[3] == 110.0); + + Eigen::VectorXd expected_col = row_to_col(grad_row, N_TOTAL_VERTS, DIM); + CHECK((grad_col.array() == expected_col.array()).all()); +} + +// ═══════════════════════════════════════════════════════════════════════════ +// 2. local_gradient_to_global_gradient – sparse Eigen::SparseVector +// ═══════════════════════════════════════════════════════════════════════════ + +TEST_CASE( + "local_gradient_to_global_gradient sparse RowMajor vs ColMajor", + "[utils][local_to_global][gradient][sparse]") +{ + const Eigen::VectorXd local_grad = make_local_grad(); + + Eigen::SparseVector grad_row(N_DOF); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_grad, ELEM_IDS, DIM, grad_row); + + Eigen::SparseVector grad_col(N_DOF); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_grad, ELEM_IDS, DIM, grad_col); + + // Convert both to dense for easy comparison. + Eigen::VectorXd dr = Eigen::VectorXd(grad_row); + Eigen::VectorXd dc = Eigen::VectorXd(grad_col); + + Eigen::VectorXd expected_col = row_to_col(dr, N_TOTAL_VERTS, DIM); + CHECK((dc.array() == expected_col.array()).all()); + + // Spot-check: only 2*DIM entries should be nonzero. + CHECK(grad_row.nonZeros() == N_LOCAL_DOF); + CHECK(grad_col.nonZeros() == N_LOCAL_DOF); +} + +// ═══════════════════════════════════════════════════════════════════════════ +// 3. local_hessian_to_global_triplets – std::vector +// ═══════════════════════════════════════════════════════════════════════════ + +TEST_CASE( + "local_hessian_to_global_triplets (triplet vector) RowMajor vs ColMajor", + "[utils][local_to_global][hessian][triplet]") +{ + const Eigen::MatrixXd local_hess = make_local_hessian(); + + // --- RowMajor --- + std::vector> trips_row; + local_hessian_to_global_triplets, Eigen::RowMajor>( + local_hess, ELEM_IDS, DIM, trips_row); + + // --- ColMajor --- + std::vector> trips_col; + local_hessian_to_global_triplets, Eigen::ColMajor>( + local_hess, ELEM_IDS, DIM, trips_col, N_TOTAL_VERTS); + + // Both should produce N_LOCAL_DOF^2 triplets. + CHECK(trips_row.size() == N_LOCAL_DOF * N_LOCAL_DOF); + CHECK(trips_col.size() == N_LOCAL_DOF * N_LOCAL_DOF); + + Eigen::MatrixXd H_row = triplets_to_dense(trips_row, N_DOF, N_DOF); + Eigen::MatrixXd H_col = triplets_to_dense(trips_col, N_DOF, N_DOF); + + Eigen::MatrixXd expected_col = row_to_col_mat(H_row, N_TOTAL_VERTS, DIM); + CHECK((H_col.array() == expected_col.array()).all()); +} + +TEST_CASE( + "local_hessian_to_global_triplets (triplet vector) ColMajor spot-check", + "[utils][local_to_global][hessian][triplet]") +{ + // 2 verts, dim=2, ids={0,2}, n_total=4 + const int dim = 2; + const int n_total = 4; + const std::vector ids = { 0, 2 }; + + // 4×4 local hessian + Eigen::MatrixXd H(4, 4); + H << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16; + + std::vector> trips; + local_hessian_to_global_triplets, Eigen::ColMajor>( + H, ids, dim, trips, n_total); + + Eigen::MatrixXd M = triplets_to_dense(trips, n_total * dim, n_total * dim); + + // RowMajor local (i=0,d=0) → global ColMajor row = n_total*0 + ids[0] = 0 + // RowMajor local (i=0,d=1) → global ColMajor row = n_total*1 + ids[0] = 4 + // RowMajor local (i=1,d=0) → global ColMajor row = n_total*0 + ids[1] = 2 + // RowMajor local (i=1,d=1) → global ColMajor row = n_total*1 + ids[1] = 6 + + // H(0,0) = local(dim*0+0, dim*0+0) = H(0,0)=1, row=0 col=0 + CHECK(M(0, 0) == 1.0); + // H(0,1) = local(0,1) = 2, row=0, col=n_total*1+0=4 + CHECK(M(0, 4) == 2.0); + // H(1,0) = local(1,0) = 5, row=n_total*1+0=4, col=0 + CHECK(M(4, 0) == 5.0); + // H(2,3) = local(dim*1+0, dim*1+1) = H(2,3) = 12 + // row = n_total*0+ids[1] = 2, col = n_total*1+ids[1] = 6 + CHECK(M(2, 6) == 12.0); + // H(3,2) = local(dim*1+1, dim*1+0) = H(3,2) = 15 + // row = n_total*1+ids[1] = 6, col = n_total*0+ids[1] = 2 + CHECK(M(6, 2) == 15.0); +} + +// ═══════════════════════════════════════════════════════════════════════════ +// 4. local_hessian_to_global_triplets – MatrixCache +// ═══════════════════════════════════════════════════════════════════════════ + +TEST_CASE( + "local_hessian_to_global_triplets (MatrixCache) RowMajor vs ColMajor", + "[utils][local_to_global][hessian][cache]") +{ + const Eigen::MatrixXd local_hess = make_local_hessian(); + + // --- RowMajor via MatrixCache --- + SparseMatrixCache cache_row(N_DOF, N_DOF); + cache_row.reserve(N_LOCAL_DOF * N_LOCAL_DOF); + local_hessian_to_global_triplets< + Eigen::MatrixXd, std::vector, Eigen::RowMajor>( + local_hess, ELEM_IDS, DIM, cache_row); + + Eigen::MatrixXd H_row = Eigen::MatrixXd(cache_row.get_matrix()); + + // --- ColMajor via MatrixCache --- + SparseMatrixCache cache_col(N_DOF, N_DOF); + cache_col.reserve(N_LOCAL_DOF * N_LOCAL_DOF); + local_hessian_to_global_triplets< + Eigen::MatrixXd, std::vector, Eigen::ColMajor>( + local_hess, ELEM_IDS, DIM, cache_col, N_TOTAL_VERTS); + + Eigen::MatrixXd H_col = Eigen::MatrixXd(cache_col.get_matrix()); + + Eigen::MatrixXd expected_col = row_to_col_mat(H_row, N_TOTAL_VERTS, DIM); + CHECK((H_col.array() == expected_col.array()).all()); +} + +TEST_CASE( + "local_hessian_to_global_triplets (MatrixCache) ColMajor with two elements", + "[utils][local_to_global][hessian][cache]") +{ + // Two elements that share vertex 2. + const int dim = 2; + const int n_total = 4; + const int n_dof = n_total * dim; + const std::vector ids_a = { 0, 2 }; + const std::vector ids_b = { 2, 3 }; + + Eigen::MatrixXd Ha = Eigen::MatrixXd::Ones(4, 4) * 2.0; + Eigen::MatrixXd Hb = Eigen::MatrixXd::Ones(4, 4) * 3.0; + + // RowMajor + SparseMatrixCache cr(n_dof, n_dof); + cr.reserve(32); + local_hessian_to_global_triplets< + Eigen::MatrixXd, std::vector, Eigen::RowMajor>( + Ha, ids_a, dim, cr); + local_hessian_to_global_triplets< + Eigen::MatrixXd, std::vector, Eigen::RowMajor>( + Hb, ids_b, dim, cr); + Eigen::MatrixXd M_row = Eigen::MatrixXd(cr.get_matrix()); + + // ColMajor + SparseMatrixCache cc(n_dof, n_dof); + cc.reserve(32); + local_hessian_to_global_triplets< + Eigen::MatrixXd, std::vector, Eigen::ColMajor>( + Ha, ids_a, dim, cc, n_total); + local_hessian_to_global_triplets< + Eigen::MatrixXd, std::vector, Eigen::ColMajor>( + Hb, ids_b, dim, cc, n_total); + Eigen::MatrixXd M_col = Eigen::MatrixXd(cc.get_matrix()); + + Eigen::MatrixXd expected = row_to_col_mat(M_row, n_total, dim); + CHECK((M_col.array() == expected.array()).all()); + + // The (2,2) block in RowMajor should accumulate 2+3=5 for the + // diagonal dim entries of vertex 2. + CHECK(M_row(dim * 2, dim * 2) == 5.0); + CHECK(M_col(n_total * 0 + 2, n_total * 0 + 2) == 5.0); +} + +// ═══════════════════════════════════════════════════════════════════════════ +// 5. local_jacobian_to_global_triplets +// ═══════════════════════════════════════════════════════════════════════════ + +TEST_CASE( + "local_jacobian_to_global_triplets RowMajor vs ColMajor square", + "[utils][local_to_global][jacobian]") +{ + // Square case (same as hessian but using the jacobian API). + const Eigen::MatrixXd local_jac = make_local_hessian(); + + std::vector> trips_row; + local_jacobian_to_global_triplets< + Eigen::MatrixXd, std::vector, std::vector, Eigen::RowMajor>( + local_jac, ELEM_IDS, ELEM_IDS, DIM, trips_row); + + std::vector> trips_col; + local_jacobian_to_global_triplets< + Eigen::MatrixXd, std::vector, std::vector, Eigen::ColMajor>( + local_jac, ELEM_IDS, ELEM_IDS, DIM, trips_col, N_TOTAL_VERTS, + N_TOTAL_VERTS); + + Eigen::MatrixXd J_row = triplets_to_dense(trips_row, N_DOF, N_DOF); + Eigen::MatrixXd J_col = triplets_to_dense(trips_col, N_DOF, N_DOF); + + Eigen::MatrixXd expected = row_to_col_mat(J_row, N_TOTAL_VERTS, DIM); + CHECK((J_col.array() == expected.array()).all()); +} + +TEST_CASE( + "local_jacobian_to_global_triplets ColMajor non-square", + "[utils][local_to_global][jacobian]") +{ + // Non-square: 2 row-verts (ids {0,3}), 3 col-verts (ids {1,2,4}), dim=2. + const int dim = 2; + const int n_row_total = 5; // global row vertices + const int n_col_total = 5; // global col vertices + const std::vector row_ids = { 0, 3 }; + const std::vector col_ids = { 1, 2, 4 }; + const int n_row_local = 2; + const int n_col_local = 3; + const int lr = n_row_local * dim; // 4 + const int lc = n_col_local * dim; // 6 + + Eigen::MatrixXd J(lr, lc); + for (int i = 0; i < lr; ++i) + for (int j = 0; j < lc; ++j) + J(i, j) = 100.0 * (i + 1) + (j + 1); + + // RowMajor + std::vector> trips_row; + local_jacobian_to_global_triplets< + Eigen::MatrixXd, std::vector, std::vector, Eigen::RowMajor>( + J, row_ids, col_ids, dim, trips_row); + + // ColMajor + std::vector> trips_col; + local_jacobian_to_global_triplets< + Eigen::MatrixXd, std::vector, std::vector, Eigen::ColMajor>( + J, row_ids, col_ids, dim, trips_col, n_row_total, n_col_total); + + const int Nr = n_row_total * dim; + const int Nc = n_col_total * dim; + Eigen::MatrixXd J_row = triplets_to_dense(trips_row, Nr, Nc); + Eigen::MatrixXd J_col = triplets_to_dense(trips_col, Nr, Nc); + + Eigen::MatrixXd expected = + row_to_col_jac(J_row, n_row_total, n_col_total, dim); + CHECK((J_col.array() == expected.array()).all()); + + // Spot-check a single entry. + // J(0,0) = 101. RowMajor: row=dim*row_ids[0]+0=0, col=dim*col_ids[0]+0=2 + CHECK(J_row(0, 2) == 101.0); + // ColMajor: row=n_row_total*0+row_ids[0]=0, col=n_col_total*0+col_ids[0]=1 + CHECK(J_col(0, 1) == 101.0); +} + +// ═══════════════════════════════════════════════════════════════════════════ +// 6. Edge cases +// ═══════════════════════════════════════════════════════════════════════════ + +TEST_CASE("ColMajor gradient 2D", "[utils][local_to_global][gradient][2D]") +{ + // 2D problem: 4 vertices, dim=2. + const int dim = 2; + const int n_total = 4; + const int n_dof = n_total * dim; // 8 + const std::vector ids = { 1, 3 }; + Eigen::VectorXd local_grad(4); + local_grad << 10, 20, 30, 40; // [x1 y1 x3 y3] + + // RowMajor + Eigen::VectorXd gr = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_grad, ids, dim, gr); + // Expected RowMajor: index 2*1+0=2 →10, 2*1+1=3 →20, 2*3+0=6 →30, 2*3+1=7 + // →40 + CHECK(gr[2] == 10.0); + CHECK(gr[3] == 20.0); + CHECK(gr[6] == 30.0); + CHECK(gr[7] == 40.0); + + // ColMajor + Eigen::VectorXd gc = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_grad, ids, dim, gc); + // Expected ColMajor: n_total*0+1=1 →10, n_total*1+1=5 →20, + // n_total*0+3=3 →30, n_total*1+3=7 →40 + CHECK(gc[1] == 10.0); + CHECK(gc[5] == 20.0); + CHECK(gc[3] == 30.0); + CHECK(gc[7] == 40.0); + + Eigen::VectorXd expected = row_to_col(gr, n_total, dim); + CHECK((gc.array() == expected.array()).all()); +} + +TEST_CASE( + "ColMajor single vertex element", + "[utils][local_to_global][gradient][single]") +{ + // A single vertex local element. + const int dim = 3; + const int n_total = 3; + const int n_dof = n_total * dim; + const std::vector ids = { 2 }; + Eigen::VectorXd local_grad(3); + local_grad << 7, 8, 9; + + Eigen::VectorXd gr = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_grad, ids, dim, gr); + + Eigen::VectorXd gc = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_grad, ids, dim, gc); + + Eigen::VectorXd expected = row_to_col(gr, n_total, dim); + CHECK((gc.array() == expected.array()).all()); + + // RowMajor: vertex 2, dim 0 → 3*2+0=6 + CHECK(gr[6] == 7.0); + // ColMajor: vertex 2, dim 0 → 3*0+2=2 + CHECK(gc[2] == 7.0); +} + +TEST_CASE( + "ColMajor hessian identity-like local matrix", + "[utils][local_to_global][hessian][identity]") +{ + // Local identity hessian to verify diagonal placement. + const int dim = 2; + const int n_total = 4; + const int n_dof = n_total * dim; + const std::vector ids = { 0, 3 }; + + Eigen::MatrixXd H = Eigen::MatrixXd::Identity(4, 4); + + std::vector> trips_row; + local_hessian_to_global_triplets, Eigen::RowMajor>( + H, ids, dim, trips_row); + + std::vector> trips_col; + local_hessian_to_global_triplets, Eigen::ColMajor>( + H, ids, dim, trips_col, n_total); + + Eigen::MatrixXd M_row = triplets_to_dense(trips_row, n_dof, n_dof); + Eigen::MatrixXd M_col = triplets_to_dense(trips_col, n_dof, n_dof); + + // RowMajor diagonal entries at (0,0),(1,1),(6,6),(7,7). + CHECK(M_row(0, 0) == 1.0); + CHECK(M_row(1, 1) == 1.0); + CHECK(M_row(6, 6) == 1.0); + CHECK(M_row(7, 7) == 1.0); + + // ColMajor diagonal entries at (0,0),(4,4),(3,3),(7,7). + CHECK(M_col(0, 0) == 1.0); + CHECK(M_col(4, 4) == 1.0); + CHECK(M_col(3, 3) == 1.0); + CHECK(M_col(7, 7) == 1.0); + + Eigen::MatrixXd expected = row_to_col_mat(M_row, n_total, dim); + CHECK((M_col.array() == expected.array()).all()); +} + +TEST_CASE( + "ColMajor with extra ids in container", + "[utils][local_to_global][gradient][extra_ids]") +{ + // ids container is larger than needed – only first n_verts are used. + const int dim = 2; + const int n_total = 5; + const int n_dof = n_total * dim; + const std::vector ids = { 1, 4, 99 }; // 3 ids, but local only needs 2 + + Eigen::VectorXd local_grad(4); // 2 verts * 2 dim + local_grad << 5, 6, 7, 8; + + Eigen::VectorXd gr = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient, Eigen::RowMajor>( + local_grad, ids, dim, gr); + + Eigen::VectorXd gc = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient, Eigen::ColMajor>( + local_grad, ids, dim, gc); + + Eigen::VectorXd expected = row_to_col(gr, n_total, dim); + CHECK((gc.array() == expected.array()).all()); +} + +TEST_CASE( + "ColMajor with Eigen::VectorXi ids", + "[utils][local_to_global][gradient][eigen_ids]") +{ + // Ensure IDContainer works with Eigen types. + const int dim = 3; + const int n_total = 4; + const int n_dof = n_total * dim; + Eigen::VectorXi ids(2); + ids << 0, 3; + + Eigen::VectorXd local_grad(6); + local_grad << 1, 2, 3, 4, 5, 6; + + Eigen::VectorXd gr = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient( + local_grad, ids, dim, gr); + + Eigen::VectorXd gc = Eigen::VectorXd::Zero(n_dof); + local_gradient_to_global_gradient( + local_grad, ids, dim, gc); + + Eigen::VectorXd expected = row_to_col(gr, n_total, dim); + CHECK((gc.array() == expected.array()).all()); +} From b5fac5b282c395cd045207d6223cc46d5fc094d3 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 15:29:17 -0500 Subject: [PATCH 12/16] Add braces to nested loops in test --- tests/src/tests/utils/test_local_to_global.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/src/tests/utils/test_local_to_global.cpp b/tests/src/tests/utils/test_local_to_global.cpp index 597e5ca7c..fb9098d37 100644 --- a/tests/src/tests/utils/test_local_to_global.cpp +++ b/tests/src/tests/utils/test_local_to_global.cpp @@ -402,9 +402,11 @@ TEST_CASE( const int lc = n_col_local * dim; // 6 Eigen::MatrixXd J(lr, lc); - for (int i = 0; i < lr; ++i) - for (int j = 0; j < lc; ++j) + for (int i = 0; i < lr; ++i) { + for (int j = 0; j < lc; ++j) { J(i, j) = 100.0 * (i + 1) + (j + 1); + } + } // RowMajor std::vector> trips_row; From b6ae3acfa9a831a23fd2fa5ed58585dbcf275f59 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 18:02:42 -0500 Subject: [PATCH 13/16] Support ColMajor DOF layout in derivatives and tests Add tests/dof_layout.hpp with helpers to flatten, unflatten, and reorder gradients/hessians so finite-difference tests work for both VERTEX_DERIVATIVE_LAYOUT settings. Pass mesh.num_vertices() (n_total_verts) into shape_derivative and local_*_to_global_triplets so global DOF indices are computed correctly for column-major layouts. --- src/ipc/potentials/normal_potential.cpp | 19 +++- src/ipc/potentials/normal_potential.hpp | 4 +- src/ipc/potentials/potential.cpp | 3 +- src/ipc/potentials/tangential_potential.cpp | 9 +- .../smooth_contact_potential.cpp | 2 +- .../collisions/test_normal_collisions.cpp | 11 +- tests/src/tests/dof_layout.hpp | 101 ++++++++++++++++++ .../tests/friction/test_force_jacobian.cpp | 46 ++++---- tests/src/tests/friction/test_friction.cpp | 10 ++ .../potential/test_adhesion_potentials.cpp | 9 +- .../potential/test_barrier_potential.cpp | 40 +++---- .../potential/test_friction_potential.cpp | 8 +- .../tests/potential/test_smooth_potential.cpp | 38 ++++--- 13 files changed, 224 insertions(+), 76 deletions(-) create mode 100644 tests/src/tests/dof_layout.hpp diff --git a/src/ipc/potentials/normal_potential.cpp b/src/ipc/potentials/normal_potential.cpp index 4e0c6f46f..94eebdda1 100644 --- a/src/ipc/potentials/normal_potential.cpp +++ b/src/ipc/potentials/normal_potential.cpp @@ -38,7 +38,8 @@ Eigen::SparseMatrix NormalPotential::shape_derivative( this->shape_derivative( collisions[i], collisions[i].vertex_ids(edges, faces), collisions[i].dof(rest_positions, edges, faces), - collisions[i].dof(vertices, edges, faces), local_triplets); + collisions[i].dof(vertices, edges, faces), local_triplets, + mesh.num_vertices()); } }); @@ -150,7 +151,8 @@ void NormalPotential::shape_derivative( const std::array& vertex_ids, Eigen::ConstRef rest_positions, // = x̄ Eigen::ConstRef positions, // = x̄ + u - std::vector>& out) const + std::vector>& out, + const int n_total_verts) const { assert(rest_positions.size() == positions.size()); @@ -174,11 +176,17 @@ void NormalPotential::shape_derivative( for (int i = 0; i < collision.num_vertices(); i++) { for (int d = 0; d < dim; d++) { + int row_idx; + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + row_idx = vertex_ids[i] * dim + d; + } else { + assert(n_total_verts > 0); + row_idx = n_total_verts * d + vertex_ids[i]; + } using Itr = Eigen::SparseVector::InnerIterator; for (Itr j(collision.weight_gradient); j; ++j) { out.emplace_back( - vertex_ids[i] * dim + d, j.index(), - grad_f[dim * i + d] * j.value()); + row_idx, j.index(), grad_f[dim * i + d] * j.value()); } } } @@ -231,7 +239,8 @@ void NormalPotential::shape_derivative( + gradu_f * gradu_m.transpose() + m * hessu_f; } - local_hessian_to_global_triplets(local_hess, vertex_ids, dim, out); + local_hessian_to_global_triplets( + local_hess, vertex_ids, dim, out, n_total_verts); } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/potentials/normal_potential.hpp b/src/ipc/potentials/normal_potential.hpp index bae76a514..30bb4827f 100644 --- a/src/ipc/potentials/normal_potential.hpp +++ b/src/ipc/potentials/normal_potential.hpp @@ -67,12 +67,14 @@ class NormalPotential : public Potential { /// @param[in] rest_positions The collision stencil's rest positions. /// @param[in] positions The collision stencil's positions. /// @param[in,out] out Store the triplets of the shape derivative here. + /// @param[in] n_total_verts The total number of vertices in the mesh, used for computing global indices in the triplets. See also `local_hessian_to_global_triplets`. void shape_derivative( const NormalCollision& collision, const std::array& vertex_ids, Eigen::ConstRef rest_positions, Eigen::ConstRef positions, - std::vector>& out) const; + std::vector>& out, + const int n_total_verts = -1) const; /// @brief Compute the force magnitude for a collision. /// @param distance_squared The squared distance between elements. diff --git a/src/ipc/potentials/potential.cpp b/src/ipc/potentials/potential.cpp index c4da5d2b2..44f1fbe91 100644 --- a/src/ipc/potentials/potential.cpp +++ b/src/ipc/potentials/potential.cpp @@ -134,7 +134,8 @@ Eigen::SparseMatrix Potential::hessian( collision.vertex_ids(edges, faces); local_hessian_to_global_triplets( - local_hess, vids, dim, *(hess_triplets.cache)); + local_hess, vids, dim, *(hess_triplets.cache), + mesh.num_vertices()); } }); diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index f594db53b..7020a3ee7 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -96,7 +96,8 @@ Eigen::SparseMatrix TangentialPotential::force_jacobian( collision.vertex_ids(mesh.edges(), mesh.faces()); local_hessian_to_global_triplets( - local_force_jacobian, vis, dim, jac_triplets); + local_force_jacobian, vis, dim, jac_triplets, + mesh.num_vertices()); } }); @@ -585,7 +586,8 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( collision.vertex_ids(mesh.edges(), mesh.faces()); local_hessian_to_global_triplets( - local_force_jacobian, vis, dim, jac_triplets); + local_force_jacobian, vis, dim, jac_triplets, + mesh.num_vertices()); if (wrt == DiffWRT::VELOCITIES) { continue; @@ -612,7 +614,8 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( local_jacobian_to_global_triplets( local_force * normal_force_grad.transpose(), vis, - cc_vert_ids, dim, jac_triplets); + cc_vert_ids, dim, jac_triplets, mesh.num_vertices(), + mesh.num_vertices()); } }); diff --git a/src/ipc/smooth_contact/smooth_contact_potential.cpp b/src/ipc/smooth_contact/smooth_contact_potential.cpp index f3b34579d..e22b88b00 100644 --- a/src/ipc/smooth_contact/smooth_contact_potential.cpp +++ b/src/ipc/smooth_contact/smooth_contact_potential.cpp @@ -109,7 +109,7 @@ Eigen::SparseMatrix SmoothContactPotential::hessian( local_hessian_to_global_triplets( local_hess, collision.vertex_ids(), dim, - *(hess_triplets.cache)); + *(hess_triplets.cache), mesh.num_vertices()); } }); diff --git a/tests/src/tests/collisions/test_normal_collisions.cpp b/tests/src/tests/collisions/test_normal_collisions.cpp index 5f62a924e..c56dee4f9 100644 --- a/tests/src/tests/collisions/test_normal_collisions.cpp +++ b/tests/src/tests/collisions/test_normal_collisions.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -91,8 +92,14 @@ TEST_CASE("Codim. vertex-vertex collisions", "[collisions][codim]") CHECK(barrier_potential(collisions, mesh, vertices) > 0.0); const Eigen::VectorXd grad = barrier_potential.gradient(collisions, mesh, vertices); - for (int i = 0; i < vertices.rows(); i++) { - const Eigen::Vector3d f = -grad.segment<3>(3 * i); + const int n_verts = vertices.rows(); + for (int i = 0; i < n_verts; i++) { + Eigen::Vector3d f; + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + f = -grad.segment<3>(3 * i); + } else { + f << -grad[i], -grad[n_verts + i], -grad[2 * n_verts + i]; + } CHECK(f.normalized().isApprox( vertices.row(i).normalized().transpose())); } diff --git a/tests/src/tests/dof_layout.hpp b/tests/src/tests/dof_layout.hpp new file mode 100644 index 000000000..8dbb26fa1 --- /dev/null +++ b/tests/src/tests/dof_layout.hpp @@ -0,0 +1,101 @@ +/// @file +/// Utility helpers that make finite-difference tests work regardless of +/// whether ipc::VERTEX_DERIVATIVE_LAYOUT is RowMajor or ColMajor. +/// +/// fd::flatten / fd::unflatten always use **RowMajor** ordering: +/// flatten : MatrixXd(n_verts, dim) → [x0 y0 z0 x1 y1 z1 …] +/// unflatten : [x0 y0 z0 x1 y1 z1 …] → MatrixXd(n_verts, dim) +/// +/// When VERTEX_DERIVATIVE_LAYOUT == ColMajor the library returns gradient / +/// hessian values laid out as +/// [x0 x1 … y0 y1 … z0 z1 …] (columns-of-vertices first) +/// +/// The helpers below bridge the two worlds so that every test can be written as +/// +/// fd::finite_gradient(dof_flatten(V), [&](auto& x){ … dof_unflatten(x, V) … +/// }, fgrad); +/// fgrad = dof_reorder_grad(fgrad, n_verts, dim); // RowMajor → layout +/// +/// and the comparison with the library result is valid. + +#pragma once + +#include + +#include +#include + +#include + +namespace ipc::tests { + +// ─── Flatten / Unflatten in VERTEX_DERIVATIVE_LAYOUT order ────────────────── + +/// Flatten a vertex matrix (n_verts × dim) to a DOF vector in the order +/// dictated by VERTEX_DERIVATIVE_LAYOUT. +/// +/// RowMajor → [x0 y0 z0 x1 y1 z1 …] (same as fd::flatten) +/// ColMajor → [x0 x1 … y0 y1 … z0 z1 …] +inline auto flatten(const Eigen::MatrixXd& V) +{ + return V.reshaped(); +} + +/// Unflatten a DOF vector back to a vertex matrix (n_verts × dim). +/// The inverse of dof_flatten. +inline auto unflatten(const Eigen::VectorXd& x, int dim) +{ + return x.reshaped(x.size() / dim, dim); +} + +// ─── Reorder FD results from RowMajor to VERTEX_DERIVATIVE_LAYOUT ────────── +// +// These are used when the FD callback is forced to stay in RowMajor order +// (e.g. because it calls fd::flatten/unflatten internally) but the library +// output is in VERTEX_DERIVATIVE_LAYOUT. + +/// Reorder a gradient vector computed with RowMajor flattening so that it +/// matches VERTEX_DERIVATIVE_LAYOUT. +/// +/// When the layout is already RowMajor this is a no-op (compiler will +/// optimize the copy away or it can be elided). +inline auto +reorder_gradient(const Eigen::VectorXd& g_rowmajor, int n_verts, int dim) +{ + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + return g_rowmajor; // Already in the correct order, no copy needed. + } else { + return g_rowmajor.reshaped(n_verts, dim) + .reshaped() + .eval(); + } +} + +/// Reorder a sparse Hessian from RowMajor DOF ordering to +/// VERTEX_DERIVATIVE_LAYOUT DOF ordering. +inline auto reorder_hessian( + const Eigen::SparseMatrix& H_rowmajor, int n_verts, int dim) +{ + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + return H_rowmajor; // Already in the correct order, no copy needed. + } else { + const int n = n_verts * dim; + assert(H_rowmajor.rows() == n && H_rowmajor.cols() == n); + std::vector> trips; + trips.reserve(H_rowmajor.nonZeros()); + for (int k = 0; k < H_rowmajor.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(H_rowmajor, k); + it; ++it) { + // row_major → col_major + trips.emplace_back( + (it.row() % dim) * n_verts + it.row() / dim, + (it.col() % dim) * n_verts + it.col() / dim, it.value()); + } + } + Eigen::SparseMatrix H(n, n); + H.setFromTriplets(trips.begin(), trips.end()); + return H; + } +} + +} // namespace ipc::tests \ No newline at end of file diff --git a/tests/src/tests/friction/test_force_jacobian.cpp b/tests/src/tests/friction/test_force_jacobian.cpp index 048913619..591f6fc6e 100644 --- a/tests/src/tests/friction/test_force_jacobian.cpp +++ b/tests/src/tests/friction/test_force_jacobian.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -60,11 +61,11 @@ void check_friction_force_jacobian( } auto PA_X = [&](const Eigen::VectorXd& x) { CollisionMesh fd_mesh( - fd::unflatten(x, X.cols()), mesh.edges(), mesh.faces()); + tests::unflatten(x, X.cols()), mesh.edges(), mesh.faces()); return fd_mesh.vertex_areas(); }; Eigen::MatrixXd fd_JPA_wrt_X; - fd::finite_jacobian(fd::flatten(X), PA_X, fd_JPA_wrt_X); + fd::finite_jacobian(tests::flatten(X), PA_X, fd_JPA_wrt_X); CHECK(fd::compare_jacobian(JPA_wrt_X, fd_JPA_wrt_X)); if (!fd::compare_jacobian(JPA_wrt_X, fd_JPA_wrt_X)) { @@ -79,11 +80,11 @@ void check_friction_force_jacobian( } auto EA_X = [&](const Eigen::VectorXd& x) { CollisionMesh fd_mesh( - fd::unflatten(x, X.cols()), mesh.edges(), mesh.faces()); + tests::unflatten(x, X.cols()), mesh.edges(), mesh.faces()); return fd_mesh.edge_areas(); }; Eigen::MatrixXd fd_JEA_wrt_X; - fd::finite_jacobian(fd::flatten(X), EA_X, fd_JEA_wrt_X); + fd::finite_jacobian(tests::flatten(X), EA_X, fd_JEA_wrt_X); CHECK(fd::compare_jacobian(JEA_wrt_X, fd_JEA_wrt_X)); if (!fd::compare_jacobian(JEA_wrt_X, fd_JEA_wrt_X)) { @@ -97,7 +98,7 @@ void check_friction_force_jacobian( FrictionPotential::DiffWRT::REST_POSITIONS); auto F_X = [&](const Eigen::VectorXd& x) { - Eigen::MatrixXd fd_X = fd::unflatten(x, X.cols()); + Eigen::MatrixXd fd_X = tests::unflatten(x, X.cols()); CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); fd_mesh.init_area_jacobians(); @@ -122,7 +123,7 @@ void check_friction_force_jacobian( fd_friction_collisions, fd_mesh, fd_X, Ut, velocities, B); }; Eigen::MatrixXd fd_JF_wrt_X; - fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); + fd::finite_jacobian(tests::flatten(X), F_X, fd_JF_wrt_X); CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { @@ -135,7 +136,7 @@ void check_friction_force_jacobian( FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); auto F_Ut = [&](const Eigen::VectorXd& ut) { - Eigen::MatrixXd fd_Ut = fd::unflatten(ut, Ut.cols()); + Eigen::MatrixXd fd_Ut = tests::unflatten(ut, Ut.cols()); TangentialCollisions fd_friction_collisions; if (recompute_collisions) { @@ -156,7 +157,7 @@ void check_friction_force_jacobian( return D.force(tangential_collisions, mesh, X, fd_Ut, velocities, B); }; Eigen::MatrixXd fd_JF_wrt_Ut; - fd::finite_jacobian(fd::flatten(Ut), F_Ut, fd_JF_wrt_Ut); + fd::finite_jacobian(tests::flatten(Ut), F_Ut, fd_JF_wrt_Ut); CHECK(fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)); if (!fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)) { @@ -172,10 +173,10 @@ void check_friction_force_jacobian( auto F_V = [&](const Eigen::VectorXd& v) { return D.force( tangential_collisions, mesh, X, Ut, - fd::unflatten(v, velocities.cols()), B); + tests::unflatten(v, velocities.cols()), B); }; Eigen::MatrixXd fd_JF_wrt_V; - fd::finite_jacobian(fd::flatten(velocities), F_V, fd_JF_wrt_V); + fd::finite_jacobian(tests::flatten(velocities), F_V, fd_JF_wrt_V); CHECK(fd::compare_jacobian(JF_wrt_V, fd_JF_wrt_V)); if (!fd::compare_jacobian(JF_wrt_V, fd_JF_wrt_V)) { @@ -189,10 +190,11 @@ void check_friction_force_jacobian( auto grad = [&](const Eigen::VectorXd& v) { return D.gradient( - tangential_collisions, mesh, fd::unflatten(v, velocities.cols())); + tangential_collisions, mesh, + tests::unflatten(v, velocities.cols())); }; Eigen::MatrixXd fd_hessian; - fd::finite_jacobian(fd::flatten(velocities), grad, fd_hessian); + fd::finite_jacobian(tests::flatten(velocities), grad, fd_hessian); CHECK(fd::compare_jacobian(hess_D, fd_hessian)); if (!fd::compare_jacobian(hess_D, fd_hessian)) { @@ -301,9 +303,9 @@ TEST_CASE( const auto dir = tests::DATA_DIR / "friction-force-jacobian" / scene; X = tests::loadMarketXd((dir / "X.mtx").string()); Ut = tests::loadMarketXd((dir / "Ut.mtx").string()); - Ut = fd::unflatten(Ut, X.cols()); + Ut = fd::unflatten(Ut, X.cols()); // data files are always RowMajor U = tests::loadMarketXd((dir / "U.mtx").string()); - U = fd::unflatten(U, X.cols()); + U = fd::unflatten(U, X.cols()); // data files are always RowMajor if (is_2D) { E = tests::loadMarketXi((dir / "F.mtx").string()); } else { @@ -390,11 +392,11 @@ void check_smooth_friction_force_jacobian( auto grad = [&](const Eigen::VectorXd& v) { return D.gradient( - friction_collisions, mesh, fd::unflatten(v, velocities.cols())); + friction_collisions, mesh, tests::unflatten(v, velocities.cols())); }; Eigen::MatrixXd fd_hessian; fd::finite_jacobian( - fd::flatten(velocities), grad, fd_hessian, fd::AccuracyOrder::FOURTH, + tests::flatten(velocities), grad, fd_hessian, fd::AccuracyOrder::FOURTH, 1e-6 * dhat); // CHECK(fd::compare_jacobian(hess_D, fd_hessian)); // if (!fd::compare_jacobian(hess_D, fd_hessian)) { @@ -511,7 +513,7 @@ void check_smooth_friction_force_jacobian( FrictionPotential::DiffWRT::REST_POSITIONS); auto F_X = [&](const Eigen::VectorXd& x) { - Eigen::MatrixXd fd_X = fd::unflatten(x, X.cols()); + Eigen::MatrixXd fd_X = tests::unflatten(x, X.cols()); Eigen::MatrixXd fd_lagged_positions = fd_X + Ut; CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); @@ -530,7 +532,7 @@ void check_smooth_friction_force_jacobian( }; Eigen::MatrixXd fd_JF_wrt_X; fd::finite_jacobian( - fd::flatten(X), F_X, fd_JF_wrt_X, fd::AccuracyOrder::FOURTH, + tests::flatten(X), F_X, fd_JF_wrt_X, fd::AccuracyOrder::FOURTH, 1e-6 * dhat); // CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); // if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { @@ -547,7 +549,7 @@ void check_smooth_friction_force_jacobian( FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); auto F_Ut = [&](const Eigen::VectorXd& ut) { - Eigen::MatrixXd fd_Ut = fd::unflatten(ut, Ut.cols()); + Eigen::MatrixXd fd_Ut = tests::unflatten(ut, Ut.cols()); Eigen::MatrixXd fd_lagged_positions = X + fd_Ut; auto fd_collisions = create_smooth_collision(mesh, fd_lagged_positions); @@ -563,7 +565,7 @@ void check_smooth_friction_force_jacobian( }; Eigen::MatrixXd fd_JF_wrt_Ut; fd::finite_jacobian( - fd::flatten(Ut), F_Ut, fd_JF_wrt_Ut, fd::AccuracyOrder::FOURTH, + tests::flatten(Ut), F_Ut, fd_JF_wrt_Ut, fd::AccuracyOrder::FOURTH, 1e-6 * dhat); // CHECK(fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)); // if (!fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)) { @@ -582,11 +584,11 @@ void check_smooth_friction_force_jacobian( auto F_V = [&](const Eigen::VectorXd& v) { return D.smooth_contact_force( friction_collisions, mesh, X, Ut, - fd::unflatten(v, velocities.cols())); + tests::unflatten(v, velocities.cols())); }; Eigen::MatrixXd fd_JF_wrt_V; fd::finite_jacobian( - fd::flatten(velocities), F_V, fd_JF_wrt_V, fd::AccuracyOrder::FOURTH, + tests::flatten(velocities), F_V, fd_JF_wrt_V, fd::AccuracyOrder::FOURTH, 1e-6 * dhat); CHECK( (JF_wrt_V.norm() == 0 diff --git a/tests/src/tests/friction/test_friction.cpp b/tests/src/tests/friction/test_friction.cpp index 54926584d..24629f526 100644 --- a/tests/src/tests/friction/test_friction.cpp +++ b/tests/src/tests/friction/test_friction.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -242,6 +243,15 @@ TEST_CASE( Eigen::VectorXd grad = D.gradient(tangential_collisions, mesh, velocity); + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::ColMajor) { + // The saved expected data is in RowMajor DOF ordering; convert to the + // current VERTEX_DERIVATIVE_LAYOUT before comparison. + const int n_verts = V_start.rows(); + const int dim = V_start.cols(); + expected_grad = tests::reorder_gradient(expected_grad, n_verts, dim); + expected_hess = tests::reorder_hessian(expected_hess, n_verts, dim); + } + CHECK(grad.isApprox(expected_grad)); Eigen::SparseMatrix hess = diff --git a/tests/src/tests/potential/test_adhesion_potentials.cpp b/tests/src/tests/potential/test_adhesion_potentials.cpp index 5dbf11435..1d4b06282 100644 --- a/tests/src/tests/potential/test_adhesion_potentials.cpp +++ b/tests/src/tests/potential/test_adhesion_potentials.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -93,10 +94,10 @@ TEST_CASE("Normal adhesion potential", "[potential][adhesion]") Eigen::VectorXd fgrad; fd::finite_gradient( - fd::flatten(vertices), + tests::flatten(vertices), [&](const Eigen::VectorXd& x) { return potential( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }, fgrad); @@ -111,10 +112,10 @@ TEST_CASE("Normal adhesion potential", "[potential][adhesion]") Eigen::MatrixXd fhess; fd::finite_jacobian( - fd::flatten(vertices), + tests::flatten(vertices), [&](const Eigen::VectorXd& x) { return potential.gradient( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }, fhess); diff --git a/tests/src/tests/potential/test_barrier_potential.cpp b/tests/src/tests/potential/test_barrier_potential.cpp index 55d344ac2..2bb56f509 100644 --- a/tests/src/tests/potential/test_barrier_potential.cpp +++ b/tests/src/tests/potential/test_barrier_potential.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -86,9 +87,9 @@ TEST_CASE( { auto f = [&](const Eigen::VectorXd& x) { return barrier_potential( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; - fd::finite_gradient(fd::flatten(vertices), f, fgrad_b); + fd::finite_gradient(tests::flatten(vertices), f, fgrad_b); } REQUIRE(grad_b.squaredNorm() > 0); @@ -106,9 +107,9 @@ TEST_CASE( { auto f = [&](const Eigen::VectorXd& x) { return barrier_potential.gradient( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; - fd::finite_jacobian(fd::flatten(vertices), f, fhess_b); + fd::finite_jacobian(tests::flatten(vertices), f, fhess_b); } REQUIRE(hess_b.squaredNorm() > 0); @@ -228,7 +229,7 @@ TEST_CASE( // Compute the gradient using finite differences auto f = [&](const Eigen::VectorXd& x) { - const Eigen::MatrixXd fd_V = fd::unflatten(x, mesh.dim()); + const Eigen::MatrixXd fd_V = tests::unflatten(x, mesh.dim()); NormalCollisions fd_collisions; fd_collisions.set_use_area_weighting(use_area_weighting); @@ -239,7 +240,7 @@ TEST_CASE( return barrier_potential(collisions, mesh, fd_V); }; Eigen::VectorXd fgrad_b; - fd::finite_gradient(fd::flatten(vertices), f, fgrad_b); + fd::finite_gradient(tests::flatten(vertices), f, fgrad_b); CHECK(fd::compare_gradient(grad_b, fgrad_b)); } @@ -305,7 +306,8 @@ TEST_CASE( barrier_potential.shape_derivative( collisions[i], collisions[i].vertex_ids(edges, faces), collisions[i].dof(rest_positions, edges, faces), - collisions[i].dof(vertices, edges, faces), triplets); + collisions[i].dof(vertices, edges, faces), triplets, + vertices.rows()); Eigen::SparseMatrix JF_wrt_X_sparse(ndof, ndof); JF_wrt_X_sparse.setFromTriplets(triplets.begin(), triplets.end()); const Eigen::MatrixXd JF_wrt_X = Eigen::MatrixXd(JF_wrt_X_sparse); @@ -314,20 +316,19 @@ TEST_CASE( // TODO: Recompute weight based on x assert(use_area_weighting == false); // Recompute eps_x based on x + const Eigen::MatrixXd X = + tests::unflatten(x, rest_positions.cols()); double prev_eps_x = -1; if (collisions.is_edge_edge(i)) { EdgeEdgeNormalCollision& c = dynamic_cast(collisions[i]); prev_eps_x = c.eps_x; c.eps_x = edge_edge_mollifier_threshold( - x.segment<3>(3 * edges(c.edge0_id, 0)), - x.segment<3>(3 * edges(c.edge0_id, 1)), - x.segment<3>(3 * edges(c.edge1_id, 0)), - x.segment<3>(3 * edges(c.edge1_id, 1))); + X.row(edges(c.edge0_id, 0)), X.row(edges(c.edge0_id, 1)), + X.row(edges(c.edge1_id, 0)), X.row(edges(c.edge1_id, 1))); } - const Eigen::MatrixXd positions = - fd::unflatten(x, rest_positions.cols()) + displacements; + const Eigen::MatrixXd positions = X + displacements; const VectorMax12d dof = collisions[i].dof(positions, edges, faces); Eigen::VectorXd grad = Eigen::VectorXd::Zero(ndof); @@ -346,7 +347,7 @@ TEST_CASE( }; Eigen::MatrixXd fd_JF_wrt_X; - fd::finite_jacobian(fd::flatten(rest_positions), F_X, fd_JF_wrt_X); + fd::finite_jacobian(tests::flatten(rest_positions), F_X, fd_JF_wrt_X); CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { @@ -365,7 +366,8 @@ TEST_CASE( barrier_potential.shape_derivative( collisions[i], collisions[i].vertex_ids(edges, faces), collisions[i].dof(rest_positions, edges, faces), - collisions[i].dof(vertices, edges, faces), triplets); + collisions[i].dof(vertices, edges, faces), triplets, + vertices.rows()); Eigen::SparseMatrix JF_wrt_X_sparse(ndof, ndof); JF_wrt_X_sparse.setFromTriplets(triplets.begin(), triplets.end()); sum += Eigen::MatrixXd(JF_wrt_X_sparse); @@ -373,7 +375,7 @@ TEST_CASE( CHECK(fd::compare_jacobian(JF_wrt_X, sum)); auto F_X = [&](const Eigen::VectorXd& x) { - const Eigen::MatrixXd fd_X = fd::unflatten(x, rest_positions.cols()); + const Eigen::MatrixXd fd_X = tests::unflatten(x, rest_positions.cols()); const Eigen::MatrixXd fd_V = fd_X + displacements; CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); @@ -388,7 +390,7 @@ TEST_CASE( return barrier_potential.gradient(collisions, fd_mesh, fd_V); }; Eigen::MatrixXd fd_JF_wrt_X; - fd::finite_jacobian(fd::flatten(rest_positions), F_X, fd_JF_wrt_X); + fd::finite_jacobian(tests::flatten(rest_positions), F_X, fd_JF_wrt_X); CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { @@ -443,7 +445,7 @@ TEST_CASE( barrier_potential.shape_derivative(collisions, mesh, vertices); auto F_X = [&](const Eigen::VectorXd& x) { - const Eigen::MatrixXd fd_X = fd::unflatten(x, X.cols()); + const Eigen::MatrixXd fd_X = tests::unflatten(x, X.cols()); const Eigen::MatrixXd fd_V = fd_X + U; CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); @@ -456,7 +458,7 @@ TEST_CASE( return barrier_potential.gradient(fd_collisions, fd_mesh, fd_V); }; Eigen::MatrixXd fd_JF_wrt_X; - fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); + fd::finite_jacobian(tests::flatten(X), F_X, fd_JF_wrt_X); CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { diff --git a/tests/src/tests/potential/test_friction_potential.cpp b/tests/src/tests/potential/test_friction_potential.cpp index f428adde4..a66495e0a 100644 --- a/tests/src/tests/potential/test_friction_potential.cpp +++ b/tests/src/tests/potential/test_friction_potential.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -32,15 +33,16 @@ TEST_CASE("Friction gradient and hessian", "[friction][gradient][hessian]") // Compute the gradient using finite differences auto f = [&](const Eigen::VectorXd& x) { - const Eigen::MatrixXd fd_U = fd::unflatten(x, data.V1.cols()) - data.V0; + const Eigen::MatrixXd fd_U = + tests::unflatten(x, data.V1.cols()) - data.V0; return D(tangential_collisions, mesh, fd_U); }; Eigen::VectorXd fgrad; - fd::finite_gradient(fd::flatten(V1), f, fgrad); + fd::finite_gradient(tests::flatten(V1), f, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); const Eigen::MatrixXd hess = D.hessian(tangential_collisions, mesh, U); Eigen::MatrixXd fhess; - fd::finite_hessian(fd::flatten(V1), f, fhess); + fd::finite_hessian(tests::flatten(V1), f, fhess); CHECK(fd::compare_hessian(hess, fhess, 1e-3)); } \ No newline at end of file diff --git a/tests/src/tests/potential/test_smooth_potential.cpp b/tests/src/tests/potential/test_smooth_potential.cpp index ca073a2c7..2e9af48e3 100644 --- a/tests/src/tests/potential/test_smooth_potential.cpp +++ b/tests/src/tests/potential/test_smooth_potential.cpp @@ -1,10 +1,12 @@ #include +#include #include #include #include #include +#include #include #include #include @@ -12,7 +14,6 @@ #include #include #include -#include using namespace ipc; @@ -65,10 +66,11 @@ TEST_CASE("Smooth barrier potential codim", "[smooth_potential]") { auto f = [&](const Eigen::VectorXd& x) { return potential( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_gradient( - fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, + 1e-8); } // REQUIRE(grad_b.squaredNorm() > 1e-8); @@ -88,10 +90,11 @@ TEST_CASE("Smooth barrier potential codim", "[smooth_potential]") { auto f = [&](const Eigen::VectorXd& x) { return potential.gradient( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_jacobian( - fd::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, + 1e-8); } REQUIRE(hess_b.squaredNorm() > 1e-8); @@ -187,10 +190,11 @@ TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) { auto f = [&](const Eigen::VectorXd& x) { return potential( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_gradient( - fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, + 1e-8); } // REQUIRE(grad_b.squaredNorm() > 1e-8); @@ -210,10 +214,11 @@ TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) { auto f = [&](const Eigen::VectorXd& x) { return potential.gradient( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_jacobian( - fd::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, + 1e-8); } REQUIRE(hess_b.squaredNorm() > 1e-8); @@ -279,10 +284,11 @@ TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") { auto f = [&](const Eigen::VectorXd& x) { return potential( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_gradient( - fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, + 1e-8); } REQUIRE(grad_b.squaredNorm() > 1e-8); @@ -302,10 +308,11 @@ TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") { auto f = [&](const Eigen::VectorXd& x) { return potential.gradient( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_jacobian( - fd::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, + 1e-8); } REQUIRE(hess_b.squaredNorm() > 1e-3); @@ -368,10 +375,11 @@ TEST_CASE("Smooth barrier potential real sim 2D C^1", "[smooth_potential]") { auto f = [&](const Eigen::VectorXd& x) { return potential( - collisions, mesh, fd::unflatten(x, vertices.cols())); + collisions, mesh, tests::unflatten(x, vertices.cols())); }; fd::finite_gradient( - fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + tests::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, + 1e-8); } REQUIRE(grad_b.squaredNorm() > 1e-8); From fd46bd10fe0325737767c8a9d43842ec8bc85056 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 18:39:19 -0500 Subject: [PATCH 14/16] Support vertex-to-dof matrix layout and tests Use VERTEX_DERIVATIVE_LAYOUT to map vertex-level sparse matrices into per-dof sparse matrices, computing triplets with n_rows/n_cols and constructing M_dof with the correct dimensions. Move the function declaration in the header and add unit tests (including Catch2 generators and config include) that verify sparsity pattern and the M_dof * x == flatten(M_V * V) semantics. --- src/ipc/collision_mesh.cpp | 15 ++++-- src/ipc/collision_mesh.hpp | 8 +-- tests/src/tests/test_collision_mesh.cpp | 71 +++++++++++++++++++++++++ 3 files changed, 87 insertions(+), 7 deletions(-) diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index e37133783..8526a0881 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -212,18 +212,27 @@ void CollisionMesh::init_selection_matrices(const int dim) Eigen::SparseMatrix CollisionMesh::vertex_matrix_to_dof_matrix( const Eigen::SparseMatrix& M_V, int dim) { + const int n_rows = M_V.rows(); + const int n_cols = M_V.cols(); + std::vector> triplets; using InnerIterator = Eigen::SparseMatrix::InnerIterator; for (int k = 0; k < M_V.outerSize(); ++k) { for (InnerIterator it(M_V, k); it; ++it) { for (int d = 0; d < dim; d++) { - triplets.emplace_back( - dim * it.row() + d, dim * it.col() + d, it.value()); + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + triplets.emplace_back( + dim * it.row() + d, dim * it.col() + d, it.value()); + } else { + triplets.emplace_back( + n_rows * d + it.row(), n_cols * d + it.col(), + it.value()); + } } } } - Eigen::SparseMatrix M_dof(M_V.rows() * dim, M_V.cols() * dim); + Eigen::SparseMatrix M_dof(n_rows * dim, n_cols * dim); M_dof.setFromTriplets(triplets.begin(), triplets.end()); M_dof.makeCompressed(); return M_dof; diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index ae1a215a6..073193dda 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -322,6 +322,10 @@ class CollisionMesh { Eigen::ConstRef faces, Eigen::ConstRef edges); + /// @brief Convert a matrix meant for M_V * vertices to M_dof * x by duplicating the entries dim times. + static Eigen::SparseMatrix vertex_matrix_to_dof_matrix( + const Eigen::SparseMatrix& M_V, int dim); + /// A function that takes two vertex IDs and returns true if the vertices /// (and faces or edges containing the vertices) can collide. By default all /// primitives can collide with all other primitives. @@ -351,10 +355,6 @@ class CollisionMesh { /// @brief Initialize vertex and edge areas. void init_areas(); - /// @brief Convert a matrix meant for M_V * vertices to M_dof * x by duplicating the entries dim times. - static Eigen::SparseMatrix vertex_matrix_to_dof_matrix( - const Eigen::SparseMatrix& M_V, int dim); - // ----------------------------------------------------------------------- /// @brief The full vertex positions at rest (|FV| × dim). diff --git a/tests/src/tests/test_collision_mesh.cpp b/tests/src/tests/test_collision_mesh.cpp index fa60d6328..2077cd74e 100644 --- a/tests/src/tests/test_collision_mesh.cpp +++ b/tests/src/tests/test_collision_mesh.cpp @@ -1,6 +1,8 @@ #include #include +#include #include +#include using namespace ipc; @@ -113,4 +115,73 @@ TEST_CASE("Codim points collision mesh", "[collision_mesh]") Eigen::VectorXi expected_codim_vertices(4); expected_codim_vertices << 0, 1, 2, 3; CHECK(mesh.codim_vertices() == expected_codim_vertices); +} + +TEST_CASE( + "vertex_matrix_to_dof_matrix", + "[collision_mesh][vertex_matrix_to_dof_matrix]") +{ + // Build a small 2×3 vertex-level matrix: + // + // M_V = [ 1 0 2 ] + // [ 0 3 0 ] + // + std::vector> triplets; + triplets.emplace_back(0, 0, 1.0); + triplets.emplace_back(0, 2, 2.0); + triplets.emplace_back(1, 1, 3.0); + + Eigen::SparseMatrix M_V(2, 3); + M_V.setFromTriplets(triplets.begin(), triplets.end()); + + const int n_rows = static_cast(M_V.rows()); // 2 + const int n_cols = static_cast(M_V.cols()); // 3 + + const int dim = GENERATE(2, 3); + CAPTURE(dim); + + Eigen::SparseMatrix M_dof = + CollisionMesh::vertex_matrix_to_dof_matrix(M_V, dim); + + CHECK(M_dof.rows() == n_rows * dim); + CHECK(M_dof.cols() == n_cols * dim); + + // Check the sparsity pattern: for each non-zero (r, c, v) in M_V, + // it should appear at (global_row(r, d), global_col(c, d)) for + // every d in [0, dim). + Eigen::MatrixXd M_dense = Eigen::MatrixXd(M_dof); + Eigen::MatrixXd expected = + Eigen::MatrixXd::Zero(n_rows * dim, n_cols * dim); + + using InnerIterator = Eigen::SparseMatrix::InnerIterator; + for (int k = 0; k < M_V.outerSize(); ++k) { + for (InnerIterator it(M_V, k); it; ++it) { + for (int d = 0; d < dim; d++) { + int gr, gc; + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + gr = dim * it.row() + d; + gc = dim * it.col() + d; + } else { + gr = n_rows * d + it.row(); + gc = n_cols * d + it.col(); + } + expected(gr, gc) = it.value(); + } + } + } + + CHECK(M_dense == expected); + + // Verify semantics: M_dof * flatten(V) == flatten(M_V * V) + // V is (n_cols × dim) because M_V has n_cols columns, one per vertex. + Eigen::MatrixXd V(n_cols, dim); // NOLINT(bugprone-argument-comment) + V.setRandom(); + + Eigen::VectorXd x_dof = V.reshaped(); + Eigen::VectorXd expected_dof = + (M_V * V).reshaped(); + + Eigen::VectorXd actual_dof = M_dof * x_dof; + + CHECK(actual_dof.isApprox(expected_dof)); } \ No newline at end of file From 0d3a23ff88f3945aac77b4f9050ad05176c0f5fb Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 19:00:11 -0500 Subject: [PATCH 15/16] Add configurable vertex derivative layout option Introduce IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT CMake cache option (default RowMajor) with allowed values ColMajor and RowMajor, and mark it advanced. Export the value into src/ipc/config.hpp.in so VERTEX_DERIVATIVE_LAYOUT is set to the chosen Eigen layout at configure time. --- CMakeLists.txt | 5 ++++- src/ipc/config.hpp.in | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2dc83ba96..d52d55134 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,9 +85,12 @@ option(IPC_TOOLKIT_WITH_PROFILER "Enable performance profiler" # Advanced options option(IPC_TOOLKIT_WITH_CODE_COVERAGE "Enable coverage reporting" OFF) - mark_as_advanced(IPC_TOOLKIT_WITH_CODE_COVERAGE) # This is used in GitHub Actions +set(IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT "RowMajor" CACHE STRING "Layout of the vertex derivatives") +set_property(CACHE IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT PROPERTY STRINGS "ColMajor" "RowMajor") +mark_as_advanced(IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT) # This is an advanced option that most users won't care about. + # Set default minimum C++ standard if(IPC_TOOLKIT_TOPLEVEL_PROJECT) set(CMAKE_CXX_STANDARD 17) diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index c4f473b7f..2cfdba279 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -39,6 +39,9 @@ using index_t = int32_t; // using scalar_t = float; // #endif -inline constexpr int VERTEX_DERIVATIVE_LAYOUT = Eigen::RowMajor; +// clang-format off +/// @brief The layout of derivatives of vertex positions. The default is `Eigen::RowMajor`. +inline constexpr int VERTEX_DERIVATIVE_LAYOUT = Eigen::@IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT@; +// clang-format on } // namespace ipc \ No newline at end of file From 744f8f446db01f6803b4e593ee62cbd70ad61d53 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 28 Feb 2026 19:13:30 -0500 Subject: [PATCH 16/16] Add vertex derivative layout docs Describe row-major default and new CMake cache option IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT (RowMajor or ColMajor), with build examples, affected APIs, and an experimental feature warning. --- docs/source/tutorials/getting_started.rst | 4 +++ docs/source/tutorials/misc.rst | 43 ++++++++++++++++++++++- 2 files changed, 46 insertions(+), 1 deletion(-) diff --git a/docs/source/tutorials/getting_started.rst b/docs/source/tutorials/getting_started.rst index 0f945cc37..caa518d61 100644 --- a/docs/source/tutorials/getting_started.rst +++ b/docs/source/tutorials/getting_started.rst @@ -242,6 +242,10 @@ and the Hessian of size :math:`|V|d \times |V|d` with the order \frac{\partial^2 B}{\partial z_n^2} \end{bmatrix}. +.. note:: + + This row-major derivative ordering is the default. If your simulation framework uses a column-major DOF layout, the IPC Toolkit can be configured to use that instead. See :ref:`Vertex Derivative Layout ` for details. + Adaptive Barrier Stiffness ^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/docs/source/tutorials/misc.rst b/docs/source/tutorials/misc.rst index 3abef8fa1..0b1e952a6 100644 --- a/docs/source/tutorials/misc.rst +++ b/docs/source/tutorials/misc.rst @@ -142,4 +142,45 @@ You can also get the current maximum number of threads as follows: .. code-block:: python - nthreads = ipctk.get_num_threads() \ No newline at end of file + nthreads = ipctk.get_num_threads() + +Vertex Derivative Layout +------------------------ + +By default, the IPC Toolkit orders derivative vectors (gradients and Hessians) with respect to vertex DOFs in a **row-major** layout: + +.. math:: + + \mathbf{x} = [x_0, y_0, z_0, x_1, y_1, z_1, \ldots] + +Some simulation frameworks instead use a **column-major** layout, where all coordinates of the same dimension are grouped together: + +.. math:: + + \mathbf{x} = [x_0, x_1, \ldots, y_0, y_1, \ldots, z_0, z_1, \ldots] + +The IPC Toolkit provides a compile-time CMake option to select which layout is used for all gradient and Hessian assembly. This is controlled by the ``IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT`` cache variable, which accepts either ``RowMajor`` (default) or ``ColMajor``. + +To build with column-major derivative ordering, pass the option when configuring CMake: + +.. code-block:: bash + + cmake -DIPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT=ColMajor -B build -S . + +Or to explicitly use the default row-major layout: + +.. code-block:: bash + + cmake -DIPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT=RowMajor -B build -S . + +.. note:: + + This is an advanced option marked as such in CMake. Most users will not need to change it. It is primarily useful when integrating the IPC Toolkit into a simulation framework that stores DOFs in a column-major layout, avoiding the need to permute vectors and matrices at the interface boundary. + +This setting affects all functions that assemble local per-element gradients and Hessians into global vectors and matrices, including ``local_gradient_to_global_gradient``, ``local_hessian_to_global_triplets``, and ``local_jacobian_to_global_triplets``. It also affects ``CollisionMesh::vertex_matrix_to_dof_matrix``, which converts vertex-indexed sparse matrices to DOF-indexed sparse matrices. + +The chosen layout is stored as the compile-time constant ``ipc::VERTEX_DERIVATIVE_LAYOUT`` (equal to ``Eigen::RowMajor`` or ``Eigen::ColMajor``) in the generated ``config.hpp`` header. + +.. warning:: + + This feature is experimental. If you encounter any bugs, please report them on `GitHub `_. \ No newline at end of file