Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ TEST(LinearSystem, MatrixSystem_springForceField)
// Compute the external force. This step is mandatory because most of the time force fields
// pre-computes required elements for the matrix assembly in the addForce method
sofa::core::MultiVecDerivId ffId = sofa::core::vec_id::write_access::externalForce;
((sofa::core::behavior::BaseForceField*)spring.get())->addForce(&mparams, ffId);
((sofa::core::behavior::BaseForceField*)spring.get())->addForce(&mparams, ffId, mparams.x(), mparams.v());


// Finally build the system matrix, which is composed of only the stiffness matrix from the spring force field
Expand Down
6 changes: 6 additions & 0 deletions Sofa/framework/Core/src/sofa/core/behavior/BaseForceField.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,12 @@ BaseForceField::BaseForceField()
{
}

void BaseForceField::addForce(const MechanicalParams* mparams, MultiVecDerivId fId)
{
assert(mparams);
addForce(mparams, fId, mparams->x(), mparams->v());
}

void BaseForceField::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId)
{
const auto kFactor = sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams,rayleighStiffness.getValue());
Expand Down
10 changes: 6 additions & 4 deletions Sofa/framework/Core/src/sofa/core/behavior/BaseForceField.h
Original file line number Diff line number Diff line change
Expand Up @@ -79,13 +79,15 @@ class SOFA_CORE_API BaseForceField : public virtual StateAccessor
/// \param mparams
/// - \a sofa::core::mechanicalparams::bFactor(mparams) is the coefficient for damping contributions (i.e. first derivatives term in the ODE)
/// - \a mparams->kFactor() is the coefficient for stiffness contributions (i.e. DOFs term in the ODE)
/// - \a mparams->readX() is the input vector of position
/// - \a mparams->readV() is the input vector of velocity
/// - \a mparams->readF() is the input vector of force
/// - if \a mparams->energy() is true, the method computes and internally stores the potential energy,
/// which will be subsequently returned by method getPotentialEnergy()
/// \param fId the output vector of forces
virtual void addForce(const MechanicalParams* mparams, MultiVecDerivId fId )=0;
/// \param xId the input vector for positions
/// \param vId the input vector for velocities
virtual void addForce(const MechanicalParams* mparams, MultiVecDerivId fId,
ConstMultiVecCoordId xId, ConstMultiVecDerivId vId )=0;
SOFA_ATTRIBUTE_DEPRECATED__ADDFORCE_OVERLOAD()
virtual void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) final;

/// \brief Compute the force derivative given a small displacement from the
/// position and velocity used in the previous call to addForce().
Expand Down
2 changes: 1 addition & 1 deletion Sofa/framework/Core/src/sofa/core/behavior/ForceField.h
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ class ForceField : public BaseForceField, public virtual SingleStateAccessor<TDa
/// This method retrieves the force, x and v vector from the MechanicalState
/// and call the internal addForce(const MechanicalParams*, DataVecDeriv&,const DataVecCoord&,const DataVecDeriv&)
/// method implemented by the component.
void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) override;
void addForce(const MechanicalParams* mparams, MultiVecDerivId fId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId ) override;

/// Given the current position and velocity states, update the current force
/// vector by computing and adding the forces associated with this
Expand Down
11 changes: 7 additions & 4 deletions Sofa/framework/Core/src/sofa/core/behavior/ForceField.inl
Original file line number Diff line number Diff line change
Expand Up @@ -41,12 +41,15 @@ template<class DataTypes>
ForceField<DataTypes>::~ForceField() = default;

