diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/JointSpringForceField.h b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/JointSpringForceField.h index c3d0c4e2251..c1fc4af5a97 100644 --- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/JointSpringForceField.h +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/JointSpringForceField.h @@ -39,10 +39,31 @@ template class JointSpring; -/** JointSpringForceField simulates 6D springs between Rigid DOFS - Use kst vector to specify the directionnal stiffnesses (on each local axe) - Use ksr vector to specify the rotational stiffnesses (on each local axe) -*/ +/** + * @class JointSpringForceField + * @brief A force field modeling complex joint interactions between two mechanical states. + * + * This component implements a force field that models elastic and dissipative interactions + * (springs and dampers) between pairs of points in two different MechanicalStates. + * It is primarily designed for use with Rigid3Types to handle both translation and rotation. + * + * Each joint (JointSpring) can have: + * - Different stiffness values for translation and rotation (soft and hard stiffness). + * - Damping factor for both translational and rotational velocities. + * - Angular limits for each rotation axis (X, Y, Z). + * - A 'blocking' stiffness applied when rotation limits are exceeded. + * - Initial translation and rotation offsets. + * + * The force computation accounts for: + * - Constant external forces/torques read from an input file. + * - Relative position and orientation between the joint points. + * - Relative velocities for damping. + * - Joint limits and blocking constraints. + * + * It also supports logging joint data to an output file at specified intervals. + * + * @tparam DataTypes The type of data used for the mechanical states (usually Rigid3Types). + */ template class JointSpringForceField : public core::behavior::PairInteractionForceField { @@ -123,6 +144,8 @@ class JointSpringForceField : public core::behavior::PairInteractionForceField #include #include +#include #include namespace sofa::component::solidmechanics::spring @@ -358,6 +359,58 @@ void JointSpringForceField::addDForce(const core::MechanicalParams *m data_df2.endEdit(); } +template +void JointSpringForceField::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) +{ + auto state1 = this->mstate1.get(); + auto state2 = this->mstate2.get(); + if (!state1 || !state2) return; + + auto df1_dx1 = matrix->getForceDerivativeIn(state1).withRespectToPositionsIn(state1); + auto df1_dx2 = matrix->getForceDerivativeIn(state1).withRespectToPositionsIn(state2); + auto df2_dx1 = matrix->getForceDerivativeIn(state2).withRespectToPositionsIn(state1); + auto df2_dx2 = matrix->getForceDerivativeIn(state2).withRespectToPositionsIn(state2); + + df1_dx1.checkValidity(this); + df1_dx2.checkValidity(this); + df2_dx1.checkValidity(this); + df2_dx2.checkValidity(this); + + const auto springsAccessor = sofa::helper::getReadAccessor(d_springs); + for (const auto& spring : springsAccessor) + { + Mat mT; + for (sofa::Index j = 0; j < 3; ++j) + { + mT(j, j) = spring.KT[j]; + } + + Mat mR; + for (sofa::Index j = 0; j < 3; ++j) + { + mR(j, j) = spring.KR[j]; + } + + Mat rot(sofa::type::NOINIT); + spring.ref.toMatrix(rot); + const Mat rotInv = rot.transposed(); + + //translation + const Mat worldKT = rot * mT * rotInv; + df1_dx1(spring.m1, spring.m1) += worldKT; + df1_dx2(spring.m1, spring.m2) += -worldKT; + df2_dx1(spring.m2, spring.m1) += -worldKT; + df2_dx2(spring.m2, spring.m2) += worldKT; + + //rotation + const Mat worldKR = rot * mR * rotInv; + df1_dx1(spring.m1 + N, spring.m1 + N) += worldKR; + df1_dx2(spring.m1 + N, spring.m2 + N) += -worldKR; + df2_dx1(spring.m2 + N, spring.m1 + N) += -worldKR; + df2_dx2(spring.m2 + N, spring.m2 + N) += worldKR; + } +} + template void JointSpringForceField::buildDampingMatrix(core::behavior::DampingMatrix*) {