ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-18 20:57:55 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

SAIntegrator: Add Runge-Kutta-Fehlberg 4(5), an adaptive step-size integrator. #1114

Browse Source
This commit is contained in:
Penn, John M 047828115 2021-02-22 17:17:29 -06:00
parent ae9ecf2196
commit 8e3f99e4fe
2 changed files with 146 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
trick_source/trick_utils/SAIntegrator/include/SAIntegrator.hh
View File

@ -147,6 +147,29 @@ namespace SA {
};
std::ostream& operator<<(std::ostream& os, const RK3_8Integrator& I);
class RKF45Integrator : public FirstOrderODEIntegrator {
protected:
double epsilon;
double next_h; // the next value of h necessary to maintain accuracy.
double last_h;
// default_h will represent the maximum value of h.
void advanceIndyVar( double h);
public:
RKF45Integrator();
RKF45Integrator( double epsilon, double h, unsigned int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
RKF45Integrator( double epsilon, double h, unsigned int N, double* in_out_vars[], DerivsFunc derivs_func, void* udata);
RKF45Integrator(const RKF45Integrator& other);
RKF45Integrator& operator=( const RKF45Integrator& rhs);
~RKF45Integrator();
void step();
// Returns the next suggested step-size.
double adaptive_step( double h);
friend std::ostream& operator<<(std::ostream& os, const SA::RKF45Integrator& I);
};
std::ostream& operator<<(std::ostream& os, const RKF45Integrator& I);
typedef void (*Derivs2Func)( double t, double x[], double v[], double derivs[], void* udata);
class EulerCromerIntegrator : public Integrator {

123
trick_source/trick_utils/SAIntegrator/src/SAIntegrator.cpp
View File

@ -571,6 +571,129 @@ std::ostream& SA::operator<<(std::ostream& os, const RK3_8Integrator& I) {
return os;
}
// ------------------------------------------------------------
// Class RKF45Integrator
// ------------------------------------------------------------
// Default Constructor
SA::RKF45Integrator::RKF45Integrator() : FirstOrderODEIntegrator() {
epsilon = 0.00000000001;
next_h = default_h;
}
// Constructor
SA::RKF45Integrator::RKF45Integrator(double eps,
double h, unsigned int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
epsilon = eps;
next_h = h;
}
// Constructor
SA::RKF45Integrator::RKF45Integrator(double eps,
double h, unsigned int N, double* in_out_vars[], DerivsFunc dfunc, void* udata)
: RKF45Integrator(eps, h, N, in_out_vars, in_out_vars, dfunc, udata) {}
// Copy Constructor
SA::RKF45Integrator::RKF45Integrator(const RKF45Integrator& other)
: FirstOrderODEIntegrator(other) {
epsilon = other.epsilon;
next_h = other.next_h;
}
// Assignment Operator
SA::RKF45Integrator& SA::RKF45Integrator::operator=( const SA::RKF45Integrator& rhs) {
if (this != &rhs) {
FirstOrderODEIntegrator::operator=(rhs);
}
epsilon = rhs.epsilon;
next_h = rhs.next_h;
return *this;
}
// Destructor
SA::RKF45Integrator::~RKF45Integrator() {}
void SA::RKF45Integrator::advanceIndyVar(double h) {
last_h = h; X_out = X_in + h;
}
void SA::RKF45Integrator::step() {
adaptive_step( next_h );
}
double SA::RKF45Integrator::adaptive_step(double h) {
double wstate[5][state_size];
double derivs[6][state_size];
double w2[state_size];
double R;
do {
(*derivs_func)( X_in, inState, derivs[0], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[0][i] = inState[i] + h * (1/4.0) * derivs[0][i];
}
(*derivs_func)( X_in + (1/4.0) * h , wstate[0], derivs[1], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[1][i] = inState[i] + h * ((3/32.0)*derivs[0][i] +
(9/32.0)*derivs[1][i]);
}
(*derivs_func)( X_in + (3/8.0) * h , wstate[1], derivs[2], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[2][i] = inState[i] + h * (( 1932/2197.0) * derivs[0][i] +
(-7200/2197.0) * derivs[1][i] +
( 7296/2197.0) * derivs[2][i]);
}
(*derivs_func)( X_in + (12/13.0) * h , wstate[2], derivs[3], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[3][i] = inState[i] + h * (( 439/216.0) * derivs[0][i] +
(-8.0) * derivs[1][i] +
( 3680/513.0) * derivs[2][i] +
(-845/4104.0) * derivs[3][i]);
}
(*derivs_func)( X_in + h , wstate[3], derivs[4], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[4][i] = inState[i] + h * ((-8/27.0) * derivs[0][i] +
( 2.0) * derivs[1][i] +
(-3544/2565.0) * derivs[2][i] +
( 1859/4104.0) * derivs[3][i] +
(-11/40.0) * derivs[4][i]);
}
(*derivs_func)( X_in + (1/2.0)* h , wstate[4], derivs[5], user_data);
for (unsigned int i=0; i<state_size; i++) {
outState[i] = inState[i] + ((25/216.0) * derivs[0][i] +
(1408/2565.0) * derivs[2][i] +
(2197/4104.0) * derivs[3][i] +
(-1/5.0) * derivs[4][i]) * h;
}
for (unsigned int i=0; i<state_size; i++) {
w2[i] = inState[i] + (( 16/135.0) * derivs[0][i] +
( 6656/12825.0) * derivs[2][i] +
( 28561/56430.0) * derivs[3][i] +
(-9/50.0) * derivs[4][i] +
( 2/55.0) * derivs[5][i]) * h;
}
last_h = h;
R = 0.0;
for (unsigned int i=0; i<state_size; i++) {
double RI = fabs(outState[i] - w2[i]) / h;
if (RI>R) R = RI;
}
if (R == 0.0) {
next_h = default_h;
} else {
double delta = 0.84 * pow((epsilon/R), 0.25);
next_h = delta * h;
if (next_h > default_h)
next_h = default_h;
}
h = next_h;
} while (R > epsilon);
advanceIndyVar(last_h);
return (default_h);
}
// Insertion Operator
std::ostream& SA::operator<<(std::ostream& os, const RKF45Integrator& I) {
os << (SA::FirstOrderODEIntegrator)I;
os << "\nepsilon: " << I.epsilon;
os << "\nnext_h: " << I.next_h;
return os;
}
// ------------------------------------------------------------
// Class EulerCromerIntegrator
// ------------------------------------------------------------