diff --git a/src/Mandos/Core/Energies/CosseratRodEnergy.cpp b/src/Mandos/Core/Energies/CosseratRodEnergy.cpp index dd5c5afb2b7cf64afa1d07b1158f02a49ed80ac3..dfec8163a557c2269fedb7e64577b85070c23752 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; } diff --git a/src/Mandos/Core/Mappings/CollisionMapping.cpp b/src/Mandos/Core/Mappings/CollisionMapping.cpp index 7452f683c04ba2b7784686180ee103dbf3dc9127..ddd9d625584aea94c608f1e177683e6d6d59870a 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