Skip to content

Commit

Permalink
Merge pull request #93 from EulalieCoevoet/pr_beamhookelawfftemplate
Browse files Browse the repository at this point in the history 
[forcefield] add template vec6types to BeamHookeLawForceField
  • Loading branch information
@adagolodjo
adagolodjo authored Dec 15, 2023
2 parents 5d7e28a + 6530902 commit a045855
Show file tree
Hide file tree
Showing 2 changed files with 146 additions and 2 deletions.
137 changes: 137 additions & 0 deletions src/Cosserat/forcefield/BeamHookeLawForceField.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,141 @@
namespace sofa::component::forcefield
{

template<>
void BeamHookeLawForceField<defaulttype::Vec6Types>::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<defaulttype::Vec6Types>::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<x.size(); i++)
f[i] -= (m_K_section66 * (x[i] - x0[i])) * d_length.getValue()[i];

d_f.endEdit();

}

template<>
void BeamHookeLawForceField<defaulttype::Vec6Types>::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<dx.size(); i++)
df[i] -= (m_K_section66 * dx[i])*kFactor* d_length.getValue()[i];
}

template<>
void BeamHookeLawForceField<defaulttype::Vec6Types>::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; n<pos.size(); n++)
{
for(unsigned int i = 0; i < 6; i++)
for (unsigned int j = 0; j< 6; j++)
mat->add(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
Expand All @@ -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<BeamHookeLawForceField<Vec3Types> >(true)
.add<BeamHookeLawForceField<Vec6Types> >()

;
//////////////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -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<Vec3Types>;
template class BeamHookeLawForceField<Vec6Types>;

} // forcefield
11 changes: 9 additions & 2 deletions src/Cosserat/forcefield/BeamHookeLawForceField.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ using sofa::helper::OptionsGroup;
* Only bending and torsion strain / stress are considered here
*/
template<typename DataTypes>
class BeamHookeLawForceField : public ForceField<DataTypes>
class SOFA_COSSERAT_API BeamHookeLawForceField : public ForceField<DataTypes>
{
public :
SOFA_CLASS(SOFA_TEMPLATE(BeamHookeLawForceField, DataTypes), SOFA_TEMPLATE(ForceField, DataTypes));
Expand All @@ -76,6 +76,7 @@ public :

typedef Vec<3, Real> Vec3;
typedef Mat<3, 3, Real> Mat33;
typedef Mat<6, 6, Real> Mat66;

typedef CompressedRowSparseMatrix<Mat33> CSRMat33B66;

Expand Down Expand Up @@ -137,9 +138,12 @@ public :
Data<Real> d_GA;
Data<Real> d_EA;
Data<Real> d_EI;
Data<Real> d_EIy;
Data<Real> d_EIz;

bool compute_df;
Mat33 m_K_section;
Mat66 m_K_section66;
type::vector<Mat33> m_K_sectionList;

/// Cross-section area
Expand All @@ -154,9 +158,12 @@ private :
/// the "this->" approach.
using ForceField<DataTypes>::getContext ;
using ForceField<DataTypes>::f_printLog ;
//using ForceField<DataTypes>::mstate ;
////////////////////////////////////////////////////////////////////////////
};

#if !defined(SOFA_COSSERAT_CPP_BeamHookeLawForceField)
extern template class SOFA_COSSERAT_API BeamHookeLawForceField<defaulttype::Vec3Types>;
extern template class SOFA_COSSERAT_API BeamHookeLawForceField<defaulttype::Vec6Types>;
#endif

} // forcefield

0 comments on commit a045855

Please sign in to comment.