From 653090265168dc3e8cd53d60cdc9473fd587314a Mon Sep 17 00:00:00 2001 From: Eulalie Coevoet Date: Fri, 15 Dec 2023 11:36:31 +0100 Subject: [PATCH] [forcefield] add template vec6types to BeamHookeLawForceField --- .../forcefield/BeamHookeLawForceField.cpp | 137 ++++++++++++++++++ .../forcefield/BeamHookeLawForceField.h | 11 +- 2 files changed, 146 insertions(+), 2 deletions(-) diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.cpp b/src/Cosserat/forcefield/BeamHookeLawForceField.cpp index aa6b94e1..a5715a89 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.cpp +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.cpp @@ -35,6 +35,141 @@ namespace sofa::component::forcefield { +template<> +void BeamHookeLawForceField::reinit() +{ + // Precompute and store values + Real Iy, Iz, J, A; + if ( d_crossSectionShape.getValue().getSelectedItem() == "rectangular") //rectangular cross-section + { + Real Ly = d_lengthY.getValue(); + Real Lz = d_lengthZ.getValue(); + + const Real LyLzLzLz = Ly * Lz * Lz * Lz; + const Real LzLyLyLy = Lz * Ly * Ly * Ly; + + Iy = LyLzLzLz / 12.0; + Iz = LzLyLyLy / 12.0; + J = Iy + Iz; + A = Ly * Lz; + + } + else //circular cross-section + { + msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem() ; + + Real r = d_radius.getValue(); + Real rInner = d_innerRadius.getValue(); + const Real r4 = r * r * r * r; + const Real rInner4 = rInner * rInner * rInner * rInner; + + Iy = M_PI * (r4 - rInner4) / 4.0; + Iz = Iy; + J = Iy + Iz; + A = M_PI * (r * r - rInner4); + + } + m_crossSectionArea = A; + + if( d_useInertiaParams.getValue() ) + { + msg_info("BeamHookeLawForceField")<< "Pre-calculated inertia parameters are used for the computation of the stiffness matrix."; + m_K_section66[0][0] = d_GI.getValue(); + m_K_section66[1][1] = d_EIy.getValue(); + m_K_section66[2][2] = d_EIz.getValue(); + m_K_section66[3][3] = d_EA.getValue(); + m_K_section66[4][4] = d_GA.getValue(); + m_K_section66[5][5] = d_GA.getValue(); + } + else + { + //Pour du vec 6, on a _m_K =diag([G*J E*Iy E*Iz E*A G*As G*As]); % stifness matrix + Real E = d_youngModulus.getValue(); + Real G = E/(2.0*(1.0+d_poissonRatio.getValue())); + //Translational Stiffness (X, Y, Z directions): + // Gs * J(i): Shearing modulus times the second moment of the area (torsional stiffness). + // E * Js(i): Young's modulus times the second moment of the area (bending stiffness). + m_K_section66[0][0] = G*J; + m_K_section66[1][1] = E*Iy; + m_K_section66[2][2] = E*Iz; + //Rotational Stiffness (X, Y, Z directions): + //E * A: Young's modulus times the cross-sectional area (axial stiffness). + //Gs * A: Shearing modulus times the cross-sectional area (shearing stiffness). + m_K_section66[3][3] = E*A; + m_K_section66[4][4] = G*A; + m_K_section66[5][5] = G*A; + } +} + +template<> +void BeamHookeLawForceField::addForce(const MechanicalParams* mparams, + DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecDeriv& d_v) +{ + SOFA_UNUSED(d_v); + SOFA_UNUSED(mparams); + + if(!this->getMState()) { + msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; + compute_df=false; + return; + } + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + // get the rest position (for non straight shape) + const VecCoord& x0 = this->mstate->read(VecCoordId::restPosition())->getValue(); + + f.resize(x.size()); + unsigned int sz = d_length.getValue().size(); + if(x.size()!= sz){ + msg_warning("BeamHookeLawForceField")<<" length : "<< sz <<"should have the same size as x... "<< x.size() <<"\n"; + compute_df = false; + return; + } + for (unsigned int i=0; i +void BeamHookeLawForceField::addDForce(const MechanicalParams* mparams, + DataVecDeriv& d_df , + const DataVecDeriv& d_dx) +{ + if (!compute_df) + return; + + WriteAccessor< DataVecDeriv > df = d_df; + ReadAccessor< DataVecDeriv > dx = d_dx; + Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + + df.resize(dx.size()); + for (unsigned int i=0; i +void BeamHookeLawForceField::addKToMatrix(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) +{ + MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); + BaseMatrix* mat = mref.matrix; + unsigned int offset = mref.offset; + Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + + const VecCoord& pos = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + for (unsigned int n=0; nadd(offset + i + 6*n, offset + j + 6*n, -kFact * m_K_section66[i][j]*d_length.getValue()[n]); + + } +} + //////////////////////////////////////////// FACTORY ////////////////////////////////////////////// // Registering the component // see: http://wiki.sofa-framework.org/wiki/ObjectFactory @@ -44,6 +179,7 @@ using namespace sofa::defaulttype; int BeamHookeLawForceFieldClass = core::RegisterObject("This component is used to compute internal stress for torsion (along x) and bending (y and z)") .add >(true) + .add >() ; ////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -54,5 +190,6 @@ int BeamHookeLawForceFieldClass = core::RegisterObject("This component is used t // see: http://www.stroustrup.com/C++11FAQ.html#extern-templates template class BeamHookeLawForceField; +template class BeamHookeLawForceField; } // forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.h b/src/Cosserat/forcefield/BeamHookeLawForceField.h index d9e086f7..a6e4a68f 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.h @@ -60,7 +60,7 @@ using sofa::helper::OptionsGroup; * Only bending and torsion strain / stress are considered here */ template -class BeamHookeLawForceField : public ForceField +class SOFA_COSSERAT_API BeamHookeLawForceField : public ForceField { public : SOFA_CLASS(SOFA_TEMPLATE(BeamHookeLawForceField, DataTypes), SOFA_TEMPLATE(ForceField, DataTypes)); @@ -76,6 +76,7 @@ public : typedef Vec<3, Real> Vec3; typedef Mat<3, 3, Real> Mat33; + typedef Mat<6, 6, Real> Mat66; typedef CompressedRowSparseMatrix CSRMat33B66; @@ -137,9 +138,12 @@ public : Data d_GA; Data d_EA; Data d_EI; + Data d_EIy; + Data d_EIz; bool compute_df; Mat33 m_K_section; + Mat66 m_K_section66; type::vector m_K_sectionList; /// Cross-section area @@ -154,9 +158,12 @@ private : /// the "this->" approach. using ForceField::getContext ; using ForceField::f_printLog ; - //using ForceField::mstate ; //////////////////////////////////////////////////////////////////////////// }; +#if !defined(SOFA_COSSERAT_CPP_BeamHookeLawForceField) +extern template class SOFA_COSSERAT_API BeamHookeLawForceField; +extern template class SOFA_COSSERAT_API BeamHookeLawForceField; +#endif } // forcefield