Skip to content

Commit 3db7d74

Browse files
alxbilgerbakpaul
andauthored
[Core,Lagrangian] Add some useful comments (#5500)
* [Core,Lagrangian] Add some useful comments * more comments --------- Co-authored-by: Paul Baksic <30337881+bakpaul@users.noreply.github.com>
1 parent ed3c534 commit 3db7d74

16 files changed

Lines changed: 131 additions & 37 deletions

File tree

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/GenericConstraintCorrection.cpp

Lines changed: 20 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -166,9 +166,10 @@ void GenericConstraintCorrection::addComplianceInConstraintSpace(const Constrain
166166
}
167167

168168
factor *= complianceFactor;
169-
// use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
170-
l_linearSolver.get()->buildComplianceMatrix(cparams, W, factor, d_regularizationTerm.getValue());
171169

170+
// use the Linear solver to compute J*A^-1*J^T, where A is the mechanical linear system matrix
171+
// the linear solver will also be in charge to assemble J
172+
l_linearSolver.get()->buildComplianceMatrix(cparams, W, factor, d_regularizationTerm.getValue());
172173
}
173174

174175
void GenericConstraintCorrection::computeMotionCorrectionFromLambda(const ConstraintParams* cparams, MultiVecDerivId dx, const linearalgebra::BaseVector * lambda)
@@ -189,9 +190,9 @@ void GenericConstraintCorrection::applyMotionCorrection(const ConstraintParams*
189190
}
190191

