diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/PenalityContactForceField.h b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/PenalityContactForceField.h index 1ad75539831..e7f485fa4f2 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/PenalityContactForceField.h +++ b/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();} diff --git a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.h b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.h index 5880a219c41..3e049f67a05 100644 --- a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.h +++ b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.h @@ -118,7 +118,8 @@ class QuadPressureForceField : public core::behavior::ForceField 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; diff --git a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.inl b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.inl index 0b869038c8c..45428a8e616 100644 --- a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.inl +++ b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/QuadPressureForceField.inl @@ -232,7 +232,15 @@ void QuadPressureForceField::buildDampingMatrix(core::behavior::Dampi // No damping in this ForceField } -template +template +SReal QuadPressureForceField::getPotentialEnergy(const core::MechanicalParams*, + const DataVecCoord&) const +{ + msg_warning() << "Method getPotentialEnergy not implemented yet."; + return 0.0; +} + +template void QuadPressureForceField::draw(const core::visual::VisualParams* vparams) { const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); diff --git a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/SurfacePressureForceField.h b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/SurfacePressureForceField.h index e866cd70f32..d5846d28abd 100644 --- a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/SurfacePressureForceField.h +++ b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/SurfacePressureForceField.h @@ -103,6 +103,7 @@ class SurfacePressureForceField : public core::behavior::ForceField 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; diff --git a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/TrianglePressureForceField.h b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/TrianglePressureForceField.h index 8aeffb3ad10..039cb74fa87 100644 --- a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/TrianglePressureForceField.h +++ b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/TrianglePressureForceField.h @@ -117,6 +117,7 @@ class TrianglePressureForceField : public core::behavior::ForceField 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; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangleFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangleFEMForceField.h index b7a64ba2234..515b4b736ed 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangleFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangleFEMForceField.h @@ -104,6 +104,7 @@ class TriangleFEMForceField : public BaseLinearElasticityFEMForceField -using sofa::testing::BaseSimulationTest; - #include -using sofa::testing::NumericTest; - -#include #include #include #include @@ -36,6 +31,7 @@ using sofa::testing::NumericTest; #include #include #include +#include #include using sofa::simulation::mechanicalvisitor::MechanicalComputeDfVisitor; @@ -57,8 +53,8 @@ namespace sofa { * @author François Faure, 2014 * */ -template -struct ForceField_test : public BaseSimulationTest, NumericTest +template requires std::is_base_of_v +struct ForceField_test : public sofa::testing::BaseSimulationTest, public sofa::testing::NumericTest { typedef _ForceFieldType ForceField; typedef typename ForceField::DataTypes DataTypes; @@ -146,6 +142,147 @@ struct ForceField_test : public BaseSimulationTest, NumericTest(node); } + void setupState( const VecCoord& x, const VecDeriv& v) + { + std::size_t n = x.size(); + this->dof->resize(static_cast(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 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 Sqmat; + const std::size_t n = dX.size(); + const sofa::SignedIndex matrixSize = static_cast(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::VecDeriv_to_Vector( dx, dX ); + + modeling::Vector Kdx = K * dx; + modeling::Vector df; + sofa::testing::data_traits::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 Sqmat; + const std::size_t n = dX.size(); + const sofa::SignedIndex matrixSize = static_cast(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::VecDeriv_to_Vector( dx, dX ); + + modeling::Vector Kdx = K * dx; + modeling::Vector df; + sofa::testing::data_traits::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, NumericTestdof->resize(static_cast(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,18 +339,21 @@ struct ForceField_test : public BaseSimulationTest, NumericTestreadForces() ); // 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; iepsilon(), 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; iepsilon(), 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); @@ -225,72 +361,13 @@ struct ForceField_test : public BaseSimulationTest, NumericTestgetPotentialEnergy(&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 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 Sqmat; - const sofa::SignedIndex matrixSize = static_cast(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::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::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); }