Caribou
LegacyStaticODESolver.h
1 #pragma once
2 
3 #include <Caribou/config.h>
4 #include <sofa/core/behavior/OdeSolver.h>
5 #include <sofa/simulation/MechanicalMatrixVisitor.h>
6 #include <sofa/core/behavior/MultiVec.h>
7 
8 namespace SofaCaribou::ode {
9 
10 using sofa::core::objectmodel::Data;
11 
12 class LegacyStaticODESolver : public sofa::core::behavior::OdeSolver
13 {
14 public:
15  SOFA_CLASS(LegacyStaticODESolver, sofa::core::behavior::OdeSolver);
17 
18 public:
19  void solve (const sofa::core::ExecParams* params /* PARAMS FIRST */, double dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override;
20 
24  auto iteration_times() const -> const std::vector<UNSIGNED_INTEGER_TYPE> & {
25  return p_times;
26  }
27 
31  auto squared_residuals() const -> const std::vector<FLOATING_POINT_TYPE> & {
32  return p_squared_residuals;
33  }
34 
38  auto squared_initial_residual() const -> const FLOATING_POINT_TYPE & {
39  return p_squared_initial_residual;
40  }
41 
46  auto iterative_linear_solver_squared_residuals() const -> const std::vector<std::vector<FLOATING_POINT_TYPE>> & {
47  return p_iterative_linear_solver_squared_residuals;
48  }
49 
54  auto iterative_linear_solver_squared_rhs_norms() const -> const std::vector<FLOATING_POINT_TYPE> & {
55  return p_iterative_linear_solver_squared_rhs_norms;
56  }
57 
62  double getVelocityIntegrationFactor() const override
63  {
64  return 1.0; // getContext()->getDt();
65  }
66 
71  double getPositionIntegrationFactor() const override
72  {
73  return getPositionIntegrationFactor(getContext()->getDt());
74  }
75 
76  virtual double getPositionIntegrationFactor(double dt ) const
77  {
78  return dt;
79  }
80 
96  double getIntegrationFactor(int inputDerivative, int outputDerivative) const override
97  {
98  return getIntegrationFactor(inputDerivative, outputDerivative, getContext()->getDt());
99  }
100 
101  double getIntegrationFactor(int inputDerivative, int outputDerivative, double dt) const
102  {
103  double matrix[3][3] =
104  {
105  { 1, dt, 0},
106  { 0, 1, 0},
107  { 0, 0, 0}
108  };
109  if (inputDerivative >= 3 || outputDerivative >= 3)
110  return 0;
111  else
112  return matrix[outputDerivative][inputDerivative];
113  }
114 
117  double getSolutionIntegrationFactor(int outputDerivative) const override
118  {
119  return getSolutionIntegrationFactor(outputDerivative, getContext()->getDt());
120  }
121 
122  double getSolutionIntegrationFactor(int outputDerivative, double dt) const
123  {
124  double vect[3] = { dt, 1, 1/dt};
125  if (outputDerivative >= 3)
126  return 0;
127  else
128  return vect[outputDerivative];
129  }
130 
131 protected:
132 
134  Data<unsigned> d_newton_iterations;
135  Data<double> d_correction_tolerance_threshold;
136  Data<double> d_residual_tolerance_threshold;
137  Data<bool> d_shoud_diverge_when_residual_is_growing;
138  Data<bool> d_warm_start;
139 
142  Data<bool> d_converged;
143 
144 private:
146  sofa::core::behavior::MultiVecDeriv dx;
147 
149  sofa::core::behavior::MultiVecDeriv U;
150 
152  std::vector<UNSIGNED_INTEGER_TYPE> p_times;
153 
155  std::vector<FLOATING_POINT_TYPE> p_squared_residuals;
156 
158  FLOATING_POINT_TYPE p_squared_initial_residual;
159 
162  std::vector<std::vector<FLOATING_POINT_TYPE>> p_iterative_linear_solver_squared_residuals;
163 
166  std::vector<FLOATING_POINT_TYPE> p_iterative_linear_solver_squared_rhs_norms;
167 };
168 
169 
170 } // namespace SofaCaribou::ode
171 
SofaCaribou::ode::LegacyStaticODESolver::iterative_linear_solver_squared_residuals
auto iterative_linear_solver_squared_residuals() const -> const std::vector< std::vector< FLOATING_POINT_TYPE >> &
Definition: LegacyStaticODESolver.h:46
SofaCaribou::ode::LegacyStaticODESolver::getSolutionIntegrationFactor
double getSolutionIntegrationFactor(int outputDerivative) const override
Given a solution of the linear system, how much will it affect the output derivative of the given ord...
Definition: LegacyStaticODESolver.h:117
SofaCaribou::ode::LegacyStaticODESolver::iterative_linear_solver_squared_rhs_norms
auto iterative_linear_solver_squared_rhs_norms() const -> const std::vector< FLOATING_POINT_TYPE > &
Definition: LegacyStaticODESolver.h:54
SofaCaribou::ode::LegacyStaticODESolver::d_newton_iterations
Data< unsigned > d_newton_iterations
INPUTS.
Definition: LegacyStaticODESolver.h:134
SofaCaribou::ode::LegacyStaticODESolver::getVelocityIntegrationFactor
double getVelocityIntegrationFactor() const override
Given a displacement as computed by the linear system inversion, how much will it affect the velocity...
Definition: LegacyStaticODESolver.h:62
SofaCaribou::ode::LegacyStaticODESolver::iteration_times
auto iteration_times() const -> const std::vector< UNSIGNED_INTEGER_TYPE > &
List of times (in nanoseconds) that each Newton-Raphson iteration took to compute in the last call to...
Definition: LegacyStaticODESolver.h:24
SofaCaribou::ode::LegacyStaticODESolver::squared_residuals
auto squared_residuals() const -> const std::vector< FLOATING_POINT_TYPE > &
Definition: LegacyStaticODESolver.h:31
SofaCaribou::ode::LegacyStaticODESolver
Definition: LegacyStaticODESolver.h:13
SofaCaribou::ode::LegacyStaticODESolver::squared_initial_residual
auto squared_initial_residual() const -> const FLOATING_POINT_TYPE &
Definition: LegacyStaticODESolver.h:38
SofaCaribou::ode::LegacyStaticODESolver::d_converged
Data< bool > d_converged
OUTPUTS Whether or not the last call to solve converged.
Definition: LegacyStaticODESolver.h:142
SofaCaribou::ode::LegacyStaticODESolver::getPositionIntegrationFactor
double getPositionIntegrationFactor() const override
Given a displacement as computed by the linear system inversion, how much will it affect the position...
Definition: LegacyStaticODESolver.h:71
SofaCaribou::ode::LegacyStaticODESolver::getIntegrationFactor
double getIntegrationFactor(int inputDerivative, int outputDerivative) const override
Given an input derivative order (0 for position, 1 for velocity, 2 for acceleration),...
Definition: LegacyStaticODESolver.h:96