From 36f7ec79b7a25e0e99f3a10245ca632af27fedea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Fri, 18 Oct 2024 22:35:16 +0200 Subject: [PATCH 1/2] Fix collision cantact event --- src/Mandos/Core/Mappings/CollisionMapping.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Mandos/Core/Mappings/CollisionMapping.cpp b/src/Mandos/Core/Mappings/CollisionMapping.cpp index 7452f683..ddd9d625 100644 --- a/src/Mandos/Core/Mappings/CollisionMapping.cpp +++ b/src/Mandos/Core/Mappings/CollisionMapping.cpp @@ -69,11 +69,11 @@ void MappingInfo::apply(const std::vector &fro void MappingInfo::applyJ(const std::vector &from, std::vector &to) const { ZoneScopedN("MappingInfo.applyJ"); - to[m_toId] = from[m_fromId]; + to[m_toId] += from[m_fromId]; } void MappingInfo::applyJT(std::vector &from, const std::vector &to) const { ZoneScopedN("MappingInfo.applyJT"); from[m_fromId] += to[m_toId]; } -} // namespace mandos::core::collisions \ No newline at end of file +} // namespace mandos::core::collisions -- GitLab From cd14c19b23def5eb9755d5124c8b39774b5fb51f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mag=C3=AD=20Romany=C3=A0?= Date: Mon, 21 Oct 2024 12:35:59 +0200 Subject: [PATCH 2/2] Fix rod stiffness scaling and gradient bug --- .../Core/Energies/CosseratRodEnergy.cpp | 42 +++++++++++-------- 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/src/Mandos/Core/Energies/CosseratRodEnergy.cpp b/src/Mandos/Core/Energies/CosseratRodEnergy.cpp index dd5c5afb..dfec8163 100644 --- a/src/Mandos/Core/Energies/CosseratRodEnergy.cpp +++ b/src/Mandos/Core/Energies/CosseratRodEnergy.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include namespace @@ -81,6 +82,8 @@ struct CosseratRodPrecomputation { this->R = 0.5 * (RA + RB); this->C = (-u - R.col(2)); + this->hessVs = one_over_L0 * one_over_L * ((L - L0) * Mat3::Identity() + L0 * uut); + TinyAD::project_positive_definite(hessVs, 1e-8); } Scalar one_over_L0; // One over the rest length @@ -95,6 +98,7 @@ struct CosseratRodPrecomputation { Mat3 uut; // Projection matrix onto direction u Mat3 RA, RB, R; // Rotation of the two Rigid Bodies Vec3 C; // Constraint to align the third director d3 with u direction + Mat3 hessVs; // Constraint to align the third director d3 with u direction }; Scalar computeCosseratRodEnergy(const CosseratRodPrecomputation &values, @@ -103,19 +107,20 @@ Scalar computeCosseratRodEnergy(const CosseratRodPrecomputation &values, Scalar cosserat_stiffness, const Vec3 &intrinsic_darboux, const Vec3 &stiffness_tensor, - Scalar translation_damping, + Scalar /*translation_damping*/, Scalar /*rotation_damping*/) { // Potential energies // Stretch - const Scalar Vs = 0.5 * Ks * (values.L - L0) * (values.L - L0); + const Scalar Vs = 0.5 * values.one_over_L0 * Ks * (values.L - L0) * (values.L - L0); const Vec3 deltaU = values.darboux_vector - intrinsic_darboux; - const Scalar Vb = 0.5 * deltaU.transpose() * stiffness_tensor.asDiagonal() * deltaU; + const Scalar Vb = 0.5 * L0 * deltaU.transpose() * stiffness_tensor.asDiagonal() * deltaU; // Dissipation // Translational dissipation - const Scalar Dt = 0.5 * L0 * translation_damping * values.v_rel.dot(values.v_rel); + // const Scalar Dt = 0.5 * L0 * translation_damping * values.v_rel.dot(values.v_rel); + const Scalar Dt = 0.0; // TODO Rotational dissipation const Scalar Dr = 0.0; @@ -130,19 +135,20 @@ inline Vec3 computeCosseratRodLinearGradient(const CosseratRodPrecomputation &va Scalar Ks, Scalar L0, Scalar cosserat_stiffness, - Scalar translation_damping) + Scalar /*translation_damping*/) { // Potential energies // Stretch - const Vec3 gradVs = Ks * (values.L - L0) * values.u; + const Vec3 gradVs = Ks * values.one_over_L0 * (values.L - L0) * values.u; // Dissipation // Translational dissipation - const Vec3 gradDt = translation_damping * values.uut * (values.vA - values.vB); + // const Vec3 gradDt = translation_damping * values.uut * (values.vA - values.vB); + const Vec3 gradDt = Vec3::Zero(); // Constraint energy - const Mat3 dL_dx = -(Mat3::Identity() - values.uut) * values.one_over_L; - const Vec3 gradEp_dx = cosserat_stiffness * L0 * values.C.transpose() * dL_dx; + const Mat3 du_dx = (Mat3::Identity() - values.uut) * values.one_over_L; + const Vec3 gradEp_dx = cosserat_stiffness * L0 * values.C.transpose() * (-du_dx); return gradVs + gradDt + gradEp_dx; } @@ -155,7 +161,7 @@ inline Vec3 computeCosseratRodRotationGradientA(const CosseratRodPrecomputation Scalar /*rotation_damping*/) { const Vec3 deltaU = values.darboux_vector - intrinsic_darboux; - const Vec3 gradVb = deltaU.transpose() * stiffness_tensor.asDiagonal() * values.darboux_vector_derivativeA; + const Vec3 gradVb = L0 * deltaU.transpose() * stiffness_tensor.asDiagonal() * values.darboux_vector_derivativeA; // Dissipation // TODO Rotational dissipation @@ -177,7 +183,7 @@ inline Vec3 computeCosseratRodRotationGradientB(const CosseratRodPrecomputation { const Vec3 deltaU = values.darboux_vector - intrinsic_darboux; const Mat3 du_dtheta = values.darboux_vector_derivativeB; - const Vec3 gradVb = deltaU.transpose() * stiffness_tensor.asDiagonal() * du_dtheta; + const Vec3 gradVb = L0 * deltaU.transpose() * stiffness_tensor.asDiagonal() * du_dtheta; // Dissipation // TODO Rotational dissipation @@ -201,11 +207,11 @@ inline Mat6 computeCosseratRodHessianA(const CosseratRodPrecomputation &values, { // Potential energies // Stretch - const Mat3 hessVs = Ks * values.one_over_L * ((values.L - L0) * Mat3::Identity() + L0 * values.uut); + const Mat3 hessVs = Ks * values.hessVs; const Mat3 du_dtheta = values.darboux_vector_derivativeA; // Here we are aproximating the hessian! - const Mat3 hessVb = du_dtheta.transpose() * stiffness_tensor.asDiagonal() * du_dtheta; + const Mat3 hessVb = L0 * du_dtheta.transpose() * stiffness_tensor.asDiagonal() * du_dtheta; // Dissipation // Translational dissipation @@ -243,10 +249,10 @@ inline Mat6 computeCosseratRodHessianB(const CosseratRodPrecomputation &values, { // Potential energies // Stretch - const Mat3 hessVs = Ks * values.one_over_L * ((values.L - L0) * Mat3::Identity() + L0 * values.uut); + const Mat3 hessVs = Ks * values.hessVs; const Mat3 du_dtheta = values.darboux_vector_derivativeB; - const Mat3 hessVb = du_dtheta.transpose() * stiffness_tensor.asDiagonal() * du_dtheta; + const Mat3 hessVb = L0 * du_dtheta.transpose() * stiffness_tensor.asDiagonal() * du_dtheta; // Dissipation // Translational dissipation @@ -284,13 +290,13 @@ inline Mat6 computeCosseratRodHessianAB(const CosseratRodPrecomputation &values, { // Potential energies // Stretch - const Mat3 hessVs = -Ks / values.L * ((values.L - L0) * Mat3::Identity() + L0 * values.uut); + const Mat3 hessVs = -Ks * values.hessVs; // REVIEW Bending const Mat3 &du_dthetaA = values.darboux_vector_derivativeA; const Mat3 &du_dthetaB = values.darboux_vector_derivativeB; // Here we are aproximating the hessian! - const Mat3 hessVb = du_dthetaA.transpose() * stiffness_tensor.asDiagonal() * du_dthetaB; + const Mat3 hessVb = L0 * du_dthetaA.transpose() * stiffness_tensor.asDiagonal() * du_dthetaB; // Dissipation // Translational dissipation @@ -410,7 +416,7 @@ Scalar CosseratRodEnergy::computeEnergyAndGradient(MechanicalState mstate.m_grad[iA].segment<3>(3) += rotationGradientA; mstate.m_grad[iB].segment<3>(0) += -linearGradient; - mstate.m_grad[iA].segment<3>(3) += rotationGradientB; + mstate.m_grad[iB].segment<3>(3) += rotationGradientB; } return energy; } -- GitLab