Skip to content

Commit

Permalink
Merge branch 'master' into remove_core_localstorage
Browse files Browse the repository at this point in the history
  • Loading branch information
hugtalbot authored Dec 8, 2023
2 parents 0cc78ad + da70359 commit e8a9df2
Show file tree
Hide file tree
Showing 58 changed files with 875 additions and 696 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ struct FixedPlaneConstraint_test : public BaseSimulationTest

/// Scene initialization
sofa::simulation::Simulation* simulation;
sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation());
simulation = sofa::simulation::getSimulation();
simulation::Node::SPtr root = simulation->createNewGraph("root");
root->setGravity( type::Vec3(0,0,0) );

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,14 @@ using namespace sofa::component::linearsystem;
template<class Matrix, class Vector>
MatrixLinearSolver<Matrix,Vector>::MatrixLinearSolver()
: Inherit()
, d_parallelInverseProduct(initData(&d_parallelInverseProduct, false,
"parallelInverseProduct", "Parallelize the computation of the product J*M^{-1}*J^T "
"where M is the matrix of the linear system and J is any "
"matrix with compatible dimensions"))
, invertData()
, linearSystem()
, currentMFactor(), currentBFactor(), currentKFactor()
, l_linearSystem(initLink("linearSystem", "The linear system to solve"))
, d_parallelInverseProduct(initData(&d_parallelInverseProduct, false,
"parallelInverseProduct", "Parallelize the computation of the product J*M^{-1}*J^T "
"where M is the matrix of the linear system and J is any "
"matrix with compatible dimensions"))
{
this->addUpdateCallback("parallelInverseProduct", {&d_parallelInverseProduct},
[this](const core::DataTracker& tracker) -> sofa::core::objectmodel::ComponentState
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,13 +52,17 @@ class ConstantLocalMatrix : public virtual AssemblingMatrixAccumulator<c>
template <class TMatrix, core::matrixaccumulator::Contribution c>
void ConstantLocalMatrix<TMatrix, c>::add(const core::matrixaccumulator::no_check_policy&, sofa::SignedIndex row, sofa::SignedIndex col, float value)
{
SOFA_UNUSED(row);
SOFA_UNUSED(col);
static_cast<TMatrix*>(this->m_globalMatrix)->colsValue[insertionOrderList[currentId++]]
+= this->m_cachedFactor * value;
}

template <class TMatrix, core::matrixaccumulator::Contribution c>
void ConstantLocalMatrix<TMatrix, c>::add(const core::matrixaccumulator::no_check_policy&, sofa::SignedIndex row, sofa::SignedIndex col, double value)
{
SOFA_UNUSED(row);
SOFA_UNUSED(col);
static_cast<TMatrix*>(this->m_globalMatrix)->colsValue[insertionOrderList[currentId++]]
+= this->m_cachedFactor * value;
}
Expand Down
36 changes: 33 additions & 3 deletions Sofa/Component/LinearSystem/tests/MatrixLinearSystem_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -250,9 +250,23 @@ template<class DataTypes>
dfdx(10, 20) += 0.;
}

void addForce(const sofa::core::MechanicalParams*, typename Inherit1::DataVecDeriv& f, const typename Inherit1::DataVecCoord& x, const typename Inherit1::DataVecDeriv& v) override {}
void addDForce(const sofa::core::MechanicalParams* mparams, typename Inherit1::DataVecDeriv& df, const typename Inherit1::DataVecDeriv& dx ) override {}
SReal getPotentialEnergy(const sofa::core::MechanicalParams*, const typename Inherit1::DataVecCoord& x) const override { return 0._sreal; }
void addForce(const sofa::core::MechanicalParams*, typename Inherit1::DataVecDeriv& f, const typename Inherit1::DataVecCoord& x, const typename Inherit1::DataVecDeriv& v) override
{
SOFA_UNUSED(f);
SOFA_UNUSED(x);
SOFA_UNUSED(v);
}
void addDForce(const sofa::core::MechanicalParams* mparams, typename Inherit1::DataVecDeriv& df, const typename Inherit1::DataVecDeriv& dx ) override
{
SOFA_UNUSED(mparams);
SOFA_UNUSED(df);
SOFA_UNUSED(dx);
}
SReal getPotentialEnergy(const sofa::core::MechanicalParams*, const typename Inherit1::DataVecCoord& x) const override
{
SOFA_UNUSED(x);
return 0._sreal;
}
};

