Merge branch 'master' of https://github.com/sofa-framework/sofa

Author: SperryBot

Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/PenalityContactForceField.h

@@ -123,6 +123,7 @@ class PenalityContactForceField : public core::behavior::PairInteractionForceFie
 
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
 
+    using Inherit::getPotentialEnergy;
     SReal getPotentialEnergy(const sofa::core::MechanicalParams*, const DataVecCoord&, const DataVecCoord& ) const override;
 
     const type::vector< Contact >& getContact() const { return contacts.getValue();}

Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.h

@@ -118,7 +118,8 @@ class QuadPressureForceField : public core::behavior::ForceField<DataTypes>
     void buildStiffnessMatrix(core::behavior::StiffnessMatrix* /*matrix*/) override;
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
 
-    SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord&  /* x */) const override { msg_warning() << "Method getPotentialEnergy not implemented yet."; return 0.0; }
+    using Inherit1::getPotentialEnergy;
+    SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord&  /* x */) const override;
 
     void draw(const core::visual::VisualParams* vparams) override;
 

Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.inl

@@ -232,7 +232,15 @@ void QuadPressureForceField<DataTypes>::buildDampingMatrix(core::behavior::Dampi
     // No damping in this ForceField
 }
 
-template<class DataTypes>
+template <class DataTypes>
+SReal QuadPressureForceField<DataTypes>::getPotentialEnergy(const core::MechanicalParams*,
+                                                            const DataVecCoord&) const
+{
+    msg_warning() << "Method getPotentialEnergy not implemented yet.";
+    return 0.0;
+}
+
+template <class DataTypes>
 void QuadPressureForceField<DataTypes>::draw(const core::visual::VisualParams* vparams)
 {
     const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle();

Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/SurfacePressureForceField.h

@@ -103,6 +103,7 @@ class SurfacePressureForceField : public core::behavior::ForceField<DataTypes>
     void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override;
     void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override;
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
+    using Inherit1::getPotentialEnergy;
     SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& /* x */) const override;
 
     void draw(const core::visual::VisualParams* vparams) override;

Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/TrianglePressureForceField.h

@@ -117,6 +117,7 @@ class TrianglePressureForceField : public core::behavior::ForceField<DataTypes>
 
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
 
+    using Inherit1::getPotentialEnergy;
     SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord&  /* x */) const override;
     void draw(const core::visual::VisualParams* vparams) override;
 

Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangleFEMForceField.h