template<class DataTypes>
void ForceField<DataTypes>::addForce(const MechanicalParams* mparams, MultiVecDerivId fId )
void ForceField<DataTypes>::addForce(const MechanicalParams* mparams, MultiVecDerivId fId,
ConstMultiVecCoordId xId, ConstMultiVecDerivId vId )
{
auto mstate = this->mstate.get();
if (mparams && mstate)
if (mparams && this->mstate)
{
addForce(mparams, *fId[mstate].write() , *mparams->readX(mstate), *mparams->readV(mstate));
DataVecDeriv* f = fId[this->mstate.get()].write(); assert(f);
const DataVecCoord* x = xId[this->mstate.get()].read(); assert(x);
const DataVecDeriv* v = vId[this->mstate.get()].read(); assert(v);
addForce(mparams, *f , *x, *v);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair
/// This method retrieves the force, x and v vector from the two MechanicalState
/// and call the internal addForce(VecDeriv&,VecDeriv&,const VecCoord&,const VecCoord&,const VecDeriv&,const VecDeriv&)
/// method implemented by the component.
void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) override;
void addForce(const MechanicalParams* mparams, MultiVecDerivId fId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId ) override;

/// Compute the force derivative given a small displacement from the
/// position and velocity used in the previous call to addForce().
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,17 +43,19 @@ MixedInteractionForceField<DataTypes1, DataTypes2>::~MixedInteractionForceField(
}

template<class DataTypes1, class DataTypes2>
void MixedInteractionForceField<DataTypes1, DataTypes2>::addForce(const MechanicalParams* mparams, MultiVecDerivId fId )
void MixedInteractionForceField<DataTypes1, DataTypes2>::addForce(
const MechanicalParams* mparams, MultiVecDerivId fId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId )
{

if (this->mstate1 && this->mstate2)
{
auto state1 = this->mstate1.get();
auto state2 = this->mstate2.get();
addForce( mparams, *fId[state1].write(), *fId[state2].write(),
*mparams->readX(state1), *mparams->readX(state2),
*mparams->readV(state1), *mparams->readV(state2));

DataVecDeriv1* f1 = fId[this->mstate1.get()].write(); assert(f1);
DataVecDeriv2* f2 = fId[this->mstate2.get()].write(); assert(f2);
const DataVecCoord1* x1 = xId[this->mstate1.get()].read(); assert(x1);
const DataVecDeriv1* v1 = vId[this->mstate1.get()].read(); assert(v1);
const DataVecCoord2* x2 = xId[this->mstate2.get()].read(); assert(x2);
const DataVecDeriv2* v2 = vId[this->mstate2.get()].read(); assert(v2);

addForce(mparams, *f1, *f2, *x1, *x2, *v1, *v2);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS
/// This method retrieves the force, x and v vector from the two MechanicalState
/// and call the internal addForce(VecDeriv&,VecDeriv&,const VecCoord&,const VecCoord&,const VecDeriv&,const VecDeriv&)
/// method implemented by the component.
void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) override;
void addForce(const MechanicalParams* mparams, MultiVecDerivId fId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId) override;

/// Given the current position and velocity states, update the current force
/// vector by computing and adding the forces associated with this
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,15 +46,20 @@ PairInteractionForceField<DataTypes>::~PairInteractionForceField()
}

template<class DataTypes>
void PairInteractionForceField<DataTypes>::addForce(const MechanicalParams* mparams, MultiVecDerivId fId )
void PairInteractionForceField<DataTypes>::addForce(const MechanicalParams* mparams, MultiVecDerivId fId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId )
{
auto state1 = this->mstate1.get();
auto state2 = this->mstate2.get();
if (state1 && state2)
{
addForce(mparams, *fId[state1].write(), *fId[state2].write(),
*mparams->readX(state1), *mparams->readX(state2),
*mparams->readV(state1), *mparams->readV(state2));
DataVecDeriv* f1 = fId[this->mstate1.get()].write(); assert(f1);
DataVecDeriv* f2 = fId[this->mstate2.get()].write(); assert(f2);
const DataVecCoord* x1 = xId[this->mstate1.get()].read(); assert(x1);
const DataVecDeriv* v1 = vId[this->mstate1.get()].read(); assert(v1);
const DataVecCoord* x2 = xId[this->mstate2.get()].read(); assert(x2);
const DataVecDeriv* v2 = vId[this->mstate2.get()].read(); assert(v2);

addForce(mparams, *f1, *f2, *x1, *x2, *v1, *v2);
}
else
msg_error() << "PairInteractionForceField<DataTypes>::addForce(const MechanicalParams* /*mparams*/, MultiVecDerivId /*fId*/ ), mstate missing";
Expand Down
7 changes: 7 additions & 0 deletions Sofa/framework/Core/src/sofa/core/config.h.in
Original file line number Diff line number Diff line change
Expand Up @@ -127,3 +127,10 @@ SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v29.06", "Use toBaseComponent instead.")
#define SOFA_CORE_DEPRECATED_RENAME_CREATORMAP_OBJECTTEMPLATECREATORMAP() \
SOFA_ATTRIBUTE_DISABLED("v25.12", "v26.06", "Type CreatorMap has been renamed to ObjectTemplateCreatorMap.")
#endif

#ifdef SOFA_BUILD_SOFA_CORE
#define SOFA_ATTRIBUTE_DEPRECATED__ADDFORCE_OVERLOAD()
#else
#define SOFA_ATTRIBUTE_DEPRECATED__ADDFORCE_OVERLOAD() \
SOFA_ATTRIBUTE_DEPRECATED("v26.12", "v27.06", "addForce must be called by providing the position vector.")
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@ void MappingGraphMechanicalOperations::projectResponse(const MappingGraph& mappi

void MappingGraphMechanicalOperations::computeForce(const MappingGraph& mappingGraph,
core::MultiVecDerivId result,
core::ConstMultiVecCoordId xId,
core::ConstMultiVecDerivId vId,
bool clearForceBefore,
bool accumulateForcesFromMappedStates,
TaskScheduler* taskScheduler)
Expand Down Expand Up @@ -76,7 +78,7 @@ void MappingGraphMechanicalOperations::computeForce(const MappingGraph& mappingG
*/
mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField)
{
forceField.addForce(&mparams, result);
forceField.addForce(&mparams, result, xId, vId);
}, sofa::simulation::VisitorApplication::ALL_NODES, taskScheduler);

if (accumulateForcesFromMappedStates)
Expand All @@ -91,6 +93,16 @@ void MappingGraphMechanicalOperations::computeForce(const MappingGraph& mappingG
});
}
}

