diff --git a/doc/examples/CaduceusRevisited/reduced_EnrichedCaduceus.py b/doc/examples/CaduceusRevisited/reduced_EnrichedCaduceus.py index 5a885ca3..029ec9da 100644 --- a/doc/examples/CaduceusRevisited/reduced_EnrichedCaduceus.py +++ b/doc/examples/CaduceusRevisited/reduced_EnrichedCaduceus.py @@ -65,9 +65,9 @@ def Reduced_test( Snake_MOR = modelRoot.addChild('Snake_MOR') Snake_MOR.addObject('EulerImplicitSolver' , rayleighStiffness = '0.1', rayleighMass = '0.1') Snake_MOR.addObject('SparseLDLSolver' , name = 'preconditioner') - Snake_MOR.addObject('GenericConstraintCorrection' , solverName = 'preconditioner') + Snake_MOR.addObject('GenericConstraintCorrection' , linearSolver = "@preconditioner") Snake_MOR.addObject('MechanicalObject' , position = [0]*nbrOfModes, template = 'Vec1d') - Snake_MOR.addObject('MechanicalMatrixMapperMOR' , object1 = '@./MechanicalObject', object2 = '@./MechanicalObject', listActiveNodesPath = path + r'/data/listActiveNodes.txt', template = 'Vec1d,Vec1d', usePrecomputedMass = True, timeInvariantMapping2 = True, performECSW = hyperReduction, timeInvariantMapping1 = True, precomputedMassPath = path + r'/data/UniformMass_reduced.txt', nodeToParse = '@./Snake') + #Snake_MOR.addObject('MechanicalMatrixMapperMOR' , object1 = '@./MechanicalObject', object2 = '@./MechanicalObject', listActiveNodesPath = path + r'/data/listActiveNodes.txt', template = 'Vec1d,Vec1d', usePrecomputedMass = True, timeInvariantMapping2 = True, performECSW = hyperReduction, timeInvariantMapping1 = True, precomputedMassPath = path + r'/data/UniformMass_reduced.txt', nodeToParse = '@./Snake') actuatorDummy = modelRoot.addChild('actuatorDummy') @@ -132,7 +132,7 @@ def createScene(rootNode): Reduced_test(rootNode, name="Reduced_test", - surfaceMeshFileName=surfaceMeshFileName) + surfaceMeshFileName=surfaceMeshFileName, hyperReduction=True) base = rootNode.addChild("base") stick = base.addChild("stick") diff --git a/doc/examples/liver/mor/myReduced_liver.py b/doc/examples/liver/mor/myReduced_liver.py index b8bb850c..746eb765 100644 --- a/doc/examples/liver/mor/myReduced_liver.py +++ b/doc/examples/liver/mor/myReduced_liver.py @@ -61,13 +61,13 @@ def Reduced_liver( liver_MOR = attachedTo.addChild(name) liver_MOR.addObject('EulerImplicitSolver' , rayleighStiffness=0.1, rayleighMass=0.1) liver_MOR.addObject('SparseLDLSolver',name='preconditioner') - liver_MOR.addObject('GenericConstraintCorrection', solverName='preconditioner') + liver_MOR.addObject('GenericConstraintCorrection', linearSolver='@preconditioner') if POD: liver_MOR.addObject('MechanicalObject' , position = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], template = 'Vec1d') else: liver_MOR.addObject('MechanicalObject' , position = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], template = 'Vec1d') - liver_MOR.addObject('MechanicalMatrixMapperMOR' , object1 = '@./MechanicalObject', object2 = '@./MechanicalObject', listActiveNodesPath = path + '/data/conectivity_liver.txt', template = 'Vec1d,Vec1d', performECSW = True, nodeToParse = '@./liver',printLog=False) + #liver_MOR.addObject('MechanicalMatrixMapperMOR' , object1 = '@./MechanicalObject', object2 = '@./MechanicalObject', listActiveNodesPath = path + '/data/conectivity_liver.txt', template = 'Vec1d,Vec1d', performECSW = True, nodeToParse = '@./liver',printLog=False) liver = liver_MOR.addChild('liver') @@ -123,7 +123,7 @@ def Reduced_liver( def createScene(rootNode): surfaceMeshFileName = 'liver-smoothUV.obj' - MainHeader(rootNode,plugins=["SofaPython","SoftRobots","ModelOrderReduction"], + MainHeader(rootNode,plugins=["SofaPython3","SoftRobots","ModelOrderReduction"], dt=0.01, gravity=[0.0,-981, 0.0]) rootNode.addObject('FreeMotionAnimationLoop') diff --git a/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.h b/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.h index 0d785d6c..dab6f3a8 100644 --- a/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.h +++ b/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.h @@ -240,9 +240,7 @@ class HyperReducedTetrahedronFEMForceField : public virtual TetrahedronFEMForceF // Make other overloaded version of getPotentialEnergy() to show up in subclass. using TetrahedronFEMForceField::getPotentialEnergy; - virtual void addKToMatrix(sofa::linearalgebra::BaseMatrix *m, SReal kFactor, unsigned int &offset) override; - virtual void addKToMatrix(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/ ) override; - + virtual void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; void draw(const core::visual::VisualParams* vparams) override; protected: diff --git a/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.inl b/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.inl index 647720e7..78c9792a 100644 --- a/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.inl +++ b/src/ModelOrderReduction/component/forcefield/HyperReducedTetrahedronFEMForceField.inl @@ -16,6 +16,7 @@ ******************************************************************************/ #pragma once +#include "sofa/core/behavior/BaseLocalForceFieldMatrix.h" #include #include #include @@ -380,13 +381,22 @@ inline void HyperReducedTetrahedronFEMForceField::addForce (const cor template inline void HyperReducedTetrahedronFEMForceField::addDForce(const core::MechanicalParams* mparams, DataVecDeriv& d_df, const DataVecDeriv& d_dx) { - VecDeriv& df = *d_df.beginEdit(); - const VecDeriv& dx = d_dx.getValue(); - Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); +// VecDeriv& df = *d_df.beginEdit(); +// const VecDeriv& dx = d_dx.getValue(); +// Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + +// sofa::helper::AdvancedTimer::stepBegin("time in AddDDDForce"); + +// df.resize(dx.size()); - sofa::helper::AdvancedTimer::stepBegin("time in AddDDDForce"); + auto dfAccessor = sofa::helper::getWriteAccessor(d_df); + VecDeriv& df = dfAccessor.wref(); + const VecDeriv& dx = d_dx.getValue(); df.resize(dx.size()); + + const Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); + unsigned int i; typename VecElement::const_iterator it,it0; if( method == SMALL ) @@ -637,211 +647,59 @@ void HyperReducedTetrahedronFEMForceField::draw(const core::visual::V } -template -void HyperReducedTetrahedronFEMForceField::addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ) -{ - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - if (r) - addKToMatrix(r.matrix, mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()), r.offset); - else msg_error(this) << "addKToMatrix found no valid matrix accessor." ; -} - -template -void HyperReducedTetrahedronFEMForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix *mat, SReal k, unsigned int &offset) +template +void HyperReducedTetrahedronFEMForceField::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) { - // Build Matrix Block for this ForceField - unsigned int i,j,n1, n2, row, column, ROW, COLUMN , IT; - - Transformation Rot; - StiffnessMatrix JKJt,tmp; + StiffnessMatrix JKJt, RJKJtRt; + sofa::type::Mat<3, 3, Real> localMatrix(type::NOINIT); - typename VecElement::const_iterator it, it0; + static constexpr Transformation identity = [] + { + Transformation i; + i.identity(); + return i; + }(); - Index noeud1, noeud2; - int offd3 = offset/3; + constexpr auto S = DataTypes::deriv_total_size; // size of node blocks + constexpr auto N = Element::size(); - Rot[0][0]=Rot[1][1]=Rot[2][2]=1; - Rot[0][1]=Rot[0][2]=0; - Rot[1][0]=Rot[1][2]=0; - Rot[2][0]=Rot[2][1]=0; + auto dfdx = matrix->getForceDerivativeIn(this->mstate) + .withRespectToPositionsIn(this->mstate); + sofa::Size tetraId = 0; - it0=_indexedElements->begin(); - unsigned int nbElementsConsidered; + typename VecElement::const_iterator it; + auto it0=_indexedElements->begin(); + int nbElementsConsidered; if (!d_performECSW.getValue()) nbElementsConsidered = _indexedElements->size(); else nbElementsConsidered = m_RIDsize; - SReal kTimesWeight; - if (sofa::linearalgebra::CompressedRowSparseMatrix > * crsmat = dynamic_cast > * >(mat)) + for( unsigned int numElem = 0 ; numElemcomputeStiffnessMatrix(JKJt,tmp,materialsStiffnesses[IT], strainDisplacements[IT],Rot); - else this->computeStiffnessMatrix(JKJt,tmp,materialsStiffnesses[IT], strainDisplacements[IT],rotations[IT]); - - if (!d_performECSW.getValue()) - kTimesWeight = k; - else - kTimesWeight = k*weights(IT); - - type::Mat<3,3,double> tmpBlock[4][4]; - // find index of node 1 - for (n1=0; n1<4; n1++) - { - for(i=0; i<3; i++) - { - for (n2=0; n2<4; n2++) - { - for (j=0; j<3; j++) - { - tmpBlock[n1][n2][i][j] = - tmp[n1*3+i][n2*3+j]*kTimesWeight; - } - } - } - } - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[0],true) += tmpBlock[0][0]; - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[1],true) += tmpBlock[0][1]; - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[2],true) += tmpBlock[0][2]; - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[3],true) += tmpBlock[0][3]; - - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[0],true) += tmpBlock[1][0]; - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[1],true) += tmpBlock[1][1]; - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[2],true) += tmpBlock[1][2]; - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[3],true) += tmpBlock[1][3]; - - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[0],true) += tmpBlock[2][0]; - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[1],true) += tmpBlock[2][1]; - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[2],true) += tmpBlock[2][2]; - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[3],true) += tmpBlock[2][3]; - - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[0],true) += tmpBlock[3][0]; - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[1],true) += tmpBlock[3][1]; - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[2],true) += tmpBlock[3][2]; - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[3],true) += tmpBlock[3][3]; + if (!d_performECSW.getValue()){ + tetraId = numElem; } - - } - else if (sofa::linearalgebra::CompressedRowSparseMatrix > * crsmat = dynamic_cast > * >(mat)) - { - for( unsigned int numElem = 0 ; numElemcomputeStiffnessMatrix(JKJt,tmp,materialsStiffnesses[IT], strainDisplacements[IT],Rot); - else this->computeStiffnessMatrix(JKJt,tmp,materialsStiffnesses[IT], strainDisplacements[IT],rotations[IT]); - - - if (!d_performECSW.getValue()) - kTimesWeight = k; - else - kTimesWeight = k*weights(IT); - - type::Mat<3,3,double> tmpBlock[4][4]; - // find index of node 1 - for (n1=0; n1<4; n1++) - { - for(i=0; i<3; i++) - { - for (n2=0; n2<4; n2++) - { - for (j=0; j<3; j++) - { - tmpBlock[n1][n2][i][j] = - tmp[n1*3+i][n2*3+j]*kTimesWeight; - } - } - } - } - - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[0],true) += tmpBlock[0][0]; - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[1],true) += tmpBlock[0][1]; - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[2],true) += tmpBlock[0][2]; - *crsmat->wbloc(offd3 + (*it)[0], offd3 + (*it)[3],true) += tmpBlock[0][3]; - - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[0],true) += tmpBlock[1][0]; - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[1],true) += tmpBlock[1][1]; - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[2],true) += tmpBlock[1][2]; - *crsmat->wbloc(offd3 + (*it)[1], offd3 + (*it)[3],true) += tmpBlock[1][3]; - - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[0],true) += tmpBlock[2][0]; - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[1],true) += tmpBlock[2][1]; - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[2],true) += tmpBlock[2][2]; - *crsmat->wbloc(offd3 + (*it)[2], offd3 + (*it)[3],true) += tmpBlock[2][3]; - - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[0],true) += tmpBlock[3][0]; - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[1],true) += tmpBlock[3][1]; - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[2],true) += tmpBlock[3][2]; - *crsmat->wbloc(offd3 + (*it)[3], offd3 + (*it)[3],true) += tmpBlock[3][3]; + tetraId = reducedIntegrationDomain(numElem); } - } - else - { - for( unsigned int numElem = 0 ; numElemcomputeStiffnessMatrix(JKJt,tmp,materialsStiffnesses[IT], strainDisplacements[IT],Rot); - else - this->computeStiffnessMatrix(JKJt,tmp,materialsStiffnesses[IT], strainDisplacements[IT],rotations[IT]); + it = it0 + tetraId; - if (!d_performECSW.getValue()) - kTimesWeight = k; - else - kTimesWeight = k*weights(IT); + const auto& rotation = method == SMALL ? identity : rotations[tetraId]; + this->computeStiffnessMatrix(JKJt, RJKJtRt, materialsStiffnesses[tetraId], strainDisplacements[tetraId], rotation); - // find index of node 1 - for (n1=0; n1<4; n1++) + for (sofa::Index n1 = 0; n1 < N; n1++) + { + for (sofa::Index n2 = 0; n2 < N; n2++) { - noeud1 = (*it)[n1]; - - for(i=0; i<3; i++) - { - ROW = offset+3*noeud1+i; - row = 3*n1+i; - // find index of node 2 - for (n2=0; n2<4; n2++) - { - noeud2 = (*it)[n2]; - - - for (j=0; j<3; j++) - { - COLUMN = offset+3*noeud2+j; - column = 3*n2+j; - mat->add(ROW, COLUMN, - tmp[row][column]*kTimesWeight); - } - } - } + RJKJtRt.getsub(S * n1, S * n2, localMatrix); //extract the submatrix corresponding to the coupling of nodes n1 and n2 + dfdx((*it)[n1] * S, (*it)[n2] * S) += -localMatrix*weights(tetraId); } - } } } + + } // namespace sofa::component::forcefield