/// Empty matrix class with the interface of a BaseMatrix
Expand All @@ -273,26 +287,42 @@ class EmptyMatrix : public sofa::linearalgebra::BaseMatrix
}
SReal element(Index i, Index j) const override
{
SOFA_UNUSED(i);
SOFA_UNUSED(j);
return {};
}
void resize(Index nbRow, Index nbCol) override
{
SOFA_UNUSED(nbRow);
SOFA_UNUSED(nbCol);
}
void clear() override
{
}
void set(Index i, Index j, double v) override
{
SOFA_UNUSED(i);
SOFA_UNUSED(j);
SOFA_UNUSED(v);
}
void add(Index row, Index col, double v) override
{
SOFA_UNUSED(row);
SOFA_UNUSED(col);
SOFA_UNUSED(v);
//add method is empty to prevent crashes in tests
}
void add(Index row, Index col, const sofa::type::Mat3x3d& _M) override
{
SOFA_UNUSED(row);
SOFA_UNUSED(col);
SOFA_UNUSED(_M);
}
void add(Index row, Index col, const sofa::type::Mat3x3f& _M) override
{
SOFA_UNUSED(row);
SOFA_UNUSED(col);
SOFA_UNUSED(_M);
}
static const char* Name() { return "EmptyMatrix"; }
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,41 +82,27 @@ class EllipsoidForceField : public core::behavior::ForceField<DataTypes>

};

union
{
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() Data<sofa::type::vector<Contact> > contacts; ///< Contacts
Data<sofa::type::vector<Contact> > d_contacts; ///< Contacts
};
Data<sofa::type::vector<Contact> > d_contacts; ///< Vector of contacts
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() DeprecatedAndRemoved contacts; ///< Contacts

EllipsoidForceFieldInternalData<DataTypes> data;

public:

union
{
Data<Coord> d_center; ///< ellipsoid center
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() Data<Coord> center; ///< ellipsoid center
};
union
{
Data<Coord> d_vradius; ///< ellipsoid radius
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() Data<Coord> vradius; ///< ellipsoid radius
};
union
{
Data<Real> d_stiffness; ///< force stiffness (positive to repulse outward, negative inward)
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() Data<Real> stiffness; ///< force stiffness (positive to repulse outward, negative inward)
};
union
{
Data<Real> d_damping; ///< force damping
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() Data<Real> damping; ///< force damping
};
union
{
Data<sofa::type::RGBAColor> d_color; ///< ellipsoid color. (default=0,0.5,1.0,1.0)
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() Data<sofa::type::RGBAColor> color; ///< ellipsoid color. (default=0,0.5,1.0,1.0)
};
Data<Coord> d_center; ///< ellipsoid center
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() DeprecatedAndRemoved center; ///< ellipsoid center

Data<Coord> d_vradius; ///< ellipsoid radius
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() DeprecatedAndRemoved vradius; ///< ellipsoid radius

Data<Real> d_stiffness; ///< force stiffness (positive to repulse outward, negative inward)
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() DeprecatedAndRemoved stiffness; ///< force stiffness (positive to repulse outward, negative inward)

Data<Real> d_damping; ///< force damping
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() DeprecatedAndRemoved damping; ///< force damping

Data<sofa::type::RGBAColor> d_color; ///< ellipsoid color. (default=0,0.5,1.0,1.0)
SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() DeprecatedAndRemoved color; ///< ellipsoid color. (default=0,0.5,1.0,1.0)

protected:
EllipsoidForceField();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ namespace sofa::component::mechanicalload