void MappingGraphMechanicalOperations::computeForce(const MappingGraph& mappingGraph,
core::MultiVecDerivId result,
bool clearForceBefore,
bool accumulateForcesFromMappedStates,
TaskScheduler* taskScheduler)
{
computeForce(mappingGraph, result, mparams.x(), mparams.v(), clearForceBefore, accumulateForcesFromMappedStates, taskScheduler);
}

void MappingGraphMechanicalOperations::addMBKv(const MappingGraph& mappingGraph,
core::MultiVecDerivId df, core::MatricesFactors::M m,
core::MatricesFactors::B b,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,10 @@ class SOFA_SIMULATION_CORE_API MappingGraphMechanicalOperations : public Mechani
using MechanicalOperations::projectResponse;

/// Compute the current force (given the latest propagated position and velocity)
void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result,
core::ConstMultiVecCoordId xId, core::ConstMultiVecDerivId vId,
bool clearForceBefore, bool accumulateForcesFromMappedStates, TaskScheduler* taskScheduler);
SOFA_ATTRIBUTE_DEPRECATED__COMPUTEFORCE_OVERLOAD()
void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clearForceBefore, bool accumulateForcesFromMappedStates, TaskScheduler* taskScheduler);
using MechanicalOperations::computeForce;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,7 @@ void MechanicalOperations::computeEnergy(SReal &kineticEnergy, SReal &potentialE
kineticEnergy = energyVisitor.getKineticEnergy();
potentialEnergy = energyVisitor.getPotentialEnergy();
}

