Skip to content

Commit

Permalink
[all] Various changes relative to the constraint matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@alxbilger @fredroy
alxbilger authored and fredroy committed Oct 10, 2024
1 parent 95beb4a commit 7c7c930
Show file tree
Hide file tree
Showing 13 changed files with 409 additions and 146 deletions.
17 changes: 13 additions & 4 deletions ...on/src/sofa/component/constraint/lagrangian/correction/LinearSolverConstraintCorrection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ class LinearSolverConstraintCorrection : public sofa::core::behavior::Constraint
protected:
LinearSolverConstraintCorrection(sofa::core::behavior::MechanicalState<DataTypes> *mm = nullptr);

virtual ~LinearSolverConstraintCorrection();
~LinearSolverConstraintCorrection() override;
public:
void init() override;

Expand Down Expand Up @@ -110,13 +110,22 @@ class LinearSolverConstraintCorrection : public sofa::core::behavior::Constraint
void getBlockDiagonalCompliance(linearalgebra::BaseMatrix* W, int begin, int end) override;

protected:
linearalgebra::SparseMatrix<SReal> J; ///< constraint matrix
DeprecatedAndRemoved J; ///< use m_constraintMatrix instead

linearalgebra::SparseMatrix<Real> m_constraintMatrix;

SOFA_ATTRIBUTE_DEPRECATED__FORCES_IN_LINEARSOLVERCONSTRAINTCORRECTION()
linearalgebra::FullVector<SReal> F; ///< forces computed from the constraints

/**
* @brief Compute the compliance matrix
* @brief Convert the constraint matrix
*/
virtual void computeJ(sofa::linearalgebra::BaseMatrix* W, const MatrixDeriv& j);
void convertConstraintMatrix(sofa::SignedIndex numberOfConstraints, const MatrixDeriv& inputConstraintMatrix);

virtual void computeJ(sofa::linearalgebra::BaseMatrix* W, const MatrixDeriv& j)
{
convertConstraintMatrix(W->rowSize(), j);
}


////////////////////////// Inherited attributes ////////////////////////////
Expand Down
80 changes: 44 additions & 36 deletions .../src/sofa/component/constraint/lagrangian/correction/LinearSolverConstraintCorrection.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@
#include <list>

#include <sofa/component/linearsolver/iterative/GraphScatteredTypes.h>
#include <sofa/helper/ScopedAdvancedTimer.h>


namespace sofa::component::constraint::lagrangian::correction
{
Expand Down Expand Up @@ -84,9 +86,9 @@ void LinearSolverConstraintCorrection<DataTypes>::init()
}
else
{
if (l_linearSolver.get()->getTemplateName() == "GraphScattered")
if (l_linearSolver->getTemplateName() == "GraphScattered")
{
msg_error() << "Can not use the solver " << l_linearSolver.get()->getName() << " because it is templated on GraphScatteredType";
msg_error() << "Can not use the solver " << l_linearSolver->getName() << " because it is templated on GraphScatteredType";
sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
return;
}
Expand Down Expand Up @@ -130,21 +132,24 @@ void LinearSolverConstraintCorrection<DataTypes>::init()
}

