From dab9c57f9b77d1765e6df778aea5c01b92a17f7c Mon Sep 17 00:00:00 2001 From: alxbilger Date: Fri, 4 Jun 2021 17:07:06 +0200 Subject: [PATCH 1/7] [SofaSimulationCore] Add tons of details in Euler solver --- .../core/behavior/BaseMechanicalState.cpp | 4 +- .../sofa/core/behavior/BaseMechanicalState.h | 6 +- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 296 +++++++++++++----- .../src/SofaExplicitOdeSolver/EulerSolver.h | 85 ++++- .../sofa/simulation/MechanicalOperations.cpp | 8 +- 5 files changed, 305 insertions(+), 94 deletions(-) diff --git a/SofaKernel/modules/SofaCore/src/sofa/core/behavior/BaseMechanicalState.cpp b/SofaKernel/modules/SofaCore/src/sofa/core/behavior/BaseMechanicalState.cpp index bfe8fa2aa3d..30168850c6f 100644 --- a/SofaKernel/modules/SofaCore/src/sofa/core/behavior/BaseMechanicalState.cpp +++ b/SofaKernel/modules/SofaCore/src/sofa/core/behavior/BaseMechanicalState.cpp @@ -77,7 +77,9 @@ void BaseMechanicalState::vMultiOp(const ExecParams* params, const VMultiOp& ops i = 1; } for (; i > > diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index 2233a2ca075..3c043b2e911 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include @@ -53,127 +52,190 @@ EulerExplicitSolver::EulerExplicitSolver() { } -void EulerExplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) +void EulerExplicitSolver::solve(const core::ExecParams* params, + SReal dt, + sofa::core::MultiVecCoordId xResult, + sofa::core::MultiVecDerivId vResult) { + sofa::helper::ScopedAdvancedTimer timer("EulerExplicitSolve"); + + // Create the vector and mechanical operations tools. These are used to execute special operations (multiplication, + // additions, etc.) on multi-vectors (a vector that is stored in different buffers inside the mechanical objects) sofa::simulation::common::VectorOperations vop( params, this->getContext() ); sofa::simulation::common::MechanicalOperations mop( params, this->getContext() ); - mop->setImplicit(false); // this solver is explicit only - MultiVecCoord pos(&vop, core::VecCoordId::position() ); - MultiVecDeriv vel(&vop, core::VecDerivId::velocity() ); - MultiVecDeriv f (&vop, core::VecDerivId::force() ); - MultiVecCoord newPos(&vop, xResult /*core::VecCoordId::position()*/ ); - MultiVecDeriv newVel(&vop, vResult /*core::VecDerivId::velocity()*/ ); - MultiVecDeriv acc(&vop, core::VecDerivId::dx()); + // Let the mechanical operations know that the current solver is explicit. This will be propagated back to the + // force fields during the addForce and addKToMatrix phase. Force fields use this information to avoid + // recomputing constant data in case of explicit solvers. + mop->setImplicit(false); + + // Initialize the set of multi-vectors computed by this solver + MultiVecDeriv acc (&vop, core::VecDerivId::dx()); // acceleration to be computed + MultiVecDeriv f (&vop, core::VecDerivId::force() ); // force to be computed + + acc.realloc(&vop, !d_threadSafeVisitor.getValue(), true); - acc.realloc(&vop, !d_threadSafeVisitor.getValue(), true); // dx is no longer allocated by default (but it will be deleted automatically by the mechanical objects) + addSeparateGravity(&mop, dt); + computeForce(&mop, f); + projectResponse(&mop, acc); // Mass matrix is diagonal, solution can thus be found by computing acc = f/m if(d_optimizedForDiagonalMatrix.getValue()) { - mop.addSeparateGravity(dt); // v += dt*g . Used if mass wants to add G separately from the other forces to v. - sofa::helper::AdvancedTimer::stepBegin("ComputeForce"); - mop.computeForce(f); - sofa::helper::AdvancedTimer::stepEnd("ComputeForce"); - - sofa::helper::AdvancedTimer::stepBegin("AccFromF"); - mop.accFromF(acc, f); - sofa::helper::AdvancedTimer::stepEnd("AccFromF"); - mop.projectResponse(acc); - - mop.solveConstraint(acc, core::ConstraintParams::ACC); - - + // acc = M^-1 * f + computeAcceleration(&mop, acc, f); + projectResponse(&mop, acc); + solveConstraints(&mop, acc); } else { - x.realloc(&vop, !d_threadSafeVisitor.getValue(), true); - - mop.addSeparateGravity(dt); // v += dt*g . Used if mass wants to added G separately from the other forces to v. + core::behavior::MultiMatrix matrix(&mop); - sofa::helper::AdvancedTimer::stepBegin("ComputeForce"); - mop.computeForce(f); - sofa::helper::AdvancedTimer::stepEnd("ComputeForce"); + // Build the global matrix. In this solver, it is the global mass matrix + // Projective constraints are also projected in this step + assembleSystemMatrix(&matrix); - sofa::helper::AdvancedTimer::stepBegin ("projectResponse"); - mop.projectResponse(f); - sofa::helper::AdvancedTimer::stepEnd ("projectResponse"); + // Solve the system to find the acceleration + // Solve M * a = f + solveSystem(&matrix, acc, f); + } - sofa::helper::AdvancedTimer::stepBegin ("MBKBuild"); - core::behavior::MultiMatrix matrix(&mop); - matrix = MechanicalMatrix(1.0,0,0); // MechanicalMatrix::M; - sofa::helper::AdvancedTimer::stepEnd ("MBKBuild"); + // Compute the new position and new velocity from the acceleration + updateState(&vop, &mop, xResult, vResult, acc, dt); +} - sofa::helper::AdvancedTimer::stepBegin ("MBKSolve"); - matrix.solve(x, f); //Call to ODE resolution: x is the solution of the system - sofa::helper::AdvancedTimer::stepEnd ("MBKSolve"); +void EulerExplicitSolver::updateState(sofa::simulation::common::VectorOperations* vop, + sofa::simulation::common::MechanicalOperations* mop, + sofa::core::MultiVecCoordId xResult, + sofa::core::MultiVecDerivId vResult, + const sofa::core::behavior::MultiVecDeriv& acc, + SReal dt) const +{ + sofa::helper::ScopedAdvancedTimer timer("updateState"); - acc.eq(x); - } + // Initialize the set of multi-vectors computed by this solver + // "xResult" could be "position()" or "freePosition()" depending on the + // animation loop calling this ODE solver. + // Similarly, "vResult" could be "velocity()" or "freeVelocity()". + // In case "xResult" refers to "position()", "newPos" refers the + // same multi-vector than "pos". Similarly, for "newVel" and "vel". + MultiVecCoord newPos(vop, xResult); // velocity to be computed + MultiVecDeriv newVel(vop, vResult); // position to be computed + // Initialize the set of multi-vectors used to compute the new velocity and position + MultiVecCoord pos(vop, core::VecCoordId::position() ); //current position + MultiVecDeriv vel(vop, core::VecDerivId::velocity() ); //current velocity - // update state #ifdef SOFA_NO_VMULTIOP // unoptimized version if (d_symplectic.getValue()) { - newVel.eq(vel, acc, dt); - mop.solveConstraint(newVel, core::ConstraintParams::VEL); + //newVel = vec + acc * dt + //newPos = pos + newVel * dt + + newVel.eq(vel, acc.id(), dt); + mop->solveConstraint(newVel, core::ConstraintParams::VEL); + newPos.eq(pos, newVel, dt); - mop.solveConstraint(newPos, core::ConstraintParams::POS); + mop->solveConstraint(newPos, core::ConstraintParams::POS); } else { + //newPos = pos + vel * dt + //newVel = vel + acc * dt + newPos.eq(pos, vel, dt); - mop.solveConstraint(newPos, core::ConstraintParams::POS); - newVel.eq(vel, acc, dt); - mop.solveConstraint(newVel, core::ConstraintParams::VEL); + mop->solveConstraint(newPos, core::ConstraintParams::POS); + + newVel.eq(vel, acc.id(), dt); + mop->solveConstraint(newVel, core::ConstraintParams::VEL); } #else // single-operation optimization { typedef core::behavior::BaseMechanicalState::VMultiOp VMultiOp; - VMultiOp ops; - ops.resize(2); - // change order of operations depending on the symplectic flag - size_t op_vel = (d_symplectic.getValue()?0:1); - size_t op_pos = (d_symplectic.getValue()?1:0); - ops[op_vel].first = newVel; - ops[op_vel].second.push_back(std::make_pair(vel.id(),1.0)); - ops[op_vel].second.push_back(std::make_pair(acc.id(),dt)); - ops[op_pos].first = newPos; - ops[op_pos].second.push_back(std::make_pair(pos.id(),1.0)); - ops[op_pos].second.push_back(std::make_pair(newVel.id(),dt)); - - vop.v_multiop(ops); - - mop.solveConstraint(newVel,core::ConstraintParams::VEL); - mop.solveConstraint(newPos,core::ConstraintParams::POS); + + // Create a set of linear operations that will be executed on two vectors + // In our case, the operations will be executed to compute the new velocity vector, + // and the new position vector. The order of execution is defined by + // the symplectic property of the solver. + VMultiOp ops(2); + + // Change order of operations depending on the symplectic flag + const VMultiOp::size_type posId = d_symplectic.getValue(); // 1 if symplectic, 0 otherwise + const VMultiOp::size_type velId = 1 - posId; // 0 if symplectic, 1 otherwise + + // Access the set of operations corresponding to the velocity vector + // In case of symplectic solver, these operations are executed first. + auto& ops_vel = ops[velId]; + + // Associate the new velocity vector as the result to this set of operations + ops_vel.first = newVel; + + // The two following operations are actually a unique operation: newVel = vel + dt * acc + // The value 1.0 indicates that the first operation is based on the values + // in the second pair and, therefore, the second operation is discarded. + ops_vel.second.emplace_back(vel.id(), 1.0); + ops_vel.second.emplace_back(acc.id(), dt); + + // Access the set of operations corresponding to the position vector + // In case of symplectic solver, these operations are executed second. + auto& ops_pos = ops[posId]; + + // Associate the new position vector as the result to this set of operations + ops_pos.first = newPos; + + // The two following operations are actually a unique operation: newPos = pos + dt * v + // where v is "newVel" in case of a symplectic solver, and "vel" otherwise. + // If symplectic: newPos = pos + dt * newVel, executed after newVel has been computed + // If not symplectic: newPos = pos + dt * vel + // The value 1.0 indicates that the first operation is based on the values + // in the second pair and, therefore, the second operation is discarded. + ops_pos.second.emplace_back(pos.id(), 1.0); + ops_pos.second.emplace_back(d_symplectic.getValue() ? newVel.id() : vel.id(), dt); + + // Execute the defined operations to compute the new velocity vector and + // the new position vector. + // 1. Calls the "vMultiOp" method of every mapped BaseMechanicalState objects found in the + // current context tree. This method may be called with different parameters than for the non-mapped + // BaseMechanicalState objects. + // 2. Calls the "vMultiOp" method of every BaseMechanicalState objects found in the + // current context tree. + vop->v_multiop(ops); + + // Calls "solveConstraint" on every ConstraintSolver objects found in the current context tree. + mop->solveConstraint(newVel,core::ConstraintParams::VEL); + mop->solveConstraint(newPos,core::ConstraintParams::POS); } #endif } double EulerExplicitSolver::getIntegrationFactor(int inputDerivative, int outputDerivative) const { - const SReal dt = getContext()->getDt(); - double matrix[3][3] = - { - { 1, dt, ((d_symplectic.getValue())?dt*dt:0.0)}, - { 0, 1, dt}, - { 0, 0, 0} - }; if (inputDerivative >= 3 || outputDerivative >= 3) + { return 0; - else - return matrix[outputDerivative][inputDerivative]; + } + + const SReal dt = getContext()->getDt(); + const SReal k_a = d_symplectic.getValue() * dt * dt; + const double matrix[3][3] = + { + { 1, dt, k_a}, //x = 1 * x + dt * v + k_a * a + { 0, 1, dt}, //v = 0 * x + 1 * v + dt * a + { 0, 0, 0} + }; + + return matrix[outputDerivative][inputDerivative]; } double EulerExplicitSolver::getSolutionIntegrationFactor(int outputDerivative) const { - const SReal dt = getContext()->getDt(); - double vect[3] = {((d_symplectic.getValue()) ? dt * dt : 0.0), dt, 1}; if (outputDerivative >= 3) return 0; - else - return vect[outputDerivative]; + + const SReal dt = getContext()->getDt(); + const SReal k_a = d_symplectic.getValue() * dt * dt; + const double vect[3] = {k_a, dt, 1}; + return vect[outputDerivative]; } void EulerExplicitSolver::init() @@ -182,5 +244,83 @@ void EulerExplicitSolver::init() reinit(); } +void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt) +{ + sofa::helper::ScopedAdvancedTimer timer("addSeparateGravity"); + + /// Calls the "addGravityToV" method of every BaseMass objects found in the current + /// context tree, if the BaseMass object has the m_separateGravity flag set to true. + /// The method "addGravityToV" usually performs v += dt * g + mop->addSeparateGravity(dt); +} + +void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f) +{ + sofa::helper::ScopedAdvancedTimer timer("ComputeForce"); + + // 1. Clear the force vector (F := 0) + // 2. Go down in the current context tree calling addForce on every forcefields + // 3. Go up from the current context tree leaves calling applyJT on every mechanical mappings + mop->computeForce(f); +} + +void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc, core::ConstMultiVecDerivId f) +{ + sofa::helper::ScopedAdvancedTimer timer("AccFromF"); + + // acc = M^-1 f + // Since it requires the inverse of the mass matrix, this method is + // probably implemented only for trivial matrix inversion, such as + // a diagonal matrix. + // For example, for a diagonal mass: a_i := f_i / M_ii + mop->accFromF(acc, f); +} + +void EulerExplicitSolver::projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc) +{ + sofa::helper::ScopedAdvancedTimer timer("projectResponse"); + + // Calls the "projectResponse" method of every BaseProjectiveConstraintSet objects found in the + // current context tree. An example of such constraint set is the FixedConstraint. In this case, + // it will set to 0 every row (i, _) of the right-hand side (force) vector for the ith degree of + // freedom. + mop->projectResponse(acc); +} + +void EulerExplicitSolver::solveConstraints(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc) +{ + sofa::helper::ScopedAdvancedTimer timer("solveConstraint"); + + // Calls "solveConstraint" method of every ConstraintSolver objects found in the current context tree. + mop->solveConstraint(acc, core::ConstraintParams::ACC); +} + +void EulerExplicitSolver::assembleSystemMatrix(core::behavior::MultiMatrix* matrix) +{ + sofa::helper::ScopedAdvancedTimer timer("MBKBuild"); + + // The MechanicalMatrix::M is a simple structure that stores three floats called factors: m, b and k. + // In the case of MechanicalMatrix::M: + // - factor 1 is associated to the matrix M + // - factor 0 is associated to the matrix B + // - factor 0 is associated to the matrix K + // The = operator first search for a linear solver in the current context. It then calls the + // "setSystemMBKMatrix" method of the linear solver. + + // A. For LinearSolver using a GraphScatteredMatrix (ie, non-assembled matrices), nothing appends. + // B. For LinearSolver using other type of matrices (FullMatrix, SparseMatrix, CompressedRowSparseMatrix), + // the "addMBKToMatrix" method is called on each BaseForceField objects and the "applyConstraint" method + // is called on every BaseProjectiveConstraintSet objects. An example of such constraint set is the + // FixedConstraint. In this case, it will set to 0 every column (_, i) and row (i, _) of the assembled + // matrix for the ith degree of freedom. + *matrix = MechanicalMatrix::M; +} + +void EulerExplicitSolver::solveSystem(core::behavior::MultiMatrix* matrix, + core::MultiVecDerivId solution, core::MultiVecDerivId rhs) +{ + sofa::helper::ScopedAdvancedTimer timer("MBKSolve"); + matrix->solve(solution, rhs); +} -} // namespace namespace sofa::component::odesolver +} // namespace sofa::component::odesolver diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h index 4948a7f18fb..539421cb80f 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h @@ -23,21 +23,60 @@ #include #include +#include + +namespace sofa::simulation::common +{ +class MechanicalOperations; +class VectorOperations; +} + +namespace sofa::core::behavior +{ +template +class MultiMatrix; +} namespace sofa::component::odesolver { -/** The simplest time integration. - Two variants are available, depending on the value of field "symplectic". - If true (the default), the symplectic variant of Euler's method is applied: - If false, the basic Euler's method is applied (less robust) +/** + * The simplest time integration. + * Two variants of the Euler solver are available in this component: + * - forward Euler method, also called explicit Euler method + * - semi-implicit Euler method, also called semi-explicit Euler method or symplectic Euler + * + * In both variants, acceleration is first computed. The system to compute the acceleration + * is M * a = f, where M is the mass matrix and f can be a force. + * In case of a diagonal mass matrix, M is trivially invertible and the acceleration + * can be computed without a linear solver. + * + * f is accumulated by force fields through the addForce function. Mappings can + * also contribute by projecting forces of mapped objects. + * f is computed based on the current state (current velocity and position). + * + * Explicit Euler method: + * The option "symplectic" must be set to false to use this variant. + * The explicit Euler method produces an approximate discrete solution by iterating + * x_{n+1} = x_n + v_n * dt + * v_{n+1} = v_n + a * dt + * + * Semi-implicit Euler method: + * The option "symplectic" must be set to true to use this variant. + * The semi-implicit Euler method produces an approximate discrete solution by iterating + * v_{n+1} = v_n + a * dt + * x_{n+1} = x_n + v_{n+1} * dt + * + * The semi-implicit Euler method is more robust than the standard Euler method. */ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::behavior::OdeSolver { public: SOFA_CLASS(EulerExplicitSolver, sofa::core::behavior::OdeSolver); + protected: EulerExplicitSolver(); + public: void solve(const core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override; @@ -53,12 +92,42 @@ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::be /// how much will it affect the output derivative of the given order. /// double getSolutionIntegrationFactor(int outputDerivative) const override ; - void init() override ; + void init() override ; protected: - /// the solution vector is stored for warm-start - core::behavior::MultiVecDeriv x; + + /// Update state variable (new position and velocity) based on the computed acceleration + /// The update takes constraints into account + void updateState(sofa::simulation::common::VectorOperations* vop, + sofa::simulation::common::MechanicalOperations* mop, + sofa::core::MultiVecCoordId xResult, + sofa::core::MultiVecDerivId vResult, + const sofa::core::behavior::MultiVecDeriv& acc, + SReal dt) const; + + /// Gravity times time step size is added to the velocity for some masses + /// v += g * dt + static void addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt); + + /// Assemble the force vector (right-hand side of the equation) + static void computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f); + + /// Compute the acceleration from the force and the inverse of the mass + /// acc = M^-1 * f + static void computeAcceleration(sofa::simulation::common::MechanicalOperations* mop, + core::MultiVecDerivId acc, + core::ConstMultiVecDerivId f); + + /// Apply projective constraints, such as FixedConstraint + static void projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc); + + static void solveConstraints(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc); + + static void assembleSystemMatrix(core::behavior::MultiMatrix* matrix); + + static void solveSystem(core::behavior::MultiMatrix* matrix, + core::MultiVecDerivId solution, core::MultiVecDerivId rhs); }; -} // namespace namespace sofa::component::odesolver +} // namespace sofa::component::odesolver diff --git a/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/MechanicalOperations.cpp b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/MechanicalOperations.cpp index a7304c44233..3c584459897 100644 --- a/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/MechanicalOperations.cpp +++ b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/MechanicalOperations.cpp @@ -422,14 +422,10 @@ void MechanicalOperations::solveConstraint(MultiVecId id, core::ConstraintParams helper::vector< core::behavior::ConstraintSolver* > constraintSolverList; ctx->get(&constraintSolverList, ctx->getTags(), BaseContext::Local); - if (constraintSolverList.empty()) - { - return; - } - for (helper::vector< core::behavior::ConstraintSolver* >::iterator it=constraintSolverList.begin(); it!=constraintSolverList.end(); ++it) + for (auto* constraintSolver : constraintSolverList) { - (*it)->solveConstraint(&cparams, id); + constraintSolver->solveConstraint(&cparams, id); } } From 496177340837b987d2d85511f6a0bfcfb481e87f Mon Sep 17 00:00:00 2001 From: alxbilger Date: Mon, 7 Jun 2021 08:30:21 +0200 Subject: [PATCH 2/7] [SofaExplicitOdeSolver] addSeparateGravity is added a parameter So it is more similar to the other signature functions. Suggestion by jnbrunet PR#2163 --- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 6 +++--- .../src/SofaExplicitOdeSolver/EulerSolver.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index 3c043b2e911..d02247caa45 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -75,7 +75,7 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, acc.realloc(&vop, !d_threadSafeVisitor.getValue(), true); - addSeparateGravity(&mop, dt); + addSeparateGravity(&mop, dt, core::VecDerivId::velocity()); computeForce(&mop, f); projectResponse(&mop, acc); @@ -244,14 +244,14 @@ void EulerExplicitSolver::init() reinit(); } -void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt) +void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v) { sofa::helper::ScopedAdvancedTimer timer("addSeparateGravity"); /// Calls the "addGravityToV" method of every BaseMass objects found in the current /// context tree, if the BaseMass object has the m_separateGravity flag set to true. /// The method "addGravityToV" usually performs v += dt * g - mop->addSeparateGravity(dt); + mop->addSeparateGravity(dt, v); } void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h index 539421cb80f..1f0aa61bcbe 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h @@ -108,7 +108,7 @@ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::be /// Gravity times time step size is added to the velocity for some masses /// v += g * dt - static void addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt); + static void addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v); /// Assemble the force vector (right-hand side of the equation) static void computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f); From 3cfff425b78a8ee595b8b0235443831720548b0f Mon Sep 17 00:00:00 2001 From: alxbilger Date: Mon, 7 Jun 2021 10:45:27 +0200 Subject: [PATCH 3/7] [SofaExplicitOdeSolver] Use a new visitor to detect if the optimization based on diagonal matrices can be used --- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 36 +++++++++++- .../src/SofaExplicitOdeSolver/EulerSolver.h | 5 +- .../modules/SofaSimulationCore/CMakeLists.txt | 2 + ...anicalGetNonDiagonalMassesCountVisitor.cpp | 35 ++++++++++++ ...chanicalGetNonDiagonalMassesCountVisitor.h | 57 +++++++++++++++++++ 5 files changed, 132 insertions(+), 3 deletions(-) create mode 100644 SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.cpp create mode 100644 SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index d02247caa45..b0b57fcefdf 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -27,6 +27,9 @@ #include #include +#include +using sofa::simulation::mechanicalvisitor::MechanicalGetNonDiagonalMassesCountVisitor; + //#define SOFA_NO_VMULTIOP namespace sofa::component::odesolver @@ -47,7 +50,8 @@ int EulerExplicitSolverClass = core::RegisterObject("A simple explicit time inte EulerExplicitSolver::EulerExplicitSolver() : d_symplectic( initData( &d_symplectic, true, "symplectic", "If true, the velocities are updated before the positions and the method is symplectic (more robust). If false, the positions are updated before the velocities (standard Euler, less robust).") ) - , d_optimizedForDiagonalMatrix(initData(&d_optimizedForDiagonalMatrix, true, "optimizedForDiagonalMatrix", "If true, solution to the system Ax=b can be directly found by computing x = f/m. Must be set to false if M is sparse.")) + , d_forceDiagonalMassMatrixOptimization(initData(&d_forceDiagonalMassMatrixOptimization, false, "forceDiagonalMassMatrixOptimization", "If true, the optimization used to solve the system assuming a diagonal mass matrix is used, even if the mass matrix is not diagonal. False by default.")) + , d_forceBuildingMatrixSystem(initData(&d_forceBuildingMatrixSystem, false, "forceBuildingMatrixSystem", "If true, the global linear system is built and solved, even if the mass matrix is diagonal. False by default.")) , d_threadSafeVisitor(initData(&d_threadSafeVisitor, false, "threadSafeVisitor", "If true, do not use realloc and free visitors in fwdInteractionForceField.")) { } @@ -79,9 +83,18 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, computeForce(&mop, f); projectResponse(&mop, acc); + SReal nbNonDiagonalMasses = 0; + MechanicalGetNonDiagonalMassesCountVisitor(&mop.mparams, &nbNonDiagonalMasses).execute(this->getContext()); + // Mass matrix is diagonal, solution can thus be found by computing acc = f/m - if(d_optimizedForDiagonalMatrix.getValue()) + if((nbNonDiagonalMasses == 0. || d_forceDiagonalMassMatrixOptimization.getValue()) && !d_forceBuildingMatrixSystem.getValue()) { + if (d_forceDiagonalMassMatrixOptimization.getValue() && nbNonDiagonalMasses > 0.) + { + msg_warning() << "You requested the optimization based on diagonal mass matrix " + "('forceDiagonalMassMatrixOptimization' is true), but at least one " + "non-diagonal mass matrix has been detected. It may not work as expected."; + } // acc = M^-1 * f computeAcceleration(&mop, acc, f); projectResponse(&mop, acc); @@ -89,6 +102,12 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, } else { + if (nbNonDiagonalMasses == 0.) + { + msg_warning() << "You requested to build explicitly the global linear system. This time consumming method" + "is not necessary if working with only diagonal mass matrices. Set 'forceBuildingMatrixSystem'" + " to false to remove this warning and use the optimized implementation."; + } core::behavior::MultiMatrix matrix(&mop); // Build the global matrix. In this solver, it is the global mass matrix @@ -244,6 +263,19 @@ void EulerExplicitSolver::init() reinit(); } +void EulerExplicitSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) +{ + const char* val = arg->getAttribute("optimizedForDiagonalMatrix",nullptr) ; + if(val) + { + msg_deprecated() << "The attribute 'optimizedForDiagonalMatrix' is deprecated since SOFA 21.06." << msgendl + << "This data was previously used to solve the system more efficiently in case the " + "global mass matrix were diagonal." << msgendl + << "Now, this property is detected automatically. See also the data 'forceDiagonalMassMatrixOptimization'" + "and 'forceBuildingMatrixSystem'"; + } +} + void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v) { sofa::helper::ScopedAdvancedTimer timer("addSeparateGravity"); diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h index 1f0aa61bcbe..e23a3366991 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h @@ -81,7 +81,8 @@ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::be void solve(const core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override; Data d_symplectic; ///< If true, the velocities are updated before the positions and the method is symplectic (more robust). If false, the positions are updated before the velocities (standard Euler, less robust). - Data d_optimizedForDiagonalMatrix; ///< If M matrix is sparse (MeshMatrixMass), must be set to false (function addMDx() will compute the mass). Else, if true, solution to the system Ax=b can be directly found by computing x = f/m. The function accFromF() in the mass API will be used. + Data d_forceDiagonalMassMatrixOptimization; ///< If true, the optimization used to solve the system assuming a diagonal mass matrix is used, even if the mass matrix is not diagonal. False by default. + Data d_forceBuildingMatrixSystem; ///< If true, the global linear system is built and solved, even if the mass matrix is diagonal. False by default. Data d_threadSafeVisitor; /// Given an input derivative order (0 for position, 1 for velocity, 2 for acceleration), @@ -95,6 +96,8 @@ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::be void init() override ; + void parse(sofa::core::objectmodel::BaseObjectDescription* arg) override; + protected: /// Update state variable (new position and velocity) based on the computed acceleration diff --git a/SofaKernel/modules/SofaSimulationCore/CMakeLists.txt b/SofaKernel/modules/SofaSimulationCore/CMakeLists.txt index bb695d89d71..1d193c74e7c 100644 --- a/SofaKernel/modules/SofaSimulationCore/CMakeLists.txt +++ b/SofaKernel/modules/SofaSimulationCore/CMakeLists.txt @@ -100,6 +100,7 @@ set(HEADER_FILES ${SRC_ROOT}/mechanicalvisitor/MechanicalGetDimensionVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalGetMatrixDimensionVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalGetMomentumVisitor.h + ${SRC_ROOT}/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalIntegrateConstraintVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalIntegrationVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalMultiVectorFromBaseVectorVisitor.h @@ -221,6 +222,7 @@ set(SOURCE_FILES ${SRC_ROOT}/mechanicalvisitor/MechanicalGetDimensionVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalGetMatrixDimensionVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalGetMomentumVisitor.cpp + ${SRC_ROOT}/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalIntegrateConstraintVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalIntegrationVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalMultiVectorFromBaseVectorVisitor.cpp diff --git a/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.cpp b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.cpp new file mode 100644 index 00000000000..cfdd08c4edb --- /dev/null +++ b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.cpp @@ -0,0 +1,35 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include + +namespace sofa::simulation::mechanicalvisitor +{ + +Visitor::Result MechanicalGetNonDiagonalMassesCountVisitor::fwdMass(VisitorContext* ctx, core::behavior::BaseMass* mass) +{ + *ctx->nodeData += !mass->isDiagonal(); + return RESULT_CONTINUE; +} + +} \ No newline at end of file diff --git a/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h new file mode 100644 index 00000000000..a056edc8a38 --- /dev/null +++ b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h @@ -0,0 +1,57 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include + +namespace sofa::simulation::mechanicalvisitor +{ + +/** Count the number of masses which are not diagonal */ +class SOFA_SIMULATION_CORE_API MechanicalGetNonDiagonalMassesCountVisitor : public MechanicalVisitor +{ +public: + MechanicalGetNonDiagonalMassesCountVisitor(const sofa::core::MechanicalParams* mparams, SReal* result) + : MechanicalVisitor(mparams) + { + rootData = result; + } + + Result fwdMass(VisitorContext* ctx, sofa::core::behavior::BaseMass* mass) override; + + /// Return a class name for this visitor + /// Only used for debugging / profiling purposes + const char* getClassName() const override { return "MechanicalGetNonDiagonalMassesCountVisitor";} + + bool writeNodeData() const override + { + return true; + } + + bool stopAtMechanicalMapping(simulation::Node* /*node*/, sofa::core::BaseMapping* /*map*/) override + { + return false; + } + +}; + +} \ No newline at end of file From f587b196df52eb2e6899bd802ab0dccad210d491 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Mon, 7 Jun 2021 10:47:07 +0200 Subject: [PATCH 4/7] [SofaExplicitOdeSolver] Remove parameters to force the optimization or the system building --- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 16 +--------------- .../src/SofaExplicitOdeSolver/EulerSolver.h | 2 -- .../MechanicalGetNonDiagonalMassesCountVisitor.h | 5 ----- 3 files changed, 1 insertion(+), 22 deletions(-) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index b0b57fcefdf..41aa4e44f3c 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -50,8 +50,6 @@ int EulerExplicitSolverClass = core::RegisterObject("A simple explicit time inte EulerExplicitSolver::EulerExplicitSolver() : d_symplectic( initData( &d_symplectic, true, "symplectic", "If true, the velocities are updated before the positions and the method is symplectic (more robust). If false, the positions are updated before the velocities (standard Euler, less robust).") ) - , d_forceDiagonalMassMatrixOptimization(initData(&d_forceDiagonalMassMatrixOptimization, false, "forceDiagonalMassMatrixOptimization", "If true, the optimization used to solve the system assuming a diagonal mass matrix is used, even if the mass matrix is not diagonal. False by default.")) - , d_forceBuildingMatrixSystem(initData(&d_forceBuildingMatrixSystem, false, "forceBuildingMatrixSystem", "If true, the global linear system is built and solved, even if the mass matrix is diagonal. False by default.")) , d_threadSafeVisitor(initData(&d_threadSafeVisitor, false, "threadSafeVisitor", "If true, do not use realloc and free visitors in fwdInteractionForceField.")) { } @@ -87,14 +85,8 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, MechanicalGetNonDiagonalMassesCountVisitor(&mop.mparams, &nbNonDiagonalMasses).execute(this->getContext()); // Mass matrix is diagonal, solution can thus be found by computing acc = f/m - if((nbNonDiagonalMasses == 0. || d_forceDiagonalMassMatrixOptimization.getValue()) && !d_forceBuildingMatrixSystem.getValue()) + if(nbNonDiagonalMasses == 0.) { - if (d_forceDiagonalMassMatrixOptimization.getValue() && nbNonDiagonalMasses > 0.) - { - msg_warning() << "You requested the optimization based on diagonal mass matrix " - "('forceDiagonalMassMatrixOptimization' is true), but at least one " - "non-diagonal mass matrix has been detected. It may not work as expected."; - } // acc = M^-1 * f computeAcceleration(&mop, acc, f); projectResponse(&mop, acc); @@ -102,12 +94,6 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, } else { - if (nbNonDiagonalMasses == 0.) - { - msg_warning() << "You requested to build explicitly the global linear system. This time consumming method" - "is not necessary if working with only diagonal mass matrices. Set 'forceBuildingMatrixSystem'" - " to false to remove this warning and use the optimized implementation."; - } core::behavior::MultiMatrix matrix(&mop); // Build the global matrix. In this solver, it is the global mass matrix diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h index e23a3366991..2c4c2de68e9 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h @@ -81,8 +81,6 @@ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::be void solve(const core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override; Data d_symplectic; ///< If true, the velocities are updated before the positions and the method is symplectic (more robust). If false, the positions are updated before the velocities (standard Euler, less robust). - Data d_forceDiagonalMassMatrixOptimization; ///< If true, the optimization used to solve the system assuming a diagonal mass matrix is used, even if the mass matrix is not diagonal. False by default. - Data d_forceBuildingMatrixSystem; ///< If true, the global linear system is built and solved, even if the mass matrix is diagonal. False by default. Data d_threadSafeVisitor; /// Given an input derivative order (0 for position, 1 for velocity, 2 for acceleration), diff --git a/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h index a056edc8a38..cbe37f6c740 100644 --- a/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h +++ b/SofaKernel/modules/SofaSimulationCore/src/sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h @@ -47,11 +47,6 @@ class SOFA_SIMULATION_CORE_API MechanicalGetNonDiagonalMassesCountVisitor : publ return true; } - bool stopAtMechanicalMapping(simulation::Node* /*node*/, sofa::core::BaseMapping* /*map*/) override - { - return false; - } - }; } \ No newline at end of file From 5c0e8585fbc42bcbb39e131c311c7e9695a1b51d Mon Sep 17 00:00:00 2001 From: alxbilger Date: Tue, 8 Jun 2021 08:54:05 +0200 Subject: [PATCH 5/7] [SofaExplicitOdeSolver] Apply separate gravity on the velocity parameter --- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index d02247caa45..1f43bf346df 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -75,7 +75,7 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, acc.realloc(&vop, !d_threadSafeVisitor.getValue(), true); - addSeparateGravity(&mop, dt, core::VecDerivId::velocity()); + addSeparateGravity(&mop, dt, vResult); computeForce(&mop, f); projectResponse(&mop, acc); From b47ec6c2467940f3a8d1a87017571810ad126542 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 24 Jun 2021 10:15:21 +0200 Subject: [PATCH 6/7] [SofaExplicitOdeSolver] Forgot super parse function --- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index 41aa4e44f3c..b379b7bb129 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -251,14 +251,15 @@ void EulerExplicitSolver::init() void EulerExplicitSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) { + Inherit1::parse(arg); + const char* val = arg->getAttribute("optimizedForDiagonalMatrix",nullptr) ; if(val) { msg_deprecated() << "The attribute 'optimizedForDiagonalMatrix' is deprecated since SOFA 21.06." << msgendl << "This data was previously used to solve the system more efficiently in case the " "global mass matrix were diagonal." << msgendl - << "Now, this property is detected automatically. See also the data 'forceDiagonalMassMatrixOptimization'" - "and 'forceBuildingMatrixSystem'"; + << "Now, this property is detected automatically."; } } From 92774c4c3e2c4f5b1c80bf7d49d6e9c42bf303a0 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 24 Jun 2021 15:48:35 +0200 Subject: [PATCH 7/7] [SofaExplicitOdeSolver] Fix constraint projection when system is assembled --- .../src/SofaExplicitOdeSolver/EulerSolver.cpp | 10 +++++----- .../src/SofaExplicitOdeSolver/EulerSolver.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp index 1f43bf346df..33ac7e44bab 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.cpp @@ -77,7 +77,6 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, addSeparateGravity(&mop, dt, vResult); computeForce(&mop, f); - projectResponse(&mop, acc); // Mass matrix is diagonal, solution can thus be found by computing acc = f/m if(d_optimizedForDiagonalMatrix.getValue()) @@ -89,6 +88,8 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, } else { + projectResponse(&mop, f); + core::behavior::MultiMatrix matrix(&mop); // Build the global matrix. In this solver, it is the global mass matrix @@ -276,15 +277,14 @@ void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::Mechanic mop->accFromF(acc, f); } -void EulerExplicitSolver::projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc) +void EulerExplicitSolver::projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId vecId) { sofa::helper::ScopedAdvancedTimer timer("projectResponse"); // Calls the "projectResponse" method of every BaseProjectiveConstraintSet objects found in the // current context tree. An example of such constraint set is the FixedConstraint. In this case, - // it will set to 0 every row (i, _) of the right-hand side (force) vector for the ith degree of - // freedom. - mop->projectResponse(acc); + // it will set to 0 every row (i, _) of the input vector for the ith degree of freedom. + mop->projectResponse(vecId); } void EulerExplicitSolver::solveConstraints(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc) diff --git a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h index 1f0aa61bcbe..71125731f8c 100644 --- a/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h +++ b/SofaKernel/modules/SofaExplicitOdeSolver/src/SofaExplicitOdeSolver/EulerSolver.h @@ -120,7 +120,7 @@ class SOFA_SOFAEXPLICITODESOLVER_API EulerExplicitSolver : public sofa::core::be core::ConstMultiVecDerivId f); /// Apply projective constraints, such as FixedConstraint - static void projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc); + static void projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId vecId); static void solveConstraints(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc);