diff --git a/src/ipc/smooth_contact/common.hpp b/src/ipc/smooth_contact/common.hpp index f7390f512..53df77678 100644 --- a/src/ipc/smooth_contact/common.hpp +++ b/src/ipc/smooth_contact/common.hpp @@ -99,6 +99,8 @@ struct SmoothContactParameters { double beta_n = 0; int r = 2; + bool use_rest_shape_measure = false; + private: double m_adaptive_dhat_ratio = 0.5; }; diff --git a/src/ipc/smooth_contact/primitives/edge2.cpp b/src/ipc/smooth_contact/primitives/edge2.cpp index 60e8f52e4..c3cf9e26b 100644 --- a/src/ipc/smooth_contact/primitives/edge2.cpp +++ b/src/ipc/smooth_contact/primitives/edge2.cpp @@ -13,6 +13,11 @@ Edge2::Edge2( { m_vertex_ids = { { mesh.edges()(id, 0), mesh.edges()(id, 1) } }; + // Rest-shape edge length: computed from rest positions — constant + // quadrature weight + const Eigen::MatrixXd& rp = mesh.rest_positions(); + m_rest_length = (rp.row(m_vertex_ids[1]) - rp.row(m_vertex_ids[0])).norm(); + if (mesh.is_orient_vertex(m_vertex_ids[0]) && mesh.is_orient_vertex(m_vertex_ids[1])) { m_is_active = @@ -32,7 +37,12 @@ double Edge2::potential( Eigen::ConstRef x) const { assert(m_is_active); - return (x.tail<2>() - x.head<2>()).norm(); + if (!m_params.use_rest_shape_measure) { + return (x.tail<2>() - x.head<2>()).norm(); + } + // Return the rest-shape edge length — constant quadrature weight per the + // paper (Eq. 13) + return m_rest_length; } Vector6d Edge2::grad( @@ -40,13 +50,17 @@ Vector6d Edge2::grad( Eigen::ConstRef x) const { assert(m_is_active); - const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); - const double len = t.norm(); - Vector6d g; - g.setZero(); - g.segment<2>(2) = -t / len; - g.segment<2>(4) = t / len; - return g; + if (!m_params.use_rest_shape_measure) { + const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); + const double len = t.norm(); + Vector6d g; + g.setZero(); + g.segment<2>(2) = -t / len; + g.segment<2>(4) = t / len; + return g; + } + // Rest length is constant — no gradient contribution from this primitive + return Vector6d::Zero(); } Matrix6d Edge2::hessian( @@ -54,23 +68,26 @@ Matrix6d Edge2::hessian( Eigen::ConstRef x) const { assert(m_is_active); - Matrix6d h; - h.setZero(); + if (!m_params.use_rest_shape_measure) { + Matrix6d h; + h.setZero(); #ifdef IPC_TOOLKIT_DEBUG_AUTODIFF - ScalarBase::setVariableCount(4); - using T = ADHessian<4>; - auto xAD = slice_positions(x); - h.block<4, 4>(2, 2) = (xAD.row(0) - xAD.row(1)).norm().Hess; + ScalarBase::setVariableCount(4); + using T = ADHessian<4>; + auto xAD = slice_positions(x); + h.block<4, 4>(2, 2) = (xAD.row(0) - xAD.row(1)).norm().Hess; #else - const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); - const double norm = t.norm(); - h.block<2, 2>(2, 2) = - (Eigen::Matrix2d::Identity() - t * (1. / norm / norm) * t.transpose()) - / norm; - h.block<2, 2>(4, 4) = h.block<2, 2>(2, 2); - h.block<2, 2>(2, 4) = -h.block<2, 2>(2, 2); - h.block<2, 2>(4, 2) = -h.block<2, 2>(2, 2); + const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); + const double norm = t.norm(); + h.block<2, 2>(2, 2) = (Eigen::Matrix2d::Identity() + - t * (1. / norm / norm) * t.transpose()) + / norm; + h.block<2, 2>(4, 4) = h.block<2, 2>(2, 2); + h.block<2, 2>(2, 4) = -h.block<2, 2>(2, 2); + h.block<2, 2>(4, 2) = -h.block<2, 2>(2, 2); #endif - return h; + return h; + } + return Matrix6d::Zero(); } } // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge2.hpp b/src/ipc/smooth_contact/primitives/edge2.hpp index d9bfe5efa..4e5a30a47 100644 --- a/src/ipc/smooth_contact/primitives/edge2.hpp +++ b/src/ipc/smooth_contact/primitives/edge2.hpp @@ -30,5 +30,9 @@ class Edge2 : public Primitive { Matrix6d hessian( Eigen::ConstRef d, Eigen::ConstRef x) const; + +private: + double + m_rest_length; ///< Rest-shape edge length (quadrature weight, constant) }; } // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge3.cpp b/src/ipc/smooth_contact/primitives/edge3.cpp index 884c355ff..54962e2ba 100644 --- a/src/ipc/smooth_contact/primitives/edge3.cpp +++ b/src/ipc/smooth_contact/primitives/edge3.cpp @@ -82,6 +82,13 @@ Edge3::Edge3( faces.row(i) = face_rows[i]; } + // Rest-shape squared edge length: from rest positions — constant quadrature + // weight + { + const Eigen::MatrixXd& rp = mesh.rest_positions(); + m_rest_sq_length = (rp.row(e1_id) - rp.row(e0_id)).squaredNorm(); + } + if (m_vertex_ids.size() > N_EDGE_NEIGHBORS_3D) { logger().error( "Too many vertex neighbors for edge3 primitive! vertex count {} exceeds N_EDGE_NEIGHBORS_3D {}. " @@ -193,8 +200,9 @@ T Edge3::smooth_edge3_term( normal_term = Math::smooth_heaviside(normal_term - 1, 1., 0); } - // Weight: squared edge length - const T weight = (e1 - e0).squaredNorm(); + // Weight: squared edge length (rest-shape if enabled, deformed otherwise) + const T weight = m_params.use_rest_shape_measure ? T(m_rest_sq_length) + : (e1 - e0).squaredNorm(); return weight * tangent_term * normal_term; } @@ -559,17 +567,18 @@ GradientType Edge3::smooth_edge3_term_gradient( Eigen::VectorXd grad_tmp = tangent_grad * normal_term + normal_grad * tangent_term; - // Weight = (e1 - e0).squaredNorm() const Eigen::RowVector3d edge = X.row(1) - X.row(0); - const double weight = edge.squaredNorm(); + const double weight = + m_params.use_rest_shape_measure ? m_rest_sq_length : edge.squaredNorm(); grad_tmp *= weight; - // Derivative of weight w.r.t. e0 and e1 - // d/d(e0) (e1-e0)^2 = 2*(e0-e1), d/d(e1) (e1-e0)^2 = 2*(e1-e0) - const int id_e0 = 3; // offset in [direction, X] - const int id_e1 = 6; - grad_tmp.segment<3>(id_e0) += (2 * val) * (-edge).transpose(); - grad_tmp.segment<3>(id_e1) += (2 * val) * edge.transpose(); - + if (!m_params.use_rest_shape_measure) { + // Derivative of weight w.r.t. e0 and e1 + // d/d(e0) (e1-e0)^2 = 2*(e0-e1), d/d(e1) (e1-e0)^2 = 2*(e1-e0) + const int id_e0 = 3; // offset in [direction, X] + const int id_e1 = 6; + grad_tmp.segment<3>(id_e0) += (2 * val) * (-edge).transpose(); + grad_tmp.segment<3>(id_e1) += (2 * val) * edge.transpose(); + } val *= weight; return std::make_tuple(val, grad_tmp); diff --git a/src/ipc/smooth_contact/primitives/edge3.hpp b/src/ipc/smooth_contact/primitives/edge3.hpp index a2f266e32..5ac56e32a 100644 --- a/src/ipc/smooth_contact/primitives/edge3.hpp +++ b/src/ipc/smooth_contact/primitives/edge3.hpp @@ -177,6 +177,8 @@ class Edge3 : public Primitive { /// @brief Whether the edge is orientable. If false, the normal term is not applied. bool orientable; + double m_rest_sq_length; ///< Rest-shape squared edge length (quadrature + ///< weight, constant) }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/primitives/face.cpp b/src/ipc/smooth_contact/primitives/face.cpp index 9621b8f7e..ce90bf6e0 100644 --- a/src/ipc/smooth_contact/primitives/face.cpp +++ b/src/ipc/smooth_contact/primitives/face.cpp @@ -30,6 +30,15 @@ Face::Face( { m_vertex_ids = { { mesh.faces()(id, 0), mesh.faces()(id, 1), mesh.faces()(id, 2) } }; + + // Rest-shape area: computed from rest positions — constant quadrature + // weight + const Eigen::MatrixXd& rp = mesh.rest_positions(); + Eigen::Vector3d ra = rp.row(m_vertex_ids[1]) - rp.row(m_vertex_ids[0]); + Eigen::Vector3d rb = rp.row(m_vertex_ids[2]) - rp.row(m_vertex_ids[0]); + m_rest_area = 0.5 * ra.cross(rb).norm(); + + // Active check uses deformed positions / direction Eigen::Vector3d a = vertices.row(m_vertex_ids[1]) - vertices.row(m_vertex_ids[0]); Eigen::Vector3d b = @@ -41,31 +50,44 @@ Face::Face( m_is_active = !orientable || a.cross(b).dot(d) > 0; } int Face::n_vertices() const { return N_FACE_NEIGHBORS_3D; } -double -Face::potential(Eigen::ConstRef d, Eigen::ConstRef x) +double Face::potential( + Eigen::ConstRef d, Eigen::ConstRef x) const { - return smooth_face_term(x.head<3>(), x.segment<3>(3), x.tail<3>()); + if (!m_params.use_rest_shape_measure) { + return smooth_face_term( + x.head<3>(), x.segment<3>(3), x.tail<3>()); + } + // Return the rest-shape area — constant quadrature weight per the paper + // (Eq. 13) + return m_rest_area; } -Vector12d -Face::grad(Eigen::ConstRef d, Eigen::ConstRef x) +Vector12d Face::grad( + Eigen::ConstRef d, Eigen::ConstRef x) const { - Vector12d g; - g.setZero(); - ScalarBase::setVariableCount(9); - auto X = slice_positions, 3, 3>(x); - g.tail<9>() = - smooth_face_term>(X.row(0), X.row(1), X.row(2)).grad; - return g; + if (!m_params.use_rest_shape_measure) { + Vector12d g; + g.setZero(); + ScalarBase::setVariableCount(9); + auto X = slice_positions, 3, 3>(x); + g.tail<9>() = + smooth_face_term>(X.row(0), X.row(1), X.row(2)).grad; + return g; + } + // Rest area is constant — no gradient contribution from this primitive + return Vector12d::Zero(); } -Matrix12d -Face::hessian(Eigen::ConstRef d, Eigen::ConstRef x) +Matrix12d Face::hessian( + Eigen::ConstRef d, Eigen::ConstRef x) const { - Matrix12d h; - h.setZero(); - ScalarBase::setVariableCount(9); - auto X = slice_positions, 3, 3>(x); - h.bottomRightCorner<9, 9>() = - smooth_face_term>(X.row(0), X.row(1), X.row(2)).Hess; - return h; + if (!m_params.use_rest_shape_measure) { + Matrix12d h; + h.setZero(); + ScalarBase::setVariableCount(9); + auto X = slice_positions, 3, 3>(x); + h.bottomRightCorner<9, 9>() = + smooth_face_term>(X.row(0), X.row(1), X.row(2)).Hess; + return h; + } + return Matrix12d::Zero(); } } // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/face.hpp b/src/ipc/smooth_contact/primitives/face.hpp index db061b3f6..a150c24be 100644 --- a/src/ipc/smooth_contact/primitives/face.hpp +++ b/src/ipc/smooth_contact/primitives/face.hpp @@ -21,12 +21,15 @@ class Face : public Primitive { int n_vertices() const override; int n_dofs() const override { return n_vertices() * DIM; } - static double - potential(Eigen::ConstRef d, Eigen::ConstRef x); - static Vector12d - grad(Eigen::ConstRef d, Eigen::ConstRef x); - static Matrix12d - hessian(Eigen::ConstRef d, Eigen::ConstRef x); + double potential( + Eigen::ConstRef d, Eigen::ConstRef x) const; + Vector12d + grad(Eigen::ConstRef d, Eigen::ConstRef x) const; + Matrix12d hessian( + Eigen::ConstRef d, Eigen::ConstRef x) const; + +private: + double m_rest_area; ///< Rest-shape face area (quadrature weight, constant) }; /// @brief d points from triangle to the point diff --git a/src/ipc/smooth_contact/primitives/point2.cpp b/src/ipc/smooth_contact/primitives/point2.cpp index 9d7e0ba79..c818d7bcb 100644 --- a/src/ipc/smooth_contact/primitives/point2.cpp +++ b/src/ipc/smooth_contact/primitives/point2.cpp @@ -61,7 +61,8 @@ namespace { Eigen::ConstRef> e0, Eigen::ConstRef> e1, const SmoothContactParameters& params, - const bool orientable) + const bool orientable, + const scalar measure) { const Eigen::Vector2 dn = -direc.normalized(); const Eigen::Vector2 t0 = (e0 - v).normalized(), @@ -83,7 +84,7 @@ namespace { * Math::smooth_heaviside(tmp - 1., params.alpha_n, 0); } - return val * ((e0 - v).norm() + (e1 - v).norm()) / 2.; + return val * measure; } template @@ -91,7 +92,8 @@ namespace { Eigen::ConstRef> v, Eigen::ConstRef> direc, Eigen::ConstRef> e0, - const SmoothContactParameters& params) + const SmoothContactParameters& params, + const scalar measure) { const Eigen::Vector2 dn = -direc.normalized(); const Eigen::Vector2 t0 = e0 - v; @@ -99,7 +101,7 @@ namespace { const scalar tangent_term = Math::smooth_heaviside( dn.dot(t0) / t0.norm(), params.alpha_t, params.beta_t); - return tangent_term * t0.norm(); + return tangent_term * measure; } } // namespace @@ -121,6 +123,12 @@ Point2::Point2( m_is_active = smooth_point2_term_type( vertices.row(id), d, vertices.row(m_vertex_ids[1]), vertices.row(m_vertex_ids[2]), params, orientable); + + // rest-shape measure: average half-edge length from rest positions + const Eigen::MatrixXd& rp = mesh.rest_positions(); + m_rest_measure = ((rp.row(neighbor_verts[0]) - rp.row(id)).norm() + + (rp.row(neighbor_verts[1]) - rp.row(id)).norm()) + / 2.0; } else if (has_neighbor_1 || has_neighbor_2) { m_vertex_ids = { { id, has_neighbor_1 ? neighbor_verts[0] : neighbor_verts[1] } @@ -132,10 +140,15 @@ Point2::Point2( .normalized(); m_is_active = dn.dot(t0) > -params.alpha_t; + // rest-shape measure: half-edge length from rest positions + const Eigen::MatrixXd& rp = mesh.rest_positions(); + m_rest_measure = + (rp.row(m_vertex_ids[1]) - rp.row(m_vertex_ids[0])).norm(); } else { m_vertex_ids.resize(1); m_vertex_ids[0] = id; m_is_active = true; + m_rest_measure = 1.0; // no neighbors — unit weight } } @@ -146,14 +159,22 @@ double Point2::potential( const VectorMax& x) const { if (has_neighbor_1 && has_neighbor_2) { + const double measure = m_params.use_rest_shape_measure + ? m_rest_measure + : ((x.segment(DIM) - x.segment(0)).norm() + + (x.segment(2 * DIM) - x.segment(0)).norm()) + / 2.0; return smooth_point2_term( x.segment(0), d, x.segment(DIM), x.segment(2 * DIM), - m_params, orientable); + m_params, orientable, measure); } else if (has_neighbor_1 || has_neighbor_2) { + const double measure = m_params.use_rest_shape_measure + ? m_rest_measure + : (x.segment(DIM) - x.segment(0)).norm(); return smooth_point2_term_one_side( - x.segment(0), d, x.segment(DIM), m_params); + x.segment(0), d, x.segment(DIM), m_params, measure); } else { - return 1.; + return m_rest_measure; } } VectorMax Point2::grad( @@ -166,8 +187,13 @@ VectorMax Point2::grad( Eigen::Vector tmp; tmp << d, x; Eigen::Matrix X = slice_positions(tmp); + const T measure = m_params.use_rest_shape_measure + ? T(m_rest_measure) + : ((X.row(2) - X.row(1)).norm() + (X.row(3) - X.row(1)).norm()) + / 2.0; return smooth_point2_term( - X.row(1), X.row(0), X.row(2), X.row(3), m_params, orientable) + X.row(1), X.row(0), X.row(2), X.row(3), m_params, orientable, + measure) .grad; } else if (has_neighbor_1 || has_neighbor_2) { ScalarBase::setVariableCount(3 * DIM); @@ -175,8 +201,11 @@ VectorMax Point2::grad( Eigen::Vector tmp; tmp << d, x; Eigen::Matrix X = slice_positions(tmp); + const T measure = m_params.use_rest_shape_measure + ? T(m_rest_measure) + : (X.row(2) - X.row(1)).norm(); return smooth_point2_term_one_side( - X.row(1), X.row(0), X.row(2), m_params) + X.row(1), X.row(0), X.row(2), m_params, measure) .grad; } else { return VectorMax::Zero( @@ -197,8 +226,13 @@ Point2::hessian( Eigen::Vector tmp; tmp << d, x; Eigen::Matrix X = slice_positions(tmp); + const T measure = m_params.use_rest_shape_measure + ? T(m_rest_measure) + : ((X.row(2) - X.row(1)).norm() + (X.row(3) - X.row(1)).norm()) + / 2.0; return smooth_point2_term( - X.row(1), X.row(0), X.row(2), X.row(3), m_params, orientable) + X.row(1), X.row(0), X.row(2), X.row(3), m_params, orientable, + measure) .Hess; } else if (has_neighbor_1 || has_neighbor_2) { ScalarBase::setVariableCount(3 * DIM); @@ -206,8 +240,11 @@ Point2::hessian( Eigen::Vector tmp; tmp << d, x; Eigen::Matrix X = slice_positions(tmp); + const T measure = m_params.use_rest_shape_measure + ? T(m_rest_measure) + : (X.row(2) - X.row(1)).norm(); return smooth_point2_term_one_side( - X.row(1), X.row(0), X.row(2), m_params) + X.row(1), X.row(0), X.row(2), m_params, measure) .Hess; } else { return MatrixMax::Zero( diff --git a/src/ipc/smooth_contact/primitives/point2.hpp b/src/ipc/smooth_contact/primitives/point2.hpp index a72ac2ea2..56f57b9cd 100644 --- a/src/ipc/smooth_contact/primitives/point2.hpp +++ b/src/ipc/smooth_contact/primitives/point2.hpp @@ -41,5 +41,7 @@ class Point2 : public Primitive { private: bool has_neighbor_1, has_neighbor_2; bool orientable; + double m_rest_measure; ///< Rest-shape vertex length measure (quadrature + ///< weight, constant) }; } // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/point3.cpp b/src/ipc/smooth_contact/primitives/point3.cpp index 2ddb6391f..35bba5b42 100644 --- a/src/ipc/smooth_contact/primitives/point3.cpp +++ b/src/ipc/smooth_contact/primitives/point3.cpp @@ -61,6 +61,21 @@ Point3::Point3( n_neighbors = local_to_global_vids.size() - 1; m_vertex_ids = local_to_global_vids; + if (m_params.use_rest_shape_measure) { + // Rest-shape vertex area measure: sum of squared rest 1-ring edge + // lengths / 3 Constant quadrature weight per the paper (Eq. 13), never + // differentiated through + const Eigen::MatrixXd& rp = mesh.rest_positions(); + m_rest_weight = 0.0; + for (int a = 0; a < edges.rows(); a++) { + const Eigen::RowVector3d t = + rp.row(local_to_global_vids[edges(a, 1)]) + - rp.row(local_to_global_vids[edges(a, 0)]); + m_rest_weight += t.squaredNorm(); + } + m_rest_weight /= 3.0; + } + if (m_vertex_ids.size() > N_VERT_NEIGHBORS_3D) { logger().error( "Too many neighbors for point3 primitive! {} > {}! Increase N_VERT_NEIGHBORS_3D in common.hpp", @@ -433,10 +448,13 @@ GradientType<-1> Point3::smooth_point3_term_gradient( Eigen::VectorXd grad_tmp = tangent_grad * normal_term + normal_grad * tangent_term; - const double weight = tangents.squaredNorm() / 3.; + const double weight = m_params.use_rest_shape_measure + ? m_rest_weight + : tangents.squaredNorm() / 3.; grad_tmp *= weight; - grad_tmp.tail(n_neighbor_dofs) += (2. / 3. * val) * tangents_vec; - + if (!m_params.use_rest_shape_measure) { + grad_tmp.tail(n_neighbor_dofs) += (2. / 3. * val) * tangents_vec; + } val *= weight; // gradient wrt. [direc, v, neighbors] @@ -489,19 +507,23 @@ HessianType<-1> Point3::smooth_point3_term_hessian( + normal_hess * tangent_term + tangent_grad * normal_grad.transpose() + normal_grad * tangent_grad.transpose(); - const double weight = tangents.squaredNorm() / 3.; + const double weight = m_params.use_rest_shape_measure + ? m_rest_weight + : tangents.squaredNorm() / 3.; hess_tmp *= weight; - hess_tmp.bottomRightCorner(n_neighbor_dofs, n_neighbor_dofs) - .diagonal() - .array() += 2. / 3. * val; - hess_tmp.bottomRows(n_neighbor_dofs) += - 2. / 3. * tangents_vec * grad_tmp.transpose(); - hess_tmp.rightCols(n_neighbor_dofs) += - 2. / 3. * grad_tmp * tangents_vec.transpose(); - + if (!m_params.use_rest_shape_measure) { + hess_tmp.bottomRightCorner(n_neighbor_dofs, n_neighbor_dofs) + .diagonal() + .array() += 2. / 3. * val; + hess_tmp.bottomRows(n_neighbor_dofs) += + 2. / 3. * tangents_vec * grad_tmp.transpose(); + hess_tmp.rightCols(n_neighbor_dofs) += + 2. / 3. * grad_tmp * tangents_vec.transpose(); + } grad_tmp *= weight; - grad_tmp.tail(n_neighbor_dofs) += (2. / 3. * val) * tangents_vec; - + if (!m_params.use_rest_shape_measure) { + grad_tmp.tail(n_neighbor_dofs) += (2. / 3. * val) * tangents_vec; + } val *= weight; // gradient wrt. [direc, v, neighbors] @@ -544,6 +566,7 @@ HessianType<-1> Point3::smooth_point3_term_hessian( template scalar Point3::smooth_point3_term( const Eigen::Matrix& X, + // NOLINTNEXTLINE(readability-named-parameter) Eigen::ConstRef> direc) const { const Eigen::RowVector3 dn = direc.normalized(); @@ -560,9 +583,13 @@ scalar Point3::smooth_point3_term( -dn.dot(t) / t.norm(), m_params.alpha_t, m_params.beta_t); } - weight = weight + t.squaredNorm(); + if (!m_params.use_rest_shape_measure) { + weight = weight + t.squaredNorm(); + } } - weight /= 3.; + + weight = + m_params.use_rest_shape_measure ? scalar(m_rest_weight) : weight / 3.; if (!orientable || otypes.normal_type(0) == HeavisideType::ONE) { normal_term = scalar(1.); diff --git a/src/ipc/smooth_contact/primitives/point3.hpp b/src/ipc/smooth_contact/primitives/point3.hpp index 015a70f00..d64185475 100644 --- a/src/ipc/smooth_contact/primitives/point3.hpp +++ b/src/ipc/smooth_contact/primitives/point3.hpp @@ -96,6 +96,8 @@ class Point3 : public Primitive { Eigen::MatrixX3i faces; Eigen::MatrixX2i edges; bool orientable; + double m_rest_weight; ///< Rest-shape vertex area measure (quadrature + ///< weight, constant) bool smooth_point3_term_type( Eigen::ConstRef X,