/// Apply projective constraints to the given velocity vector
void MechanicalOperations::projectVelocity(core::MultiVecDerivId v, SReal time)
{
Expand Down Expand Up @@ -248,15 +249,22 @@ void MechanicalOperations::accFromF(core::MultiVecDerivId a, core::ConstMultiVec
}

/// Compute the current force (given the latest propagated position and velocity)
void MechanicalOperations::computeForce(core::MultiVecDerivId result, bool clear, bool accumulate)
void MechanicalOperations::computeForce(core::MultiVecDerivId result,
core::ConstMultiVecCoordId xId,
core::ConstMultiVecDerivId vId, bool clear, bool accumulate)
{
setF(result);
if (clear)
{
executeVisitor( MechanicalResetForceVisitor(&mparams, result, false) );
//finish();
}
executeVisitor( MechanicalComputeForceVisitor(&mparams, result, accumulate) );
executeVisitor( MechanicalComputeForceVisitor(&mparams, result, xId, vId, accumulate) );
}

void MechanicalOperations::computeForce(core::MultiVecDerivId result, bool clear, bool accumulate)
{
computeForce(result, mparams.x(), mparams.v(), clear, accumulate);
}


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,9 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations
/// Compute Energy
void computeEnergy(SReal &kineticEnergy, SReal &potentialEnergy);
/// Compute the current force (given the latest propagated position and velocity)
void computeForce(core::MultiVecDerivId result, core::ConstMultiVecCoordId xId, core::ConstMultiVecDerivId vId,
bool clear = true, bool accumulate = true);
SOFA_ATTRIBUTE_DEPRECATED__COMPUTEFORCE_OVERLOAD()
void computeForce(core::MultiVecDerivId result, bool clear = true, bool accumulate = true);
/// Compute the current force delta (given the latest propagated displacement)
void computeDf(core::MultiVecDerivId df, bool clear = true, bool accumulate = true);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ void SolveVisitor::fwdInteractionForceField(Node* node, core::behavior::BaseInte
const core::MultiVecDerivId ffId = core::vec_id::write_access::externalForce;
core::MechanicalParams mparams;
mparams.setDt(dt);
forceField->addForce(&mparams, ffId);
forceField->addForce(&mparams, ffId, mparams.x(), mparams.v());
}

Visitor::Result SolveVisitor::processNodeTopDown(simulation::Node* node)
Expand Down
14 changes: 14 additions & 0 deletions Sofa/framework/Simulation/Core/src/sofa/simulation/config.h.in
Original file line number Diff line number Diff line change
Expand Up @@ -38,3 +38,17 @@
#define SOFA_ATTRIBUTE_DEPRECATED__MECHANICALGETNONDIAGONALMASSESCOUNTIVISITOR() \
SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.06", "This visitor is unused.")
#endif

#ifdef SOFA_BUILD_SOFA_SIMULATION_CORE
#define SOFA_ATTRIBUTE_DEPRECATED__MECHANICALCOMPUTEFORCEVISITOR_CONSTRUCTOR_OVERLOAD()
#else
#define SOFA_ATTRIBUTE_DEPRECATED__MECHANICALCOMPUTEFORCEVISITOR_CONSTRUCTOR_OVERLOAD() \
SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.06", "Constructor must be used by providing the x and v vectors.")
#endif

#ifdef SOFA_BUILD_SOFA_SIMULATION_CORE
#define SOFA_ATTRIBUTE_DEPRECATED__COMPUTEFORCE_OVERLOAD()
#else
#define SOFA_ATTRIBUTE_DEPRECATED__COMPUTEFORCE_OVERLOAD() \
SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.06", "computeForce must be used by providing the x and v vectors.")
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ Visitor::Result MechanicalComputeForceVisitor::fwdMappedMechanicalState(simulati

Visitor::Result MechanicalComputeForceVisitor::fwdForceField(simulation::Node* /*node*/, core::behavior::BaseForceField* ff)
{
ff->addForce(this->mparams, res);
ff->addForce(this->mparams, res, m_xId, m_vId);

return RESULT_CONTINUE;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,18 +31,33 @@ This action is typically called after a MechanicalResetForceVisitor.
*/
class SOFA_SIMULATION_CORE_API MechanicalComputeForceVisitor : public MechanicalVisitor
{
sofa::core::ConstMultiVecCoordId m_xId;
sofa::core::ConstMultiVecDerivId m_vId;

public:
sofa::core::MultiVecDerivId res;
bool accumulate; ///< Accumulate everything back to the DOFs through the mappings

MechanicalComputeForceVisitor(const sofa::core::MechanicalParams* mechaparams,
sofa::core::MultiVecDerivId resvecid, bool bAccumulate = true )
: MechanicalVisitor(mechaparams) , res(resvecid), accumulate(bAccumulate)
sofa::core::MultiVecDerivId resvecid,
sofa::core::ConstMultiVecCoordId xId,
sofa::core::ConstMultiVecDerivId vId,
bool bAccumulate = true )
: MechanicalVisitor(mechaparams),
m_xId(xId), m_vId(vId),
res(resvecid), accumulate(bAccumulate)
{
#ifdef SOFA_DUMP_VISITOR_INFO
setReadWriteVectors();
#endif
}

SOFA_ATTRIBUTE_DEPRECATED__MECHANICALCOMPUTEFORCEVISITOR_CONSTRUCTOR_OVERLOAD()
MechanicalComputeForceVisitor(const sofa::core::MechanicalParams* mechaparams,
sofa::core::MultiVecDerivId resvecid, bool bAccumulate = true )
: MechanicalComputeForceVisitor(mechaparams, resvecid, mparams->x(), mparams->v(), bAccumulate)
{}

Result fwdMechanicalState(simulation::Node* /*node*/,sofa::core::behavior::BaseMechanicalState* mm) override;
Result fwdMappedMechanicalState(simulation::Node* /*node*/,sofa::core::behavior::BaseMechanicalState* mm) override;
Result fwdForceField(simulation::Node* /*node*/,sofa::core::behavior::BaseForceField* ff) override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ Visitor::Result MechanicalIntegrationVisitor::fwdInteractionForceField(simulatio
core::MechanicalParams m_mparams(*this->params);
m_mparams.setDt(this->dt);

obj->addForce(&m_mparams, ffId);
obj->addForce(&m_mparams, ffId, m_mparams.x(), m_mparams.v());
return RESULT_CONTINUE;
}

Expand Down
4 changes: 2 additions & 2 deletions Sofa/framework/Simulation/Core/test/Visitor_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ class TestVisitorWithInteractionForceField : public simulation::MechanicalVisito
class TestForceField : public core::behavior::BaseForceField
{
public:
void addForce(const core::MechanicalParams* mparams, core::MultiVecDerivId fId) override {}
void addForce(const core::MechanicalParams* mparams, core::MultiVecDerivId fId, core::ConstMultiVecCoordId xId, core::ConstMultiVecDerivId vId) override {}
void addDForce(const core::MechanicalParams* mparams, core::MultiVecDerivId dfId) override {}
SReal getPotentialEnergy(const core::MechanicalParams* mparams) const override { return {}; }
void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override {}
Expand All @@ -114,7 +114,7 @@ class TestMass : public core::behavior::BaseMass
class TestInteractionForceField : public core::behavior::BaseInteractionForceField
{
public:
void addForce(const core::MechanicalParams* mparams, core::MultiVecDerivId fId) override {}
void addForce(const core::MechanicalParams* mparams, core::MultiVecDerivId fId, core::ConstMultiVecCoordId xId, core::ConstMultiVecDerivId vId) override {}
void addDForce(const core::MechanicalParams* mparams, core::MultiVecDerivId dfId) override {}
SReal getPotentialEnergy(const core::MechanicalParams* mparams) const override { return {}; }
};
Expand Down
Loading