diff --git a/src/ipc/collisions/collision.cpp b/src/ipc/collisions/collision.cpp index d54d1f182..0b06cf490 100644 --- a/src/ipc/collisions/collision.cpp +++ b/src/ipc/collisions/collision.cpp @@ -24,28 +24,27 @@ double Collision::mollifier( return 1.0; } template -Vector::max_size> -Collision::mollifier_gradient( +auto Collision::mollifier_gradient( const Vector::max_size>& positions) const + -> Vector { return Vector::max_size>::Zero( positions.size()); } template -Vector::max_size> -Collision::mollifier_gradient( +auto Collision::mollifier_gradient( const Vector::max_size>& positions, - double eps_x) const + double eps_x) const -> Vector { return Vector::max_size>::Zero( positions.size()); } template -MatrixMax::max_size, Collision::max_size> -Collision::mollifier_hessian( +auto Collision::mollifier_hessian( const Vector::max_size>& positions) const + -> MatrixMax { return MatrixMax< double, Collision::max_size, Collision::max_size>:: @@ -53,10 +52,9 @@ Collision::mollifier_hessian( } template -MatrixMax::max_size, Collision::max_size> -Collision::mollifier_hessian( +auto Collision::mollifier_hessian( const Vector::max_size>& positions, - double eps_x) const + double eps_x) const -> MatrixMax { return MatrixMax< double, Collision::max_size, Collision::max_size>:: diff --git a/src/ipc/smooth_contact/collisions/smooth_collision.cpp b/src/ipc/smooth_contact/collisions/smooth_collision.cpp index 288b1b297..7981af61e 100644 --- a/src/ipc/smooth_contact/collisions/smooth_collision.cpp +++ b/src/ipc/smooth_contact/collisions/smooth_collision.cpp @@ -2,32 +2,44 @@ namespace ipc { -template -CollisionType SmoothCollisionTemplate::type() const +template +CollisionType +SmoothCollisionTemplate::type() const { - if constexpr (std::is_same_v && std::is_same_v) + if constexpr ( + std::is_same_v && std::is_same_v) return CollisionType::EdgeVertex; - if constexpr (std::is_same_v && std::is_same_v) + if constexpr ( + std::is_same_v + && std::is_same_v) return CollisionType::VertexVertex; - if constexpr (std::is_same_v && std::is_same_v) + if constexpr ( + std::is_same_v && std::is_same_v) return CollisionType::FaceVertex; - if constexpr (std::is_same_v && std::is_same_v) + if constexpr ( + std::is_same_v && std::is_same_v) return CollisionType::EdgeVertex; - if constexpr (std::is_same_v && std::is_same_v) + if constexpr ( + std::is_same_v && std::is_same_v) return CollisionType::EdgeEdge; - if constexpr (std::is_same_v && std::is_same_v) + if constexpr ( + std::is_same_v + && std::is_same_v) return CollisionType::VertexVertex; throw std::runtime_error("Invalid collision pair type!"); return CollisionType::VertexVertex; } -template Vector::n_core_dofs> -SmoothCollisionTemplate::get_core_indices() const +template +auto SmoothCollisionTemplate:: + get_core_indices() const -> Vector { Vector core_indices; - core_indices << Eigen::VectorXi::LinSpaced(n_core_dofs_A, 0, n_core_dofs_A - 1), - Eigen::VectorXi::LinSpaced(n_core_dofs_B, pA->n_dofs(), pA->n_dofs() + n_core_dofs_B - 1); + core_indices << Eigen::VectorXi::LinSpaced( + n_core_dofs_A, 0, n_core_dofs_A - 1), + Eigen::VectorXi::LinSpaced( + n_core_dofs_B, pA->n_dofs(), pA->n_dofs() + n_core_dofs_B - 1); return core_indices; } @@ -46,11 +58,9 @@ SmoothCollisionTemplate:: VectorMax3d d = PrimitiveDistance::compute_closest_direction( mesh, V, primitive0_, primitive1_, dtype); - pA = std::make_unique( - primitive0_, mesh, V, d, param); - pB = std::make_unique( - primitive1_, mesh, V, -d, param); - + pA = std::make_unique(primitive0_, mesh, V, d, param); + pB = std::make_unique(primitive1_, mesh, V, -d, param); + if (pA->n_vertices() + pB->n_vertices() > max_vert) logger().error( "Too many neighbors for collision pair! {} > {}! Increase max_vert_3d in common.hpp", @@ -66,8 +76,10 @@ SmoothCollisionTemplate:: (d.norm() < Super::get_dhat()) && pA->is_active() && pB->is_active(); if (d.norm() < 1e-12) - logger().warn("pair distance {}, id {} and {}, dtype {}, active {}", d.norm(), primitive0_, - primitive1_, PrimitiveDistType::name, Super::is_active_); + logger().warn( + "pair distance {}, id {} and {}, dtype {}, active {}", d.norm(), + primitive0_, primitive1_, + PrimitiveDistType::name, Super::is_active_); } template @@ -96,43 +108,44 @@ double SmoothCollisionTemplate::operator()( double a4 = PrimitiveDistanceTemplate::mollifier( x, dist * dist); - + if (dist < 1e-12) - logger().warn("pair distance {:.3e}, barrier {:.3e}, mollifier {:.3e}, orient {:.3e} {:.3e}", dist, a3, a4, a1, a2); + logger().warn( + "pair distance {:.3e}, barrier {:.3e}, mollifier {:.3e}, orient {:.3e} {:.3e}", + dist, a3, a4, a1, a2); return a1 * a2 * a3 * a4; } template -Vector< - double, - -1, - SmoothCollisionTemplate::max_size> -SmoothCollisionTemplate::gradient( +auto SmoothCollisionTemplate::gradient( const Vector< double, -1, SmoothCollisionTemplate::max_size>& positions, - const ParameterType& params) const + const ParameterType& params) const -> Vector { const auto core_indices = get_core_indices(); Vector x; x = positions(core_indices); - const auto dtype = PrimitiveDistance::compute_distance_type(x); - + const auto dtype = + PrimitiveDistance::compute_distance_type(x); + Vector closest_direction; Eigen::Matrix closest_direction_grad; - std::tie(closest_direction, closest_direction_grad) = PrimitiveDistance::compute_closest_direction_gradient(x, dtype); - + std::tie(closest_direction, closest_direction_grad) = PrimitiveDistance< + PrimitiveA, PrimitiveB>::compute_closest_direction_gradient(x, dtype); + const double dist = closest_direction.norm(); assert(dist > 0); // these two use autodiff with different variable count auto gA_reduced = pA->grad(closest_direction, positions.head(pA->n_dofs())); - auto gB_reduced = pB->grad(-closest_direction, positions.tail(pB->n_dofs())); + auto gB_reduced = + pB->grad(-closest_direction, positions.tail(pB->n_dofs())); // gradient of barrier potential double barrier = 0; @@ -153,12 +166,14 @@ SmoothCollisionTemplate::gradient( // gradient of mollifier { double mollifier = 0; - Vector gMollifier = Vector::Zero(); + Vector gMollifier = + Vector::Zero(); #ifdef DERIVATIVES_WITH_AUTODIFF DiffScalarBase::setVariableCount(n_core_dofs); using T = ADGrad; Vector xAD = slice_positions(x); - Vector closest_direction_autodiff = PrimitiveDistanceTemplate::compute_closest_direction(xAD, dtype); + Vector closest_direction_autodiff = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_closest_direction(xAD, dtype); const auto dist_sqr_AD = closest_direction_autodiff.squaredNorm(); auto mollifier_autodiff = PrimitiveDistanceTemplate::mollifier( @@ -167,11 +182,13 @@ SmoothCollisionTemplate::gradient( gMollifier = mollifier_autodiff.getGradient(); #else Vector mollifier_grad; - std::tie(mollifier, mollifier_grad) = - PrimitiveDistance::compute_mollifier_gradient(x, dist * dist); + std::tie(mollifier, mollifier_grad) = PrimitiveDistance< + PrimitiveA, PrimitiveB>::compute_mollifier_gradient(x, dist * dist); - const Vector dist_sqr_grad = 2 * closest_direction_grad.transpose() * closest_direction; - mollifier_grad.head(n_core_dofs) += mollifier_grad(n_core_dofs) * dist_sqr_grad; + const Vector dist_sqr_grad = + 2 * closest_direction_grad.transpose() * closest_direction; + mollifier_grad.head(n_core_dofs) += + mollifier_grad(n_core_dofs) * dist_sqr_grad; gMollifier = mollifier_grad.head(n_core_dofs); #endif // merge mollifier into barrier @@ -183,12 +200,17 @@ SmoothCollisionTemplate::gradient( double orient = 0; Vector gOrient; { - Vector gA = Vector::Zero(n_dofs()), gB = Vector::Zero(n_dofs()); + Vector gA = Vector::Zero( + n_dofs()), + gB = Vector::Zero( + n_dofs()); { - gA(core_indices) = closest_direction_grad.transpose() * gA_reduced.head(dim); + gA(core_indices) = + closest_direction_grad.transpose() * gA_reduced.head(dim); gA.head(pA->n_dofs()) += gA_reduced.tail(pA->n_dofs()); - gB(core_indices) = closest_direction_grad.transpose() * -gB_reduced.head(dim); + gB(core_indices) = + closest_direction_grad.transpose() * -gB_reduced.head(dim); gB.tail(pB->n_dofs()) += gB_reduced.tail(pB->n_dofs()); } const double potential_a = @@ -209,42 +231,47 @@ SmoothCollisionTemplate::gradient( } template -MatrixMax< - double, - SmoothCollisionTemplate::max_size, - SmoothCollisionTemplate::max_size> -SmoothCollisionTemplate::hessian( +auto SmoothCollisionTemplate::hessian( const Vector< double, -1, SmoothCollisionTemplate::max_size>& positions, - const ParameterType& params) const + const ParameterType& params) const -> MatrixMax { const auto core_indices = get_core_indices(); Vector x; x = positions(core_indices); - const auto dtype = PrimitiveDistance::compute_distance_type(x); - + const auto dtype = + PrimitiveDistance::compute_distance_type(x); + Vector closest_direction; Eigen::Matrix closest_direction_grad; - std::array, dim> closest_direction_hess; - std::tie(closest_direction, closest_direction_grad, closest_direction_hess) = PrimitiveDistance::compute_closest_direction_hessian(x, dtype); - + std::array, dim> + closest_direction_hess; + std::tie( + closest_direction, closest_direction_grad, closest_direction_hess) = + PrimitiveDistance:: + compute_closest_direction_hessian(x, dtype); + const double dist = closest_direction.norm(); // these two use autodiff with different variable count auto gA_reduced = pA->grad(closest_direction, positions.head(pA->n_dofs())); - auto hA_reduced = pA->hessian(closest_direction, positions.head(pA->n_dofs())); - auto gB_reduced = pB->grad(-closest_direction, positions.tail(pB->n_dofs())); - auto hB_reduced = pB->hessian(-closest_direction, positions.tail(pB->n_dofs())); + auto hA_reduced = + pA->hessian(closest_direction, positions.head(pA->n_dofs())); + auto gB_reduced = + pB->grad(-closest_direction, positions.tail(pB->n_dofs())); + auto hB_reduced = + pB->hessian(-closest_direction, positions.tail(pB->n_dofs())); // hessian of barrier potential double barrier = 0; Vector gBarrier = Vector::Zero(); - Eigen::Matrix hBarrier = Eigen::Matrix::Zero(); + Eigen::Matrix hBarrier = + Eigen::Matrix::Zero(); { barrier = Math::inv_barrier(dist / Super::get_dhat(), params.r); @@ -275,13 +302,16 @@ SmoothCollisionTemplate::hessian( // hessian of mollifier { double mollifier = 0; - Vector gMollifier = Vector::Zero(); - Eigen::Matrix hMollifier = Eigen::Matrix::Zero(); + Vector gMollifier = + Vector::Zero(); + Eigen::Matrix hMollifier = + Eigen::Matrix::Zero(); #ifdef DERIVATIVES_WITH_AUTODIFF DiffScalarBase::setVariableCount(n_core_dofs); using T = ADHessian; Vector xAD = slice_positions(x); - Vector closest_direction_autodiff = PrimitiveDistanceTemplate::compute_closest_direction(xAD, dtype); + Vector closest_direction_autodiff = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_closest_direction(xAD, dtype); const auto dist_sqr_AD = closest_direction_autodiff.squaredNorm(); auto mollifier_autodiff = PrimitiveDistanceTemplate::mollifier( @@ -293,25 +323,34 @@ SmoothCollisionTemplate::hessian( #else Vector mollifier_grad; Eigen::Matrix mollifier_hess; - std::tie(mollifier, mollifier_grad, mollifier_hess) = - PrimitiveDistance::compute_mollifier_hessian(x, dist * dist); - - const Vector dist_sqr_grad = 2 * closest_direction_grad.transpose() * closest_direction; - mollifier_grad.head(n_core_dofs) += mollifier_grad(n_core_dofs) * dist_sqr_grad; - Eigen::Matrix dist_sqr_hess = 2 * closest_direction_grad.transpose() * closest_direction_grad; + std::tie(mollifier, mollifier_grad, mollifier_hess) = PrimitiveDistance< + PrimitiveA, PrimitiveB>::compute_mollifier_hessian(x, dist * dist); + + const Vector dist_sqr_grad = + 2 * closest_direction_grad.transpose() * closest_direction; + mollifier_grad.head(n_core_dofs) += + mollifier_grad(n_core_dofs) * dist_sqr_grad; + Eigen::Matrix dist_sqr_hess = + 2 * closest_direction_grad.transpose() * closest_direction_grad; for (int d = 0; d < dim; d++) - dist_sqr_hess += 2 * closest_direction(d) * closest_direction_hess[d]; + dist_sqr_hess += + 2 * closest_direction(d) * closest_direction_hess[d]; mollifier_hess.topLeftCorner(n_core_dofs, n_core_dofs) += dist_sqr_hess * mollifier_grad(n_core_dofs) - + dist_sqr_grad * mollifier_hess(n_core_dofs, n_core_dofs) * dist_sqr_grad.transpose() - + dist_sqr_grad * mollifier_hess.block(n_core_dofs, 0, 1, n_core_dofs) - + mollifier_hess.block(0, n_core_dofs, n_core_dofs, 1) * dist_sqr_grad.transpose(); + + dist_sqr_grad * mollifier_hess(n_core_dofs, n_core_dofs) + * dist_sqr_grad.transpose() + + dist_sqr_grad + * mollifier_hess.block(n_core_dofs, 0, 1, n_core_dofs) + + mollifier_hess.block(0, n_core_dofs, n_core_dofs, 1) + * dist_sqr_grad.transpose(); gMollifier = mollifier_grad.head(core_indices.size()); - hMollifier = mollifier_hess.topLeftCorner(core_indices.size(), core_indices.size()); + hMollifier = mollifier_hess.topLeftCorner( + core_indices.size(), core_indices.size()); #endif // merge mollifier into barrier - hBarrier = hBarrier * mollifier + gBarrier * gMollifier.transpose() + gMollifier * gBarrier.transpose() + hMollifier * barrier; + hBarrier = hBarrier * mollifier + gBarrier * gMollifier.transpose() + + gMollifier * gBarrier.transpose() + hMollifier * barrier; gBarrier = gBarrier * mollifier + gMollifier * barrier; barrier *= mollifier; } @@ -321,35 +360,49 @@ SmoothCollisionTemplate::hessian( Vector gOrient; MatrixMax hOrient; { - Vector gA = Vector::Zero(n_dofs()), gB = Vector::Zero(n_dofs()); - MatrixMax hA = MatrixMax::Zero(n_dofs(), n_dofs()), hB = MatrixMax::Zero(n_dofs(), n_dofs()); + Vector gA = Vector::Zero( + n_dofs()), + gB = Vector::Zero( + n_dofs()); + MatrixMax + hA = + MatrixMax::Zero(n_dofs(), n_dofs()), + hB = + MatrixMax::Zero(n_dofs(), n_dofs()); { - gA(core_indices) = closest_direction_grad.transpose() * gA_reduced.head(dim); + gA(core_indices) = + closest_direction_grad.transpose() * gA_reduced.head(dim); gA.head(pA->n_dofs()) += gA_reduced.tail(pA->n_dofs()); - hA(core_indices, core_indices) = closest_direction_grad.transpose() * hA_reduced.topLeftCorner(dim, dim) * closest_direction_grad; + hA(core_indices, core_indices) = closest_direction_grad.transpose() + * hA_reduced.topLeftCorner(dim, dim) * closest_direction_grad; for (int d = 0; d < dim; d++) - hA(core_indices, core_indices) += gA_reduced(d) * closest_direction_hess[d]; + hA(core_indices, core_indices) += + gA_reduced(d) * closest_direction_hess[d]; hA.topLeftCorner(pA->n_dofs(), pA->n_dofs()) += hA_reduced.bottomRightCorner(pA->n_dofs(), pA->n_dofs()); - hA(core_indices, Eigen::seqN(0, pA->n_dofs())) += closest_direction_grad.transpose() + hA(core_indices, Eigen::seqN(0, pA->n_dofs())) += + closest_direction_grad.transpose() * hA_reduced.topRightCorner(dim, pA->n_dofs()); hA(Eigen::seqN(0, pA->n_dofs()), core_indices) += hA_reduced.bottomLeftCorner(pA->n_dofs(), dim) * closest_direction_grad; - gB(core_indices) = closest_direction_grad.transpose() * -gB_reduced.head(dim); + gB(core_indices) = + closest_direction_grad.transpose() * -gB_reduced.head(dim); gB.tail(pB->n_dofs()) += gB_reduced.tail(pB->n_dofs()); - hB(core_indices, core_indices) = closest_direction_grad.transpose() * hB_reduced.topLeftCorner(dim, dim) - * closest_direction_grad; + hB(core_indices, core_indices) = closest_direction_grad.transpose() + * hB_reduced.topLeftCorner(dim, dim) * closest_direction_grad; for (int d = 0; d < dim; d++) - hB(core_indices, core_indices) -= gB_reduced(d) * closest_direction_hess[d]; + hB(core_indices, core_indices) -= + gB_reduced(d) * closest_direction_hess[d]; hB.bottomRightCorner(pB->n_dofs(), pB->n_dofs()) += hB_reduced.bottomRightCorner(pB->n_dofs(), pB->n_dofs()); - hB(core_indices, Eigen::seqN(pA->n_dofs(), pB->n_dofs())) -= closest_direction_grad.transpose() + hB(core_indices, Eigen::seqN(pA->n_dofs(), pB->n_dofs())) -= + closest_direction_grad.transpose() * hB_reduced.topRightCorner(dim, pB->n_dofs()); hB(Eigen::seqN(pA->n_dofs(), pB->n_dofs()), core_indices) -= hB_reduced.bottomLeftCorner(pB->n_dofs(), dim) @@ -403,17 +456,13 @@ SmoothCollisionTemplate::compute_distance( } template -Vector< - double, - -1, - SmoothCollisionTemplate::max_size> -SmoothCollisionTemplate:: +auto SmoothCollisionTemplate:: compute_distance_gradient( const Vector< double, -1, SmoothCollisionTemplate:: - max_size>& positions) const + max_size>& positions) const -> Vector { Vector x; x << positions.head(PrimitiveA::n_core_points * dim), @@ -421,23 +470,22 @@ SmoothCollisionTemplate:: Vector closest_direction; Eigen::Matrix closest_direction_grad; - std::tie(closest_direction, closest_direction_grad) = PrimitiveDistance::compute_closest_direction_gradient(x, DTYPE::AUTO); + std::tie(closest_direction, closest_direction_grad) = + PrimitiveDistance:: + compute_closest_direction_gradient(x, DTYPE::AUTO); return 2 * closest_direction_grad.transpose() * closest_direction; } template -MatrixMax< - double, - SmoothCollisionTemplate::max_size, - SmoothCollisionTemplate::max_size> -SmoothCollisionTemplate:: +auto SmoothCollisionTemplate:: compute_distance_hessian( const Vector< double, -1, SmoothCollisionTemplate:: max_size>& positions) const + -> MatrixMax { Vector x; x << positions.head(PrimitiveA::n_core_points * dim), @@ -445,20 +493,25 @@ SmoothCollisionTemplate:: Vector closest_direction; Eigen::Matrix closest_direction_grad; - std::array, dim> closest_direction_hess; - std::tie(closest_direction, closest_direction_grad, closest_direction_hess) = PrimitiveDistance::compute_closest_direction_hessian(x, DTYPE::AUTO); - - Eigen::Matrix dist_sqr_hess = 2 * closest_direction_grad.transpose() * closest_direction_grad; + std::array, dim> + closest_direction_hess; + std::tie( + closest_direction, closest_direction_grad, closest_direction_hess) = + PrimitiveDistance:: + compute_closest_direction_hessian(x, DTYPE::AUTO); + + Eigen::Matrix dist_sqr_hess = + 2 * closest_direction_grad.transpose() * closest_direction_grad; for (int d = 0; d < dim; d++) dist_sqr_hess += 2 * closest_direction(d) * closest_direction_hess[d]; - + return dist_sqr_hess; } template -std::array::n_core_dofs> -SmoothCollisionTemplate:: -core_vertex_ids(const Eigen::MatrixXi& edges, const Eigen::MatrixXi& faces) const +auto SmoothCollisionTemplate::core_vertex_ids( + const Eigen::MatrixXi& edges, const Eigen::MatrixXi& faces) const + -> std::array { std::array vids; auto ids = get_core_indices();