LegacyStaticODESolver.h

#pragma once
 
#include <Caribou/config.h>
#include <sofa/core/behavior/OdeSolver.h>
#include <sofa/simulation/MechanicalMatrixVisitor.h>
#include <sofa/core/behavior/MultiVec.h>
 
namespace SofaCaribou::ode {
 
using sofa::core::objectmodel::Data;
 
class LegacyStaticODESolver : public sofa::core::behavior::OdeSolver
{
public:
    SOFA_CLASS(LegacyStaticODESolver, sofa::core::behavior::OdeSolver);
    LegacyStaticODESolver();
 
public:
    void solve (const sofa::core::ExecParams* params /* PARAMS FIRST */, double dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override;
 
    auto iteration_times() const -> const std::vector<UNSIGNED_INTEGER_TYPE> & {
        return p_times;
    }
 
    auto squared_residuals() const -> const std::vector<FLOATING_POINT_TYPE> & {
        return p_squared_residuals;
    }
 
    auto squared_initial_residual() const -> const FLOATING_POINT_TYPE & {
        return p_squared_initial_residual;
    }
 
    auto iterative_linear_solver_squared_residuals() const -> const std::vector<std::vector<FLOATING_POINT_TYPE>> & {
        return p_iterative_linear_solver_squared_residuals;
    }
 
    auto iterative_linear_solver_squared_rhs_norms() const -> const std::vector<FLOATING_POINT_TYPE> & {
        return p_iterative_linear_solver_squared_rhs_norms;
    }
 
    double getVelocityIntegrationFactor() const override
    {
        return 1.0; // getContext()->getDt();
    }
 
    double getPositionIntegrationFactor() const override
    {
        return getPositionIntegrationFactor(getContext()->getDt());
    }
 
    virtual double getPositionIntegrationFactor(double dt ) const
    {
        return dt;
    }
 
    double getIntegrationFactor(int inputDerivative, int outputDerivative) const override
    {
        return getIntegrationFactor(inputDerivative, outputDerivative, getContext()->getDt());
    }
 
    double getIntegrationFactor(int inputDerivative, int outputDerivative, double dt) const
    {
        double matrix[3][3] =
            {
                { 1, dt, 0},
                { 0, 1, 0},
                { 0, 0, 0}
            };
        if (inputDerivative >= 3 || outputDerivative >= 3)
            return 0;
        else
            return matrix[outputDerivative][inputDerivative];
    }
 
    double getSolutionIntegrationFactor(int outputDerivative) const override
    {
        return getSolutionIntegrationFactor(outputDerivative, getContext()->getDt());
    }
 
    double getSolutionIntegrationFactor(int outputDerivative, double dt) const
    {
        double vect[3] = { dt, 1, 1/dt};
        if (outputDerivative >= 3)
            return 0;
        else
            return vect[outputDerivative];
    }
 
protected:
 
    Data<unsigned> d_newton_iterations;
    Data<double> d_correction_tolerance_threshold;
    Data<double> d_residual_tolerance_threshold;
    Data<bool> d_shoud_diverge_when_residual_is_growing;
    Data<bool> d_warm_start;
 
    Data<bool> d_converged;
 
private:
    sofa::core::behavior::MultiVecDeriv dx;
 
    sofa::core::behavior::MultiVecDeriv U;
 
    std::vector<UNSIGNED_INTEGER_TYPE> p_times;
 
    std::vector<FLOATING_POINT_TYPE> p_squared_residuals;
 
    FLOATING_POINT_TYPE p_squared_initial_residual;
 
    std::vector<std::vector<FLOATING_POINT_TYPE>> p_iterative_linear_solver_squared_residuals;
 
    std::vector<FLOATING_POINT_TYPE> p_iterative_linear_solver_squared_rhs_norms;
};
 
 
} // namespace SofaCaribou::ode