template<class DataTypes>
EllipsoidForceField<DataTypes>::EllipsoidForceField()
: d_contacts(initData(&d_contacts,"contacts", "Contacts"))
: d_contacts(initData(&d_contacts,"contacts", "Vector of contacts"))
, d_center(initData(&d_center, "center", "ellipsoid center"))
, d_vradius(initData(&d_vradius, "vradius", "ellipsoid radius"))
, d_stiffness(initData(&d_stiffness, (Real)500, "stiffness", "force stiffness (positive to repulse outward, negative inward)"))
Expand All @@ -69,12 +69,6 @@ EllipsoidForceField<DataTypes>::EllipsoidForceField()
template <class DataTypes>
EllipsoidForceField<DataTypes>::~EllipsoidForceField()
{
d_contacts.~Data();
d_center.~Data();
d_vradius.~Data();
d_stiffness.~Data();
d_damping.~Data();
d_color.~Data();
}

template<class DataTypes>
Expand Down Expand Up @@ -115,7 +109,7 @@ void EllipsoidForceField<DataTypes>::addForce(const sofa::core::MechanicalParams
{
Coord dp = p1[i] - center;
Real norm2 = 0;
for (int j=0; j<N; j++) norm2 += (dp[j]*dp[j])*inv_r2[j];
for (unsigned int j=0; j<N; j++) norm2 += (dp[j]*dp[j])*inv_r2[j];
//Real d = (norm2-1)*s2;
if ((norm2-1)*stiffness<0)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,10 @@
#endif

#ifdef SOFA_BUILD_SOFA_COMPONENT_MECHANICALLOAD
#define SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED()
#define SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED()
#else
#define SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DEPRECATED() \
SOFA_ATTRIBUTE_DEPRECATED( \
#define SOFA_ELLIPSOIDFORCEFIELD_RENAMEDDATA_DISABLED() \
SOFA_ATTRIBUTE_DISABLED( \
"v23.12", "v24.06", "This Data has been renamed to match convention with d_ prefix.")
#endif // SOFA_BUILD_SOFA_COMPONENT_MECHANICALLOAD

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,16 @@ using namespace sofa::defaulttype;
using namespace core::behavior;

NewmarkImplicitSolver::NewmarkImplicitSolver()
: d_rayleighStiffness(initData(&d_rayleighStiffness,0.0,"rayleighStiffness","Rayleigh damping coefficient related to stiffness") )
, d_rayleighMass( initData(&d_rayleighMass,0.0,"rayleighMass","Rayleigh damping coefficient related to mass"))
, d_velocityDamping( initData(&d_velocityDamping,0.0,"vdamping","Velocity decay coefficient (no decay if null)") )
, d_gamma( initData(&d_gamma, 0.5, "gamma", "Newmark scheme gamma coefficient"))
, d_beta( initData(&d_beta, 0.25, "beta", "Newmark scheme beta coefficient") )
: d_rayleighStiffness(initData(&d_rayleighStiffness, 0_sreal,
"rayleighStiffness",
"Rayleigh damping coefficient related to stiffness"))
, d_rayleighMass(initData(&d_rayleighMass, 0_sreal, "rayleighMass",
"Rayleigh damping coefficient related to mass"))
, d_velocityDamping(initData(&d_velocityDamping, 0_sreal, "vdamping",
"Velocity decay coefficient (no decay if null)"))
, d_gamma(initData(&d_gamma, 0.5_sreal, "gamma",
"Newmark scheme gamma coefficient"))
, d_beta(initData(&d_beta, 0.25_sreal, "beta", "Newmark scheme beta coefficient"))
, d_threadSafeVisitor(initData(&d_threadSafeVisitor, false, "threadSafeVisitor", "If true, do not use realloc and free visitors in fwdInteractionForceField."))
{
cpt=0;
Expand All @@ -70,10 +75,10 @@ void NewmarkImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa


const SReal h = dt;
const double gamma = d_gamma.getValue();
const double beta = d_beta.getValue();
const double rM = d_rayleighMass.getValue();
const double rK = d_rayleighStiffness.getValue();
const SReal gamma = d_gamma.getValue();
const SReal beta = d_beta.getValue();
const SReal rM = d_rayleighMass.getValue();
const SReal rK = d_rayleighStiffness.getValue();

// 1. Initialize a_t and to store it as a vecId to be used in the resolution of this solver (using as well old xand v)
// Once we have a_{t+dt} we can update the new x and v.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,12 +56,12 @@ class SOFA_COMPONENT_ODESOLVER_BACKWARD_API NewmarkImplicitSolver : public sofa:

public:
SOFA_CLASS(NewmarkImplicitSolver, sofa::core::behavior::OdeSolver);
Data<double> d_rayleighStiffness; ///< Rayleigh damping coefficient related to stiffness
Data<double> d_rayleighMass; ///< Rayleigh damping coefficient related to mass
Data<double> d_velocityDamping; ///< Velocity decay coefficient (no decay if null)
Data<SReal> d_rayleighStiffness; ///< Rayleigh damping coefficient related to stiffness
Data<SReal> d_rayleighMass; ///< Rayleigh damping coefficient related to mass
Data<SReal> d_velocityDamping; ///< Velocity decay coefficient (no decay if null)

Data<double> d_gamma; ///< Newmark scheme gamma coefficient
Data<double> d_beta; ///< Newmark scheme beta coefficient
Data<SReal> d_gamma; ///< Newmark scheme gamma coefficient
Data<SReal> d_beta; ///< Newmark scheme beta coefficient

Data<bool> d_threadSafeVisitor; ///< If true, do not use realloc and free visitors in fwdInteractionForceField.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -103,9 +103,9 @@ void CentralDifferenceSolver::solve(const core::ExecParams* params, SReal dt, so
{
#ifdef SOFA_NO_VMULTIOP // unoptimized version
vel2.eq( vel, dx, dt ); // vel = vel + dt M^{-1} ( P_n - K u_n )
mop.solveConstraint(vel2, core::ConstraintParams::ConstOrder::VEL);
mop.solveConstraint(vel2,core::ConstraintOrder::VEL);
pos2.eq( pos, vel2, dt ); // pos = pos + h vel
mop.solveConstraint(pos2, core::ConstraintParams::ConstOrder::POS);
mop.solveConstraint(pos2,core::ConstraintOrder::POS);

#else // single-operation optimization

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,21 +135,21 @@ void EulerExplicitSolver::updateState(sofa::simulation::common::VectorOperations
//newPos = pos + newVel * dt

newVel.eq(vel, acc.id(), dt);
mop->solveConstraint(newVel, core::ConstraintParams::ConstOrder::VEL);
mop->solveConstraint(newVel,core::ConstraintOrder::VEL);

newPos.eq(pos, newVel, dt);
mop->solveConstraint(newPos, core::ConstraintParams::ConstOrder::POS);
mop->solveConstraint(newPos,core::ConstraintOrder::POS);
}
else
{
//newPos = pos + vel * dt
//newVel = vel + acc * dt

newPos.eq(pos, vel, dt);
mop->solveConstraint(newPos, core::ConstraintParams::ConstOrder::POS);
mop->solveConstraint(newPos,core::ConstraintOrder::POS);

newVel.eq(vel, acc.id(), dt);
mop->solveConstraint(newVel, core::ConstraintParams::ConstOrder::VEL);
mop->solveConstraint(newVel,core::ConstraintOrder::VEL);
}
#else // single-operation optimization
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -91,9 +91,9 @@ void RungeKutta2Solver::solve(const core::ExecParams* params, SReal dt, sofa::co
// Use the derivative at newX, newV to update the state
#ifdef SOFA_NO_VMULTIOP // unoptimized version
pos2.eq(pos,newV,dt);
solveConstraint(dt,pos2,core::ConstraintParams::ConstOrder::POS);
mop.solveConstraint(pos2,core::ConstraintOrder::POS);
vel2.eq(vel,acc,dt);
solveConstraint(dt,vel2,core::ConstraintParams::ConstOrder::VEL);
mop.solveConstraint(vel2,core::ConstraintOrder::VEL);
#else // single-operation optimization
{
typedef core::behavior::BaseMechanicalState::VMultiOp VMultiOp;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -160,9 +160,9 @@ void RungeKutta4Solver::solve(const core::ExecParams* params, SReal dt, sofa::co
pos2.peq(k3v,stepBy3);
vel2.peq(k3a,stepBy3);
pos2.peq(k4v,stepBy6);
solveConstraint(dt, pos2, core::ConstraintParams::ConstOrder::POS);
mop.solveConstraint(pos2, core::ConstraintOrder::POS);
vel2.peq(k4a,stepBy6);
solveConstraint(dt, vel2, core::ConstraintParams::ConstOrder::VEL);
mop.solveConstraint(vel2, core::ConstraintOrder::VEL);
#else // single-operation optimization
{
typedef core::behavior::BaseMechanicalState::VMultiOp VMultiOp;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,7 @@ class TetrahedronFEMForceField : public core::behavior::ForceField<DataTypes>, p
Data<int> _computeVonMisesStress; ///< compute and display von Mises stress: 0: no computations, 1: using corotational strain, 2: using full Green strain
Data<type::vector<Real> > _vonMisesPerElement; ///< von Mises Stress per element
Data<type::vector<Real> > _vonMisesPerNode; ///< von Mises Stress per node
Data<type::vector<type::Vec4f> > _vonMisesStressColors; ///< Vector of colors describing the VonMises stress
Data<type::vector<type::RGBAColor> > _vonMisesStressColors; ///< Vector of colors describing the VonMises stress

Real m_minVonMisesPerNode;
Real m_maxVonMisesPerNode;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1900,7 +1900,7 @@ void TetrahedronFEMForceField<DataTypes>::drawTrianglesFromRangeOfTetrahedra(
else
{
sofa::helper::ColorMap::evaluator<Real> evalColor = this->m_VonMisesColorMap->getEvaluator(minVM, maxVM);
auto col = sofa::type::RGBAColor::fromVec4(evalColor(vM[elementId]));
auto col = evalColor(vM[elementId]);
col[3] = 1.0f;
color[0] = col;
color[1] = col;
Expand Down Expand Up @@ -2542,7 +2542,7 @@ void TetrahedronFEMForceField<DataTypes>::computeVonMisesStress()

updateVonMisesStress=false;

helper::WriteAccessor<Data<type::vector<type::Vec4f> > > vonMisesStressColors(_vonMisesStressColors);
helper::WriteAccessor<Data<type::vector<type::RGBAColor> > > vonMisesStressColors(_vonMisesStressColors);
vonMisesStressColors.clear();
type::vector<unsigned int> vonMisesStressColorsCoeff;

Expand All @@ -2565,12 +2565,12 @@ void TetrahedronFEMForceField<DataTypes>::computeVonMisesStress()
for(it = _indexedElements->begin() ; it != _indexedElements->end() ; ++it, ++i)
{
helper::ColorMap::evaluator<Real> evalColor = m_VonMisesColorMap->getEvaluator(minVM, maxVM);
const type::Vec4f col = evalColor(vME[i]);
const auto col = evalColor(vME[i]);
Tetrahedron tetra = (*_indexedElements)[i];

for(unsigned int j=0 ; j<4 ; j++)
{
vonMisesStressColors[tetra[j]] += (col);
vonMisesStressColors[tetra[j]] = vonMisesStressColors[tetra[j]]+(col);
vonMisesStressColorsCoeff[tetra[j]] ++;
}
}
Expand All @@ -2579,7 +2579,7 @@ void TetrahedronFEMForceField<DataTypes>::computeVonMisesStress()
{
if(vonMisesStressColorsCoeff[i] != 0)
{
vonMisesStressColors[i] /= vonMisesStressColorsCoeff[i];
vonMisesStressColors[i] = vonMisesStressColors[i] / vonMisesStressColorsCoeff[i];
}
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -215,7 +215,7 @@ void TriangularFEMForceField<DataTypes>::initLarge(int i, Index& a, Index& b, In
}
else
{
VecCoord initialPoints = (this->mstate->read(core::ConstVecCoordId::restPosition())->getValue());
const VecCoord& initialPoints = (this->mstate->read(core::ConstVecCoordId::restPosition())->getValue());
tinfo->rotation = tinfo->initialTransformation;
if (a >= (initialPoints).size() || b >= (initialPoints).size() || c >= (initialPoints).size())
{
Expand Down
Loading

0 comments on commit e8a9df2

Please sign in to comment.