template<class TDataTypes>
void LinearSolverConstraintCorrection<TDataTypes>::computeJ(sofa::linearalgebra::BaseMatrix* W, const MatrixDeriv& c)
void LinearSolverConstraintCorrection<TDataTypes>::convertConstraintMatrix(const sofa::SignedIndex numberOfConstraints, const MatrixDeriv& inputConstraintMatrix)
{
if(d_componentState.getValue() != ComponentState::Valid)
if (d_componentState.getValue() != ComponentState::Valid)
{
return ;
}

SCOPED_TIMER("convertConstraintMatrix");

const unsigned int numDOFs = mstate->getSize();
const unsigned int N = Deriv::size();
const unsigned int numDOFReals = numDOFs*N;
const unsigned int totalNumConstraints = W->rowSize();
static constexpr unsigned int N = Deriv::size();
const unsigned int numDOFReals = numDOFs * N;

J.resize(totalNumConstraints, numDOFReals);
m_constraintMatrix.resize(numberOfConstraints, numDOFReals);

MatrixDerivRowConstIterator rowItEnd = c.end();
MatrixDerivRowConstIterator rowItEnd = inputConstraintMatrix.end();

for (MatrixDerivRowConstIterator rowIt = c.begin(); rowIt != rowItEnd; ++rowIt)
for (MatrixDerivRowConstIterator rowIt = inputConstraintMatrix.begin(); rowIt != rowItEnd; ++rowIt)
{
const int cid = rowIt.index();

Expand All @@ -153,11 +158,11 @@ void LinearSolverConstraintCorrection<TDataTypes>::computeJ(sofa::linearalgebra:
for (MatrixDerivColConstIterator colIt = rowIt.begin(); colIt != colItEnd; ++colIt)
{
const unsigned int dof = colIt.index();
const Deriv n = colIt.val();
const Deriv& n = colIt.val();

for (unsigned int r = 0; r < N; ++r)
{
J.add(cid, dof * N + r, n[r]);
m_constraintMatrix.add(cid, dof * N + r, n[r]);
}
}
}
Expand Down Expand Up @@ -188,19 +193,22 @@ void LinearSolverConstraintCorrection<DataTypes>::addComplianceInConstraintSpace
break;
}

// Compute J
this->computeJ(W, cparams->readJ(this->mstate)->getValue());
{
helper::ReadAccessor inputConstraintMatrix ( *cparams->readJ(this->mstate) );
const sofa::SignedIndex numberOfConstraints = W->rowSize();
convertConstraintMatrix(numberOfConstraints, inputConstraintMatrix.ref());
}

// use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
l_linearSolver.get()->setSystemLHVector(sofa::core::MultiVecDerivId::null());
l_linearSolver.get()->addJMInvJt(W, &J, factor);
l_linearSolver->setSystemLHVector(sofa::core::MultiVecDerivId::null());
l_linearSolver->addJMInvJt(W, &m_constraintMatrix, factor);
}


template<class DataTypes>
void LinearSolverConstraintCorrection<DataTypes>::rebuildSystem(SReal massFactor, SReal forceFactor)
{
l_linearSolver.get()->rebuildSystem(massFactor, forceFactor);
l_linearSolver->rebuildSystem(massFactor, forceFactor);
}

template<class DataTypes>
Expand All @@ -225,17 +233,17 @@ void LinearSolverConstraintCorrection<DataTypes>::getComplianceMatrix(linearalge
Minv->resize(numDOFReals,numDOFReals);

// use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
l_linearSolver.get()->addJMInvJt(Minv, &J, factor);
l_linearSolver->addJMInvJt(Minv, &J, factor);
}

template< class DataTypes >
void LinearSolverConstraintCorrection< DataTypes >::computeMotionCorrection(const core::ConstraintParams* /*cparams*/, core::MultiVecDerivId dx, core::MultiVecDerivId f)
{
if (mstate && l_linearSolver.get())
{
l_linearSolver.get()->setSystemRHVector(f);
l_linearSolver.get()->setSystemLHVector(dx);
l_linearSolver.get()->solveSystem();
l_linearSolver->setSystemRHVector(f);
l_linearSolver->setSystemLHVector(dx);
l_linearSolver->solveSystem();
}
}

Expand Down Expand Up @@ -359,9 +367,9 @@ void LinearSolverConstraintCorrection<DataTypes>::applyContactForce(const linear
}
}
}
l_linearSolver.get()->setSystemRHVector(forceID);
l_linearSolver.get()->setSystemLHVector(dxID);
l_linearSolver.get()->solveSystem();
l_linearSolver->setSystemRHVector(forceID);
l_linearSolver->setSystemLHVector(dxID);
l_linearSolver->solveSystem();

//TODO: tell the solver not to recompute the matrix