191192
void GenericConstraintCorrection::applyMotionCorrection(const ConstraintParams * cparams,
192-
MultiVecCoordId xId,
193-
MultiVecDerivId vId,
194-
MultiVecDerivId dxId,
193+
MultiVecCoordId x,
194+
MultiVecDerivId v,
195+
MultiVecDerivId dx,
195196
ConstMultiVecDerivId correction)
196197
{
197198
if (!l_ODESolver.get()) return;
@@ -200,7 +201,7 @@ void GenericConstraintCorrection::applyMotionCorrection(const ConstraintParams *
200201
const SReal positionFactor = l_ODESolver.get()->getPositionIntegrationFactor() * complianceFactor;
201202
const SReal velocityFactor = l_ODESolver.get()->getVelocityIntegrationFactor() * complianceFactor;
202203

203-
applyMotionCorrection(cparams, xId, vId, dxId, correction, positionFactor, velocityFactor);
204+
applyMotionCorrection(cparams, x, v, dx, correction, positionFactor, velocityFactor);
204205
}
205206

206207
void GenericConstraintCorrection::applyPositionCorrection(const ConstraintParams * cparams,
@@ -234,9 +235,19 @@ void GenericConstraintCorrection::applyContactForce(const BaseVector *f)
234235

235236
ConstraintParams cparams(*sofa::core::execparams::defaultInstance());
236237

238+
MultiVecDerivId dx = cparams.dx();
239+
240+
// force = J^T * f
241+
// dx = A^-1 * force
242+
computeMotionCorrectionFromLambda(&cparams, dx, f);
243+
244+
//cparams.lambda() is an implicit output of computeMotionCorrectionFromLambda
245+
const MultiVecDerivId& force = cparams.lambda();
237246

238-
computeMotionCorrectionFromLambda(&cparams, cparams.dx(), f);
239-
applyMotionCorrection(&cparams, core::vec_id::write_access::position, core::vec_id::write_access::velocity, cparams.dx(), cparams.lambda());
247+
// x = x_free + force * positionFactor
248+
// v = v_free + force * velocityFactor
249+
// dx *= correctionFactor
250+
applyMotionCorrection(&cparams, core::vec_id::write_access::position, core::vec_id::write_access::velocity, dx, force);
240251
}
241252

242253
void GenericConstraintCorrection::computeResidual(const ExecParams* params, linearalgebra::BaseVector *lambda)
@@ -267,7 +278,7 @@ void GenericConstraintCorrection::resetContactForce(){}
267278

268279
void registerGenericConstraintCorrection(sofa::core::ObjectFactory* factory)
269280
{
270-
factory->registerObjects(core::ObjectRegistrationData("Generic Constraint Correction.")
281+
factory->registerObjects(core::ObjectRegistrationData("Component computing constraint forces within a simulated body using the compliance method.")
271282
.add< GenericConstraintCorrection >());
272283
}
273284

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/GenericConstraintCorrection.h

Lines changed: 60 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,8 @@
2727
namespace sofa::component::constraint::lagrangian::correction
2828
{
2929

30-
class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API GenericConstraintCorrection : public core::behavior::BaseConstraintCorrection
30+
class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API GenericConstraintCorrection
31+
: public core::behavior::BaseConstraintCorrection
3132
{
3233
public:
3334
SOFA_CLASS(GenericConstraintCorrection, core::behavior::BaseConstraintCorrection);
@@ -37,16 +38,70 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API GenericConstraintCorre
3738
void addConstraintSolver(core::behavior::ConstraintSolver *s) override;
3839
void removeConstraintSolver(core::behavior::ConstraintSolver *s) override;
3940

40-
void computeMotionCorrectionFromLambda(const core::ConstraintParams* cparams, core::MultiVecDerivId dx, const linearalgebra::BaseVector * lambda) override;
41-
41+
/**
42+
* \brief \copybrief BaseConstraintCorrection::computeMotionCorrectionFromLambda
43+
*
44+
* \details Calls the linear solver to perform the computations:
45+
* f = J^T * lambda
46+
* dx = A^-1 * f
47+
*
48+
* f is implicitly stored in cparams->lambda()
49+
*/
50+
void computeMotionCorrectionFromLambda(
51+
const core::ConstraintParams* cparams,
52+
core::MultiVecDerivId dx,
53+
const linearalgebra::BaseVector * lambda) override;
54+
55+
/**
56+
* \brief \copybrief BaseConstraintCorrection::addComplianceInConstraintSpace
57+
*
58+
* \details Calls the linear solver to perform the computation W += J A^-1 J^T
59+
*/
4260
void addComplianceInConstraintSpace(const core::ConstraintParams *cparams, linearalgebra::BaseMatrix* W) override;
4361

4462
void getComplianceMatrix(linearalgebra::BaseMatrix* ) const override;
4563

46-
void applyMotionCorrection(const core::ConstraintParams *cparams, core::MultiVecCoordId x, core::MultiVecDerivId v, core::MultiVecDerivId dx, core::ConstMultiVecDerivId correction) override;
47-
64+
/**
65+
* Compute:
66+
* x = x_free + correction * positionFactor
67+
* v = v_free + correction * velocityFactor
68+
* dx *= correctionFactor
69+
*
70+
* x_free and v_free correspond to the position and velocity of the free motion. Both vectors
71+
* are referred in \p cparams.
72+
*
73+
* positionFactor and velocityFactor are factors provided by the ODE solver.
74+
*
75+
* correctionFactor is either positionFactor or velocityFactor depending on
76+
* cparams->constOrder()
77+
*/
78+
void applyMotionCorrection(
79+
const core::ConstraintParams *cparams,
80+
core::MultiVecCoordId x,
81+
core::MultiVecDerivId v,
82+
core::MultiVecDerivId dx,
83+
core::ConstMultiVecDerivId correction) override;
84+
85+
/**
86+
* Compute:
87+
* x = x_free + correction * positionFactor
88+
* dx *= correctionFactor
89+
*
90+
* x_free corresponds to the position of the free motion. x_free is referred in \p cparams.
91+
*
92+
* positionFactor is a factor provided by the ODE solver.
93+
*/
4894
void applyPositionCorrection(const core::ConstraintParams *cparams, core::MultiVecCoordId x, core::MultiVecDerivId dx, core::ConstMultiVecDerivId correction) override;
4995

96+
/**
97+
* Compute:
98+
* v = v_free + correction * velocityFactor
99+
* dx *= correctionFactor
100+
*
101+
* v_free corresponds to the velocity of the free motion. v_free is referred in \p cparams.
102+
*
103+
* velocityFactor is a factor provided by the ODE solver.
104+
*/
50105
void applyVelocityCorrection(const core::ConstraintParams *cparams, core::MultiVecDerivId v, core::MultiVecDerivId dv, core::ConstMultiVecDerivId correction) override;
51106

52107
void applyPredictiveConstraintForce(const core::ConstraintParams *cparams, core::MultiVecDerivId f, const linearalgebra::BaseVector *lambda) override;

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/LinearSolverConstraintCorrection.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ using namespace sofa::defaulttype;
3232

3333
void registerLinearSolverConstraintCorrection(sofa::core::ObjectFactory* factory)
3434
{
35-
factory->registerObjects(core::ObjectRegistrationData("Constraint Correction for Linear Solvers.")
35+
factory->registerObjects(core::ObjectRegistrationData("Component computing constraint forces within a simulated body using the compliance method.")
3636
.add< LinearSolverConstraintCorrection<Vec3Types> >()
3737
.add< LinearSolverConstraintCorrection<Vec2Types> >()
3838
.add< LinearSolverConstraintCorrection<Vec1Types> >()

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/LinearSolverConstraintCorrection.inl

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -211,13 +211,14 @@ void LinearSolverConstraintCorrection<DataTypes>::addComplianceInConstraintSpace
211211
break;
212212
}
213213

214+
// J is read from the mechanical state and converted to m_constraintJacobian
214215
{
215216
helper::ReadAccessor inputConstraintMatrix ( *cparams->readJ(this->mstate.get()) );
216217
const sofa::SignedIndex numberOfConstraints = W->rowSize();
217218
convertConstraintMatrix(numberOfConstraints, inputConstraintMatrix.ref());
218219
}
219220

220-
// use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
221+
// use the Linear solver to compute J*A^-1*J^T, where A is the mechanical linear system matrix
221222
l_linearSolver->setSystemLHVector(sofa::core::MultiVecDerivId::null());
222223
l_linearSolver->addJMInvJt(W, &m_constraintJacobian, factor);
223224

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/PrecomputedConstraintCorrection.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -125,7 +125,7 @@ using namespace sofa::defaulttype;
125125

126126
void registerPrecomputedConstraintCorrection(sofa::core::ObjectFactory* factory)
127127
{
128-
factory->registerObjects(core::ObjectRegistrationData("Component precomputing constraint forces within a simulated body using the compliance method.")
128+
factory->registerObjects(core::ObjectRegistrationData("Component precomputing constraint forces within a simulated body using the compliance method. It approximates the compliance matrix by a precomputed matrix inverse. The approximation can be updated based on the rotation of elements.")
129129
.add< PrecomputedConstraintCorrection<Vec3Types> >()
130130
.add< PrecomputedConstraintCorrection<Vec1Types> >()
131131
.add< PrecomputedConstraintCorrection<Rigid3Types> >());

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/PrecomputedConstraintCorrection.inl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ namespace sofa::component::constraint::lagrangian::correction
5353
template<class DataTypes>
5454
PrecomputedConstraintCorrection<DataTypes>::PrecomputedConstraintCorrection(sofa::core::behavior::MechanicalState<DataTypes> *mm)
5555
: Inherit(mm)
56-
, d_rotations(initData(&d_rotations, false, "rotations", ""))
56+
, d_rotations(initData(&d_rotations, false, "rotations", "Project the precomputed matrix with a rotation matrix"))
5757
, d_restRotations(initData(&d_restRotations, false, "restDeformations", ""))
5858
, d_recompute(initData(&d_recompute, false, "recompute", "if true, always recompute the compliance"))
5959
, d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints"))

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/UncoupledConstraintCorrection.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -157,7 +157,7 @@ SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API void UncoupledConstraintCorr
157157

158158
void registerUncoupledConstraintCorrection(sofa::core::ObjectFactory* factory)
159159
{
160-
factory->registerObjects(core::ObjectRegistrationData("Component computing constraint forces within a simulated body using the compliance method.")
160+
factory->registerObjects(core::ObjectRegistrationData("Component computing constraint forces within a simulated body using the compliance method, approximating the compliance matrix by a diagonal matrix.")
161161
.add< UncoupledConstraintCorrection< Vec1Types > >()
162162
.add< UncoupledConstraintCorrection< Vec2Types > >()
163163
.add< UncoupledConstraintCorrection< Vec3Types > >()

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h

Lines changed: 16 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,8 +38,16 @@ namespace sofa::component::constraint::lagrangian::solver
3838
class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem
3939
{
4040
public:
41+
// The compliance matrix projected in the constraint space
42+
// If J is the jacobian matrix of the constraints and A is the mechanical matrix of the system,
43+
// then W = J A^{-1} J^T
4144
sofa::linearalgebra::LPtrFullMatrix<SReal> W;
42-
sofa::linearalgebra::FullVector<SReal> dFree, f;
45+
46+
// The constraint values of the "free motion" state
47+
sofa::linearalgebra::FullVector<SReal> dFree;
48+
49+
// The lambda values from the Lagrange multipliers
50+
sofa::linearalgebra::FullVector<SReal> f;
4351

4452
ConstraintProblem();
4553
virtual ~ConstraintProblem();
@@ -48,10 +56,13 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem
4856
int maxIterations;
4957

5058
virtual void clear(int nbConstraints);
51-
int getDimension() { return dimension; }
52-
SReal** getW() { return W.lptr(); }
53-
SReal* getDfree() { return dFree.ptr(); }
54-
SReal* getF() { return f.ptr(); }
59+
60+
// Returns the number of scalar constraints, or equivalently the number of Lagrange multipliers
61+
int getDimension() const { return dimension; }
62+
63+
SReal** getW() { return W.lptr(); }
64+
SReal* getDfree() { return dFree.ptr(); }
65+
SReal* getF() { return f.ptr(); }
5566

5667
virtual void solveTimed(SReal tolerance, int maxIt, SReal timeout) = 0;
5768

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -197,6 +197,7 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams,
197197
// suppress the constraints that are on DOFS currently concerned by projective constraint
198198
applyProjectiveConstraintOnConstraintMatrix(cParams);
199199

200+
//clear and/or resize based on the number of constraints
200201
current_cp->clear(numConstraints);
201202

202203
getConstraintViolation(cParams, &current_cp->dFree);

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,9 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver :
138138
const core::ConstraintParams* cParams,
139139
MultiVecId res1, MultiVecId res2,
140140
core::behavior::BaseConstraintCorrection* constraintCorrection) const;
141+
142+
// Accumulate the lambda values projected in the motion space in the states
143+
// f += J^T * lambda
141144
void storeConstraintLambdas(const core::ConstraintParams* cParams);
142145

143146
};

0 commit comments

Comments
 (0)