@@ -104,6 +104,7 @@ class TriangleFEMForceField : public BaseLinearElasticityFEMForceField<DataTypes
     void addKToMatrix(sofa::linearalgebra::BaseMatrix *mat, SReal k, unsigned int &offset) override; // compute and add all the element stiffnesses to the global stiffness matrix
     void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override;
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
+    using Inherit1::getPotentialEnergy;
     SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord&  /* x */) const override
     {
         msg_warning() << "Method getPotentialEnergy not implemented yet.";

Sofa/Component/SolidMechanics/Testing/src/sofa/component/solidmechanics/testing/ForceFieldTestCreation.h

@@ -22,12 +22,7 @@
 #pragma once
 
 #include <sofa/testing/BaseSimulationTest.h>
-using sofa::testing::BaseSimulationTest;
-
 #include <sofa/testing/NumericTest.h>
-using sofa::testing::NumericTest;
-
-#include <sofa/simulation/graph/DAGSimulation.h>
 #include <sofa/simulation/MechanicalVisitor.h>
 #include <sofa/linearalgebra/EigenBaseSparseMatrix.h>
 #include <sofa/core/behavior/SingleMatrixAccessor.h>
@@ -36,6 +31,7 @@ using sofa::testing::NumericTest;
 #include <sofa/defaulttype/VecTypes.h>
 #include <sofa/component/statecontainer/MechanicalObject.h>
 #include <sofa/core/behavior/BaseForceField.h>
+#include <sofa/core/behavior/BaseLocalForceFieldMatrix.h>
 
 #include <sofa/simulation/mechanicalvisitor/MechanicalComputeDfVisitor.h>
 using sofa::simulation::mechanicalvisitor::MechanicalComputeDfVisitor;
@@ -57,8 +53,8 @@ namespace sofa {
  * @author François Faure, 2014
  *
  */
-template <typename _ForceFieldType>
-struct ForceField_test : public BaseSimulationTest, NumericTest<typename _ForceFieldType::DataTypes::Real>
+template <typename _ForceFieldType> requires std::is_base_of_v<core::behavior::BaseForceField, _ForceFieldType>
+struct ForceField_test : public sofa::testing::BaseSimulationTest, public sofa::testing::NumericTest<typename _ForceFieldType::DataTypes::Real>
 {
     typedef _ForceFieldType ForceField;
     typedef typename ForceField::DataTypes DataTypes;
@@ -146,6 +142,147 @@ struct ForceField_test : public BaseSimulationTest, NumericTest<typename _ForceF
         force = addNew<ForceField>(node);
     }
 
+    void setupState( const VecCoord& x, const VecDeriv& v)
+    {
+        std::size_t n = x.size();
+        this->dof->resize(static_cast<sofa::Size>(n));
+        typename DOF::WriteVecCoord xdof = this->dof->writePositions();
+        sofa::testing::copyToData( xdof, x );
+        typename DOF::WriteVecDeriv vdof = this->dof->writeVelocities();
+        sofa::testing::copyToData( vdof, v );
+    }
+
+    void computeForce(core::MechanicalParams* mparams) const
+    {
+        MechanicalResetForceVisitor resetForce(mparams, core::vec_id::write_access::force);
+        node->execute(resetForce);
+        MechanicalComputeForceVisitor computeForce( mparams, core::vec_id::write_access::force );
+        this->node->execute(computeForce);
+    }
+
+    void checkForce(const VecDeriv& ef, const std::string& message = "")
+    {
+        typename DOF::ReadVecDeriv f= this->dof->readForces();
+        EXPECT_LT( this->vectorMaxDiff(f,ef), errorMax*this->epsilon() ) << message;
+    }
+
+    void checkPotentialEnergy(const core::MechanicalParams* mparams, SReal potentialEnergyBefore, const VecDeriv& curF, const VecDeriv& dX)
+    {
+        if( !(flags & TEST_POTENTIAL_ENERGY) ) return;
+
+        // Get potential energy after displacement of dofs
+        SReal potentialEnergyAfterDisplacement = force->getPotentialEnergy(mparams);
+
+        // Check getPotentialEnergy() we should have dE = -dX.F
+
+        // Compute dE = E(x+dx)-E(x)
+        SReal differencePotentialEnergy = potentialEnergyAfterDisplacement-potentialEnergyBefore;
+
+        // Compute the expected difference of potential energy: -dX.F (dot product between applied displacement and Force)
+        SReal expectedDifferencePotentialEnergy = 0;
+        for( unsigned i=0; i<dX.size(); ++i){
+            expectedDifferencePotentialEnergy = expectedDifferencePotentialEnergy - dot(dX[i],curF[i]);
+        }
+
+        SReal absoluteErrorPotentialEnergy = std::abs(differencePotentialEnergy - expectedDifferencePotentialEnergy);
+        if( absoluteErrorPotentialEnergy> errorFactorPotentialEnergy*errorMax*this->epsilon() )
+        {
+            ADD_FAILURE()<<"dPotentialEnergy differs from -dX.F (threshold=" << errorFactorPotentialEnergy*errorMax*this->epsilon() << ")" << std::endl
+                        << "dPotentialEnergy is " << differencePotentialEnergy << std::endl
+                        << "-dX.F is " << expectedDifferencePotentialEnergy << std::endl
+                        << "Failed seed number = " << this->seed << std::endl;
+        }
+    }
+
+    void checkComputeDf(core::MechanicalParams* mparams, const VecDeriv& dX, const VecDeriv& changeOfForce)
+    {
+        MechanicalResetForceVisitor resetForce(mparams, core::vec_id::write_access::force);
+        node->execute(resetForce);
+        dof->vRealloc( mparams, core::vec_id::write_access::dx); // dx is not allocated by default
+        typename DOF::WriteVecDeriv wdx = dof->writeDx();
+        sofa::testing::copyToData ( wdx, dX );
+        MechanicalComputeDfVisitor computeDf( mparams, core::vec_id::write_access::force );
+        node->execute(computeDf);
+        VecDeriv dF;
+        sofa::testing::copyFromData( dF, dof->readForces() );
+
+        EXPECT_LE(this->vectorMaxDiff(changeOfForce, dF), errorMax * this->epsilon()) <<
+            "dF differs from change of force\n"
+            "Failed seed number = " << this->seed;
+    }
+
+    void checkAddKToMatrix(core::MechanicalParams* mparams, const VecDeriv& dX, const VecDeriv& changeOfForce)
+    {
+        typedef sofa::linearalgebra::EigenBaseSparseMatrix<SReal> Sqmat;
+        const std::size_t n = dX.size();
+        const sofa::SignedIndex matrixSize = static_cast<sofa::SignedIndex>(n * DataTypes::deriv_total_size);
+        Sqmat K( matrixSize, matrixSize);
+        sofa::core::behavior::SingleMatrixAccessor accessor( &K );
+        mparams->setKFactor(1.0);
+        force->addKToMatrix( mparams, &accessor);
+        K.compress();
+
+        modeling::Vector dx;
+        sofa::testing::data_traits<DataTypes>::VecDeriv_to_Vector( dx, dX );
+
+        modeling::Vector Kdx = K * dx;
+        modeling::Vector df;
+        sofa::testing::data_traits<DataTypes>::VecDeriv_to_Vector( df, changeOfForce );
+
+        if( debug )
+        {
+            std::cout << "        [addKToMatrix] dX = " << dX << std::endl;
+            std::cout << "     change of force = " << changeOfForce << std::endl;
+            std::cout << "                 Kdx = " << Kdx.transpose() << std::endl;
+        }
+
+        EXPECT_LE( this->vectorMaxDiff(Kdx, df), errorMax * this->epsilon() ) <<
+            "Kdx (from addKToMatrix) differs from change of force"
+            "\nFailed seed number = " << this->seed;
+    }
+
+    void checkBuildStiffnessMatrix(core::MechanicalParams* mparams, const VecDeriv& dX, const VecDeriv& changeOfForce)
+    {
+        typedef sofa::linearalgebra::EigenBaseSparseMatrix<SReal> Sqmat;
+        const std::size_t n = dX.size();
+        const sofa::SignedIndex matrixSize = static_cast<sofa::SignedIndex>(n * DataTypes::deriv_total_size);
+        Sqmat K( matrixSize, matrixSize);
+
+        struct StiffnessMatrixAccumulatorTest : public core::behavior::StiffnessMatrixAccumulator
+        {
+            StiffnessMatrixAccumulatorTest(Sqmat& m) : matrix(m) {}
+            void add(sofa::SignedIndex row, sofa::SignedIndex col, float value) override { matrix.add(row, col, (SReal)value); }
+            void add(sofa::SignedIndex row, sofa::SignedIndex col, double value) override { matrix.add(row, col, (SReal)value); }
+            Sqmat& matrix;
+        };
+
+        StiffnessMatrixAccumulatorTest accumulator(K);
+        sofa::core::behavior::StiffnessMatrix stiffnessMatrix;
+        stiffnessMatrix.setMatrixAccumulator(&accumulator, dof.get());
+        stiffnessMatrix.setMechanicalParams(mparams);
+
+        force->buildStiffnessMatrix(&stiffnessMatrix);
+        K.compress();
+
+        modeling::Vector dx;
+        sofa::testing::data_traits<DataTypes>::VecDeriv_to_Vector( dx, dX );
+
+        modeling::Vector Kdx = K * dx;
+        modeling::Vector df;
+        sofa::testing::data_traits<DataTypes>::VecDeriv_to_Vector( df, changeOfForce );
+
+        if( debug )
+        {
+            std::cout << "  [buildStiffnessMatrix] dX = " << dX << std::endl;
+            std::cout << "     change of force = " << changeOfForce << std::endl;
+            std::cout << "                 Kdx = " << Kdx.transpose() << std::endl;
+        }
+
+        EXPECT_LE( this->vectorMaxDiff(Kdx, df), errorMax * this->epsilon() ) <<
+            "Kdx (from buildStiffnessMatrix) differs from change of force"
+            "\nFailed seed number = " << this->seed;
+    }
+
     /**
      * @brief Given positions and velocities, checks that the expected forces are obtained, and that a small change of positions generates the corresponding change of forces.
      * @param x positions
@@ -169,33 +306,29 @@ struct ForceField_test : public BaseSimulationTest, NumericTest<typename _ForceF
         std::size_t n = x.size();
 
         // copy the position and velocities to the scene graph
-        this->dof->resize(static_cast<sofa::Size>(n));
-        typename DOF::WriteVecCoord xdof = this->dof->writePositions();
-        sofa::testing::copyToData( xdof, x );
-        typename DOF::WriteVecDeriv vdof = this->dof->writeVelocities();
-        sofa::testing::copyToData( vdof, v );
+        setupState(x, v);
 
         // init scene and compute force
         if (initScene)
         {
             sofa::simulation::node::initRoot(this->node.get());
         }
+
         core::MechanicalParams mparams;
         mparams.setKFactor(1.0);
-        MechanicalResetForceVisitor resetForce(&mparams, core::vec_id::write_access::force);
-        node->execute(resetForce);
-        MechanicalComputeForceVisitor computeForce( &mparams, core::vec_id::write_access::force );
-        this->node->execute(computeForce);
+
+        computeForce(&mparams);
 
         // check force
-        typename DOF::ReadVecDeriv f= this->dof->readForces();
-        if(debug){
+        if(debug)
+        {
+            typename DOF::ReadVecDeriv f= this->dof->readForces();
             std::cout << "run_test,          x = " << x << std::endl;
             std::cout << "                   v = " << v << std::endl;
             std::cout << "            expected f = " << ef << std::endl;
             std::cout << "            actual f = " <<  f.ref() << std::endl;
         }
-        ASSERT_LT( this->vectorMaxDiff(f,ef), errorMax*this->epsilon() );
+        checkForce(ef);
 
         if( !checkStiffness ) return;
 
@@ -206,91 +339,35 @@ struct ForceField_test : public BaseSimulationTest, NumericTest<typename _ForceF
         sofa::testing::copyFromData( curF, dof->readForces() );
 
         // Get potential Energy before applying a displacement to dofs
-        SReal potentialEnergyBeforeDisplacement = (flags & TEST_POTENTIAL_ENERGY) ? ((const core::behavior::BaseForceField*)force.get())->getPotentialEnergy(&mparams) : 0;
+        SReal potentialEnergyBeforeDisplacement = (flags & TEST_POTENTIAL_ENERGY) ? force->getPotentialEnergy(&mparams) : 0;
 
         // change position
         VecDeriv dX(n);
-        for( unsigned i=0; i<n; i++ ){
-            dX[i] = DataTypes::randomDeriv( deltaRange.first * this->epsilon(), deltaRange.second * this->epsilon() );  // todo: better random, with negative values
-            xdof[i] += dX[i];
+        {
+            typename DOF::WriteVecCoord xdof = this->dof->writePositions();
+            for( unsigned i=0; i<n; i++ ){
+                dX[i] = DataTypes::randomDeriv( deltaRange.first * this->epsilon(), deltaRange.second * this->epsilon() );  // todo: better random, with negative values
+                xdof[i] += dX[i];
+            }
         }
 
         // compute new force and difference between previous force
-        node->execute(resetForce);
-        node->execute(computeForce);
+        computeForce(&mparams);
+
         VecDeriv newF;
         sofa::testing::copyFromData( newF, dof->readForces() );
         VecDeriv changeOfForce(curF);
         for( unsigned i=0; i<curF.size(); ++i){
             changeOfForce[i] = newF[i] - curF[i];
         }
 
-        if( flags & TEST_POTENTIAL_ENERGY )
-        {
-            // Get potential energy after displacement of dofs
-            SReal potentialEnergyAfterDisplacement = ((const core::behavior::BaseForceField*)force.get())->getPotentialEnergy(&mparams);
+        checkPotentialEnergy(&mparams, potentialEnergyBeforeDisplacement, curF, dX);
 
-            // Check getPotentialEnergy() we should have dE = -dX.F
+        checkComputeDf(&mparams, dX, changeOfForce);
 
-            // Compute dE = E(x+dx)-E(x)
-            SReal differencePotentialEnergy = potentialEnergyAfterDisplacement-potentialEnergyBeforeDisplacement;
+        checkAddKToMatrix(&mparams, dX, changeOfForce);
 
-            // Compute the expected difference of potential energy: -dX.F (dot product between applied displacement and Force)
-            SReal expectedDifferencePotentialEnergy = 0;
-            for( unsigned i=0; i<n; ++i){
-                expectedDifferencePotentialEnergy = expectedDifferencePotentialEnergy - dot(dX[i],curF[i]);
-            }
-
-            SReal absoluteErrorPotentialEnergy = std::abs(differencePotentialEnergy - expectedDifferencePotentialEnergy);
-            if( absoluteErrorPotentialEnergy> errorFactorPotentialEnergy*errorMax*this->epsilon() ){
-                ADD_FAILURE()<<"dPotentialEnergy differs from -dX.F (threshold=" << errorFactorPotentialEnergy*errorMax*this->epsilon() << ")" << std::endl
-                            << "dPotentialEnergy is " << differencePotentialEnergy << std::endl
-                            << "-dX.F is " << expectedDifferencePotentialEnergy << std::endl
-                            << "Failed seed number = " << this->seed << std::endl;
-            }
-        }
-
-
-        // check computeDf: compare its result to actual change
-        node->execute(resetForce);
-        dof->vRealloc( &mparams, core::vec_id::write_access::dx); // dx is not allocated by default
-        typename DOF::WriteVecDeriv wdx = dof->writeDx();
-        sofa::testing::copyToData ( wdx, dX );
-        MechanicalComputeDfVisitor computeDf( &mparams, core::vec_id::write_access::force );
-        node->execute(computeDf);
-        VecDeriv dF;
-        sofa::testing::copyFromData( dF, dof->readForces() );
-
-        EXPECT_LE(this->vectorMaxDiff(changeOfForce, dF), errorMax * this->epsilon()) <<
-            "dF differs from change of force\n"
-            "Failed seed number = " << this->seed;
-
-        // check stiffness matrix: compare its product with dx to actual force change
-        typedef sofa::linearalgebra::EigenBaseSparseMatrix<SReal> Sqmat;
-        const sofa::SignedIndex matrixSize = static_cast<sofa::SignedIndex>(n * DataTypes::deriv_total_size);
-        Sqmat K( matrixSize, matrixSize);
-        sofa::core::behavior::SingleMatrixAccessor accessor( &K );
-        mparams.setKFactor(1.0);
-        force->addKToMatrix( &mparams, &accessor);
-        K.compress();
-        //        cout << "stiffness: " << K << endl;
-        modeling::Vector dx;
-        sofa::testing::data_traits<DataTypes>::VecDeriv_to_Vector( dx, dX );
-
-        modeling::Vector Kdx = K * dx;
-        if( debug ){
-            std::cout << "                  dX = " << dX << std::endl;
-            std::cout << "                newF = " << newF << std::endl;
-            std::cout << "     change of force = " << changeOfForce << std::endl;
-            std::cout << "           addDforce = " << dF << std::endl;
-            std::cout << "                 Kdx = " << Kdx.transpose() << std::endl;
-        }
-
-        modeling::Vector df;
-        sofa::testing::data_traits<DataTypes>::VecDeriv_to_Vector( df, changeOfForce );
-        EXPECT_LE( this->vectorMaxDiff(Kdx, df), errorMax * this->epsilon() ) <<
-            "Kdx differs from change of force"
-            "\nFailed seed number = " << this->seed;
+        checkBuildStiffnessMatrix(&mparams, dX, changeOfForce);
     }