Skip to content

Commit

Permalink
restore the previous formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@alxbilger
alxbilger committed Jan 9, 2025
1 parent fc2946c commit 89c9053
Showing 1 changed file with 131 additions and 133 deletions.
264 changes: 131 additions & 133 deletions src/Cosserat/forcefield/BeamHookeLawForceField.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,137 +32,142 @@

#include <sofa/core/ObjectFactory.h>

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;
}
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>::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>::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]);
}
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]);

}
}

using namespace sofa::defaulttype;
Expand All @@ -183,12 +188,5 @@ void registerBeamHookeLawForceField(sofa::core::ObjectFactory* factory)
.add<sofa::component::forcefield::BeamHookeLawForceField<sofa::defaulttype::Vec3Types>>(true)
.add<sofa::component::forcefield::BeamHookeLawForceField<sofa::defaulttype::Vec6Types>>());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////

// Force template specialization for the most common sofa floating point related type.
// This goes with the extern template declaration in the .h. Declaring extern template
// avoid the code generation of the template for each compilation unit.
// see: http://www.stroustrup.com/C++11FAQ.html#extern-templates


} // forcefield
}

0 comments on commit 89c9053

Please sign in to comment.