Expand Down Expand Up @@ -529,13 +537,13 @@ void LinearSolverConstraintCorrection<DataTypes>::resetForUnbuiltResolution(SRea
core::VecDerivId forceID(core::VecDerivId::V_FIRST_DYNAMIC_INDEX);
core::VecDerivId dxID = core::VecDerivId::dx();

l_linearSolver.get()->setSystemRHVector(forceID);
l_linearSolver.get()->setSystemLHVector(dxID);
l_linearSolver->setSystemRHVector(forceID);
l_linearSolver->setSystemLHVector(dxID);


systemMatrix_buf = l_linearSolver.get()->getSystemBaseMatrix();
systemRHVector_buf = l_linearSolver.get()->getSystemRHBaseVector();
systemLHVector_buf = l_linearSolver.get()->getSystemLHBaseVector();
systemMatrix_buf = l_linearSolver->getSystemBaseMatrix();
systemRHVector_buf = l_linearSolver->getSystemRHBaseVector();
systemLHVector_buf = l_linearSolver->getSystemLHBaseVector();
systemLHVector_buf_fullvector = dynamic_cast<linearalgebra::FullVector<Real>*>(systemLHVector_buf); // Cast checking whether the LH vector is a FullVector to improve performances

constexpr const auto derivDim = Deriv::total_size;
Expand All @@ -550,7 +558,7 @@ void LinearSolverConstraintCorrection<DataTypes>::resetForUnbuiltResolution(SRea
}

// Init the internal data of the solver for partial solving
l_linearSolver.get()->init_partial_solve();
l_linearSolver->init_partial_solve();


///////// new : precalcul des liste d'indice ///////
Expand Down Expand Up @@ -687,14 +695,14 @@ void LinearSolverConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(lin
const SReal factor = l_ODESolver.get()->getPositionIntegrationFactor(); //*m_ODESolver->getPositionIntegrationFactor(); // dt*dt

const unsigned int numDOFs = mstate->getSize();
const unsigned int N = Deriv::size();
const unsigned int numDOFReals = numDOFs*N;
static constexpr unsigned int N = Deriv::size();
const unsigned int numDOFReals = numDOFs * N;

// Compute J
const MatrixDeriv& constraints = mstate->read(core::ConstMatrixDerivId::constraintJacobian())->getValue();
const unsigned int totalNumConstraints = W->rowSize();
const sofa::SignedIndex totalNumConstraints = W->rowSize();

J.resize(totalNumConstraints, numDOFReals);
m_constraintMatrix.resize(totalNumConstraints, numDOFReals);

for (int i = begin; i <= end; i++)
{
Expand All @@ -715,7 +723,7 @@ void LinearSolverConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(lin
const Deriv n = colIt.val();

for (unsigned int r = 0; r < N; ++r)
J.add(i, dof * N + r, n[r]);
m_constraintMatrix.add(i, dof * N + r, n[r]);

if (debug!=0)
{
Expand All @@ -730,7 +738,7 @@ void LinearSolverConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(lin
}

// use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
l_linearSolver.get()->addJMInvJt(W, &J, factor);
l_linearSolver->addJMInvJt(W, &m_constraintMatrix, factor);

// construction of Vec_I_list_dof : vector containing, for each constraint block, the list of dof concerned

Expand Down
11 changes: 10 additions & 1 deletion ...int/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/config.h.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,4 +44,13 @@ namespace sofa::component::constraint::lagrangian::correction
SOFA_ATTRIBUTE_DEPRECATED( \
"v24.06", "v24.12", \
"Data renamed according to the guidelines")
#endif
#endif

#ifdef SOFA_BUILD_SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION
#define SOFA_ATTRIBUTE_DEPRECATED__FORCES_IN_LINEARSOLVERCONSTRAINTCORRECTION()
#else
#define SOFA_ATTRIBUTE_DEPRECATED__FORCES_IN_LINEARSOLVERCONSTRAINTCORRECTION() \
SOFA_ATTRIBUTE_DEPRECATED( \
"v24.12", "v25.06", \
"Class member 'LinearSolverConstraintCorrection::F' is not used.")
#endif
83 changes: 47 additions & 36 deletions ...ent/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/MatrixLinearSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include <sofa/linearalgebra/RotationMatrix.h>
#include <sofa/component/linearsystem/TypedMatrixLinearSystem.h>
#include <sofa/component/linearsolver/iterative/MatrixLinearSystem[GraphScattered].h>
#include <sofa/type/trait/Rebind.h>

#if SOFA_CORE_ENABLE_CRSMULTIMATRIXACCESSOR
#include <sofa/core/behavior/CRSMultiMatrixAccessor.h>
Expand Down Expand Up @@ -86,25 +87,6 @@ class MatrixLinearSolverInternalData
typedef sofa::linearalgebra::SparseMatrix<Real> JMatrixType;
typedef linearalgebra::BaseMatrix ResMatrixType;

template<typename MReal>
JMatrixType * copyJmatrix(linearalgebra::SparseMatrix<MReal> * J)
{
J_local.clear();
J_local.resize(J->rowSize(),J->colSize());

for (auto jit1 = J->begin(); jit1 != J->end(); jit1++)
{
auto l = jit1->first;
for (auto i1 = jit1->second.begin(); i1 != jit1->second.end(); i1++)
{
auto c = i1->first;
MReal val = i1->second;
J_local.set(l,c,val);
}
}
return &J_local;
}

void projectForceInConstraintSpace(linearalgebra::BaseVector* r,const linearalgebra::BaseVector* f) {
for (typename linearalgebra::SparseMatrix<Real>::LineConstIterator jit = J_local.begin(), jitend = J_local.end(); jit != jitend; ++jit) {
auto row = jit->first;
Expand All @@ -121,35 +103,43 @@ class MatrixLinearSolverInternalData
return &J_local;
}

/**
* Returns a JMatrixType as a pointer to the input matrix (if the input
* matrix type is a JMatrixType), or a copy of the input matrix as a pointer
* to the class member @J_local.
*/
JMatrixType * getLocalJ(linearalgebra::BaseMatrix * J)
{
if (JMatrixType * j = dynamic_cast<JMatrixType *>(J))
{
return j;
}
else if (linearalgebra::SparseMatrix<double> * j = dynamic_cast<linearalgebra::SparseMatrix<double> *>(J))
{
return copyJmatrix(j);
}
else if (linearalgebra::SparseMatrix<float> * j = dynamic_cast<linearalgebra::SparseMatrix<float> *>(J))

using OtherReal = std::conditional_t<std::is_same_v<Real, double>, float, double>;

//in case the matrix J is not the same type as JMatrixType, it is
//copied in the local variable. There are 2 cases:

//Case 1: J can be rebound: the copy is optimized
if (auto * j_d = dynamic_cast<sofa::type::rebind_to<JMatrixType, OtherReal> *>(J))
{
return copyJmatrix(j);
return convertMatrix(*j_d);
}
else
{
J_local.clear();
J_local.resize(J->rowSize(),J->colSize());

for (typename JMatrixType::Index j=0; j<J->rowSize(); j++)
//Case 2: generic case: slow copy
J_local.clear();
J_local.resize(J->rowSize(),J->colSize());

using Index = typename JMatrixType::Index;
for (Index j = 0; j < J->rowSize(); ++j)
{
for (Index i = 0; i < J->colSize(); ++i)
{
for (typename JMatrixType::Index i=0; i<J->colSize(); i++)
{
J_local.set(j,i,J->element(j,i));
}
J_local.set(j, i, J->element(j, i));
}

return &J_local;
}

return &J_local;
}

ResMatrixType * getLocalRes(linearalgebra::BaseMatrix * R)
Expand All @@ -161,6 +151,27 @@ class MatrixLinearSolverInternalData
void addLocalRes(linearalgebra::BaseMatrix * /*R*/)
{}

protected:

template<typename MReal>
JMatrixType * convertMatrix(const linearalgebra::SparseMatrix<MReal>& inputMatrix)
{
J_local.clear();
J_local.resize(inputMatrix.rowSize(), inputMatrix.colSize());

for (auto jit1 = inputMatrix.begin(); jit1 != inputMatrix.end(); ++jit1)
{
const auto l = jit1->first;
for (auto i1 = jit1->second.begin(); i1 != jit1->second.end(); ++i1)
{
const auto c = i1->first;
const MReal val = i1->second;
J_local.set(l, c, val);
}
}
return &J_local;
}

private :
JMatrixType J_local;
};
Expand Down
Loading

0 comments on commit 7c7c930

Please sign in to comment.