diff --git a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h index a02f33025..c6e47c193 100644 --- a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h +++ b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h @@ -140,6 +140,10 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass void addMToMatrix(const MechanicalParams *mparams, const MultiMatrixAccessor* matrix) override; void addMBKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix) override; + void buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) override; + void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; + void buildDampingMatrix(core::behavior::DampingMatrix* matrices) override; + //TODO(dmarchal 2017-05-17) So what do we do ? For who is this message intended for ? How can we make this code "more" manageable. void accFromF(const MechanicalParams* mparams, DataVecDeriv& , const DataVecDeriv& ) override { diff --git a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl index 6f828f3e6..c124ff59d 100644 --- a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl +++ b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl @@ -32,6 +32,9 @@ // #pragma once +#include +#include + #include #include #include @@ -362,6 +365,32 @@ void AdaptiveBeamForceFieldAndMass::addMToMatrix(const MechanicalPara } } +template +void AdaptiveBeamForceFieldAndMass::buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) +{ + const unsigned int numBeams = l_interpolation->getNumBeams(); + + + for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); + + /// matrices in global frame + const Matrix6x6 M00 = bLM.m_A0Ref.multTranspose(bLM.m_M00 * bLM.m_A0Ref); + const Matrix6x6 M01 = bLM.m_A0Ref.multTranspose(bLM.m_M01 * bLM.m_A1Ref); + const Matrix6x6 M10 = bLM.m_A1Ref.multTranspose(bLM.m_M10 * bLM.m_A0Ref); + const Matrix6x6 M11 = bLM.m_A1Ref.multTranspose(bLM.m_M11 * bLM.m_A1Ref); + + + matrices->add(node0Idx * 6, node0Idx * 6, M00); + matrices->add(node0Idx * 6, node1Idx * 6, M01); + matrices->add(node1Idx * 6, node0Idx * 6, M10); + matrices->add(node1Idx * 6, node1Idx * 6, M11); + } +} + template void AdaptiveBeamForceFieldAndMass::addMBKToMatrix(const MechanicalParams* mparams, @@ -426,6 +455,12 @@ void AdaptiveBeamForceFieldAndMass::addMBKToMatrix(const MechanicalPa } } +template +void AdaptiveBeamForceFieldAndMass::buildDampingMatrix(core::behavior::DampingMatrix*) +{ + // No damping in this ForceField +} + ///////////////////////////////////// /// ForceField Interface @@ -685,6 +720,37 @@ void AdaptiveBeamForceFieldAndMass::addKToMatrix(const MechanicalPara } } +template +void AdaptiveBeamForceFieldAndMass::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) +{ + const unsigned int numBeams = l_interpolation->getNumBeams(); + + + auto dfdx = matrix->getForceDerivativeIn(this->mstate) + .withRespectToPositionsIn(this->mstate); + + for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); + + if(node0Idx==node1Idx) + continue; + + // matrices in global frame + Matrix6x6 K00 = beamLocalMatrix.m_A0Ref.multTranspose(beamLocalMatrix.m_K00 * beamLocalMatrix.m_A0Ref); + Matrix6x6 K01 = beamLocalMatrix.m_A0Ref.multTranspose(beamLocalMatrix.m_K01 * beamLocalMatrix.m_A1Ref); + Matrix6x6 K10 = beamLocalMatrix.m_A1Ref.multTranspose(beamLocalMatrix.m_K10 * beamLocalMatrix.m_A0Ref); + Matrix6x6 K11 = beamLocalMatrix.m_A1Ref.multTranspose(beamLocalMatrix.m_K11 * beamLocalMatrix.m_A1Ref); + + + dfdx(node0Idx*6, node0Idx*6) += - K00; + dfdx(node0Idx*6, node1Idx*6) += - K01; + dfdx(node1Idx*6, node0Idx*6) += - K10; + dfdx(node1Idx*6, node1Idx*6) += - K11; + } +} /////////////////////////////////////