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

Add rootfinding to stand-alone integrator library. #1070

Browse Source
This commit is contained in:
Penn, John M 047828115 2020-10-29 12:30:38 -05:00
parent 5aa2a62a7d
commit 23f04ffea2
18 changed files with 618 additions and 282 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
trick_source/trick_utils/SAIntegrator/README.md
View File

@ -6,6 +6,7 @@
* [class Integrator](#class-Integrator)
* [typedef derivsFunc](#typedef-derivsFunc)
* [class FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator)
* [typedef RootErrorFunc](#typedef-RootErrorFunc)
* [class FirstOrderODEVariableStepIntegrator](#class-FirstOrderODEVariableStepIntegrator)
* [class EulerIntegrator](#class-EulerIntegrator)
* [class HeunsMethod](#class-HeunsMethod)
@ -33,41 +34,52 @@ Some examples of using these integrators can be found in the **examples/** direc
### Description
This class represents an **integrator**.
|Member |Type |Description|
|----------|------------|--------------------|
|indyVarIn |```double```|Independent variable value of the input state.|
|indyVarOut|```double```|Independent variable value of the output state.|
|default_h |```double```|Default integration step-size|
|user_data |```void*``` | A pointer to user defined data that will be passed to user-defined functions when called by the Integrator. |
### Constructor
#### ```Integrator(double h, void* user_data);```
#### ```Integrator(double h, void* udata);```
|Parameter|Type |Description |
|---------|------------|-------------|
|h |```double```| Default integration step-size |
|udata |```void*``` | A pointer to user defined data that will be passed to user-defined functions when called by the Integrator. |
### Destructor
#### ```virtual ~Integrator() {}```
|Parameter|Type |Description|
|---------|------------|--------------------|
|h |```double```|Default step-size|
|user_data|```void*```| A pointer to user defined data that will be passed to a derivsFunc when called by the Integrator. |
### Member Functions
#### ```virtual void step();```
Increment the independent variable by the default step-size.
Derived classes should override this method to perform a numeric integration step. The default behavior is to simply increment the independent variable by the default integration step-size.
#### ```virtual void load() = 0;```
#### ```virtual void load();```
Load the input state.
Derived classes should override this method to load/prepare the integrator for the next integration step. The default behavior is to set the input value of the independent value to its previous output value, i.e, ```indyVarIn = indyVarOut```.
#### ```virtual void unload() = 0;```
Load the output state.
Derived classes must override this method to disseminate the values of the output state to their respective destination variables.
#### ```double getIndyVar();```
Return the value of the independent variable (i.e, the variable you are integrating over.) If you are integrating over time, this value will be the accumulated time.
Return the value of the independent variable (i.e, the variable over which you are integrating) If you are integrating over time, this value will be the accumulated time.
#### ```double setIndyVar( double t);```
Set the value of the independent variable. (i.e, the variable you are integrating over.) If you are integrating over time, this value will be the accumulated time.
Set the value of the independent variable. (i.e, the variable over which you are integrating) If you are integrating over time, this value will be the accumulated time.
<a id=typedef-derivsFunc></a>
## typedef derivsFunc
```
typedef void (*derivsFunc)( double x, double state[], double derivs[], void* user_data);
typedef void (*derivsFunc)( double x, double state[], double derivs[], void* udata);
```
where:
@ -76,7 +88,7 @@ where:
|x|```double```| independent variable |
|state|```double*```| (input) an array of state variable values |
|derivs|```double*```| (output) an array into which derivatives are to be returned|
|user_data|```void*```| a pointer to user_data |
|udata|```void*```| a pointer to user_data.|
#### Example
```
@ -87,6 +99,16 @@ void my_derivs( double t, double state[], double deriv[], void* udata) { ... }
## class FirstOrderODEIntegrator
Derived from [Integrator](#class-Integrator).
|Member |Type |Description|
|-----------|------------------|-------------------------|
|state_size |```unsigned int```|Size of the state vector.|
|inState |```double*``` |Input state vector to the integrator.|
|outState |```double*``` |Output state vector from the integrator.|
|inVars |```double**``` |Array of pointers to the variables from which input state vector values are copied.|
|outVars |```double**``` |Array of pointers to the variables to which output state vector values are copied.|
|derivs_func|```DerivsFunc``` |Function thats generates the function (an array of state derivatives) to be integrated.|
|last_h |```double``` |the **last** integration step-size.|
### Description
This class represents an integrator for a first order [Ordinary Differential Equation (ODE)]([https://en.wikipedia.org/wiki/Ordinary_differential_equation).
@ -104,53 +126,82 @@ where:
<a id=FOODEConstructorParameters></a>
|Parameter|Type|Description|
|---|---|---|
|h|```double```|Default integration step-size. |
|N|```int```|Number of state variables to be integrated|
|in_vars|```double*```|Array of pointers to the state variables from which we ```load()``` the integrator state (```in_vars``` and ```out_vars``` will generally point to the same array of pointers.)|
|out_vars|```double*```|Array of pointers to the state variables to which we ```unload()``` the integrator state (```in_vars``` and ```out_vars``` will generally point to the same array of pointers.)|
| derivs_func |[derivsFunc](#typedef-derivsFunc)| A function that returns |
|user_data|```void*```| A pointer to user defined data that will be passed to a derivsFunc when called by the Integrator. |
|Parameter |Type |Description|
|-------------|-------------|-----------|
|h |```double``` |Default integration step-size. |
|N |```int``` |Number of state variables to be integrated|
|in_vars |```double*```|Array of pointers to the state variables from which we ```load()``` the integrator state (```in_vars``` and ```out_vars``` will generally point to the same array of pointers.)|
|out_vars |```double*```|Array of pointers to the state variables to which we ```unload()``` the integrator state (```in_vars``` and ```out_vars``` will generally point to the same array of pointers.)|
| derivs_func |[derivsFunc](#typedef-derivsFunc)| Function thats generates the function (the derivatives) to be integrated. |
|user_data |```void*``` | A pointer to user defined data that will be passed to a derivsFunc when called by the Integrator. |
### Member Functions
#### void step( double h )
Integrate over step-size specified by **h**.
|Parameter|Type|Description|
|---|---|---|
|h | double| The integration step-size |
#### virtual void undo_step()
If the integrator has not already been reset then :
1. Subtract the last step-size from the independent variable.
2. Copy the input state back to the origin variables.
3. Set the integrator's 'reset' mode to true.
Calling load() sets the 'reset' mode to false.
#### void step()
Integrate over the default step-size.
#### void load()
Load the integrator's initial state from the variables specified by **in_vars**.
#### void unload();
#### ```void load()```
Load the integrator's initial state from the variables specified by **in_vars**. The initial value of the independent variable for the next step will be its final value from the previous step.
![load](images/half_load.png)
#### ```void unload()```
Unload the integrator's result state to the variables specified by **out_vars**.
![unload](images/unload.png)
#### ```virtual void undo_step()```
Undo the effect of the last integration step, and unload.
![undo_step](images/undo_step.png)
#### ```void load_from_outState()```
Load the integrator's initial state from ```outState```, rather than from the variables referenced by ```in_vars```.
![load_from_outState](images/load_from_outState.png)
#### ```double** set_in_vars( double* in_vars[])```
Specify the variables from which ```inState``` values are to be copied when ```load()``` is called. The number of elements in this array must equal the number of state variables.
![set_in_vars](images/set_in_vars.png)
#### ```double** set_out_vars( double* out_vars[])```
Specify the variables to which ```outState``` values are to be copied when ```unload()``` is called. The number of elements in this array must equal the number of state variables.
![set_out_vars](images/set_out_vars.png)
<a id=typedef-RootErrorFunc></a>
## typedef RootErrorFunc
```
typedef double (*RootErrorFunc)( double x, double state[], RootFinder* root_finder, void* udata);
```
where:
|Parameter |Type |Description|
|------------|-----------------|-----------|
|x |```double``` | independent variable |
|state |```double*``` | array of state variable values |
|root_finder |```RootFinder*```||
|udata |```void*``` | a pointer to user_data.|
<a id=class-FirstOrderODEVariableStepIntegrator></a>
## class FirstOrderODEVariableStepIntegrator
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|Member |Type |Description|
|-------------------|--------------------|-----------|
| root_finder |```RootFinder*``` ||
| root\_error\_func |```RootErrorFunc ```||
### Description
This class represents an integrator for a first order ODE, that be
This class represents a first order ODE integrator whose step-size can be varied.
### Constructor
### Member Functions
#### virtual void step( double h );
#### ```virtual void variable_step( double h)```
Derived classes should override this method to calculate ```outState``` using some integration algorithm, given ```X_in```, ```inState```, and ```derivs_func```. The over-riding method should also pass the ```user_data``` when calling the ```derivsFunc```. The default behavior is to simply add the integration step-size (```h```) to ```X_in```.
![step](images/step.png)
#### ```void step()```
Call the virtual function (```variable_step()```) with the default step-size.
Then, if a RootFinder has been specified, search that interval for roots .
#### ```void add_Rootfinder( RootFinder* root_finder, RootErrorFunc rfunc)```
<a id=class-EulerIntegrator></a>
## class EulerIntegrator
@ -162,7 +213,7 @@ The Euler method is a first order numerical integration method. It is the simple
```
EulerIntegrator(double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-HeunsMethod></a>
## class HeunsMethod
@ -175,7 +226,7 @@ This integrator implements
```
HeunsMethod( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-RK2Integrator></a>
## class RK2Integrator
@ -186,7 +237,7 @@ The Runga-Kutta-2 method is a second order, explicit, numerical integration meth
```
RK2Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-RK4Integrator></a>
## class RK4Integrator
@ -197,7 +248,7 @@ The Runga-Kutta-4 method is a fourth order, explicit, numerical integration meth
```
RK4Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-RK3_8Integrator></a>
## class RK3_8Integrator
@ -208,7 +259,8 @@ The Runga-Kutta-3/8 method is a fourth order, explicit, numerical integration me
```
RK3_8Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of
[FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-ABM2Integrator></a>
## class ABM2Integrator
@ -221,7 +273,7 @@ The ABM2Integrator implements the second-order Adams-Bashforth-Moulton predictor
```
ABM2Integrator ( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-ABM4Integrator></a>
## class ABM4Integrator
@ -235,7 +287,7 @@ The ABM2Integrator implements the second-order Adams-Bashforth-Moulton predictor
ABM4Integrator ( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
```
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
[Constructor Parameters](#FOODEConstructorParameters) are those of [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
<a id=class-EulerCromerIntegrator></a>
## EulerCromerIntegrator

52
trick_source/trick_utils/SAIntegrator/examples/CannonBall/CannonBall.cpp
View File

@ -1,45 +1,69 @@
#include <math.h>
#include <stdio.h>
#include <iostream>
#include "SAIntegrator.hh"
struct Cannon {
double pos[2];
double vel[2];
double acc[2];
};
void init_state ( Cannon& cannon ) {
cannon.pos[0] = 0.0;
cannon.pos[1] = 0.0;
cannon.vel[0] = 50.0 * cos(M_PI/6.0);
cannon.vel[1] = 50.0 * sin(M_PI/6.0);
cannon.acc[0] = 0.0;
cannon.acc[1] = -9.81;
}
void deriv( double t, double state[], double derivs[], void* udata) {
void calc_derivs( double t, double state[], double derivs[], void* udata) {
derivs[0] = state[2]; // vel_x
derivs[1] = state[3]; // vel_y
derivs[2] = 0.0; // acc_x
derivs[3] = -9.81; // acc_y
derivs[2] = state[4]; // acc_x
derivs[3] = state[5]; // acc_y
derivs[4] = 0.0; // Zero acc rate of change
derivs[5] = 0.0;
}
double impact( double t, double state[], RootFinder* root_finder, void* udata) {
double root_error = root_finder->find_roots(t, state[1]);
if (root_error == 0.0) {
printf("---------------------------------------------------------------\n");
printf("Impact at t = %5.10f x = %5.10f y = %5.10f.\n", t, state[0], state[1]);
printf("---------------------------------------------------------------\n");
root_finder->init();
state[1] = 0.0000000001; // pos_y
state[2] = 0.0; // vel_x
state[3] = 0.0; // vel_y
state[4] = 0.0; // acc_x
state[5] = 0.0; // acc_y
}
return (root_error);
}
void print_header() {
printf ("t, cannon.pos[0], cannon.pos[1], cannon.vel[0], cannon.vel[1]\n");
printf ("t, cannon.pos[0], cannon.pos[1], cannon.vel[0], cannon.vel[1], cannon.acc[0], cannon.acc[1]\n");
}
void print_state( double t, Cannon& cannon) {
printf ("%5.3f, %10.10f, %10.10f, %10.10f, %10.10f\n", t, cannon.pos[0], cannon.pos[1], cannon.vel[0], cannon.vel[1]);
printf ("%5.3f, %5.10f, %5.10f, %5.10f, %5.10f, %5.10f, %5.10f\n",
t, cannon.pos[0], cannon.pos[1], cannon.vel[0], cannon.vel[1], cannon.acc[0], cannon.acc[1]);
}
int main ( int argc, char* argv[]) {
Cannon cannon;
double* state_var_p[4] = { &(cannon.pos[0]), &(cannon.pos[1]), &(cannon.vel[0]), &(cannon.vel[1]) };
unsigned count = 0;
double* state_var_p[6] = { &(cannon.pos[0]), &(cannon.pos[1]),
&(cannon.vel[0]), &(cannon.vel[1]),
&(cannon.acc[0]), &(cannon.acc[1]) };
double dt = 0.01;
double t = count * dt;
SA::RK2Integrator integ(dt, 4, state_var_p, state_var_p, deriv, NULL);
double t = 0.0;
SA::RK2Integrator integ(dt, 6, state_var_p, state_var_p, calc_derivs, NULL);
RootFinder root_finder(0.00000000001, Negative);
integ.add_Rootfinder(&root_finder, &impact);
init_state(cannon);
print_header();
print_state(t, cannon );
while (t < 5.1) {
print_state( t, cannon);
while (t < 5.3) {
integ.load();
integ.step();
integ.unload();
count ++;
t = count * dt;
print_state(t, cannon );
t = integ.getIndyVar();
print_state( t, cannon);
}
}

BIN
trick_source/trick_utils/SAIntegrator/images/load.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 89 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/load_from_outState.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 98 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/setIndyVar.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/set_in_vars.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 81 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/set_out_vars.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 83 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/step.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 84 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/undo_step.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 88 KiB

BIN
trick_source/trick_utils/SAIntegrator/images/unload.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 78 KiB

26
trick_source/trick_utils/SAIntegrator/include/Rootfinder.hh Normal file
View File

@ -0,0 +1,26 @@
typedef enum {
Negative = -1,
Unconstrained,
Positive
} SlopeConstraint;
class RootFinder {
public:
void init();
RootFinder();
RootFinder (double tolerance, SlopeConstraint constraint);
double find_roots( double x, double f_error );
private:
double f_upper;
double x_upper;
bool upper_set;
double f_lower;
double x_lower;
bool lower_set;
double prev_f_error;
double f_error_tol;
int iterations;
SlopeConstraint slope_constraint;
SlopeConstraint f_slope;
};

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

@ -1,107 +1,105 @@
#include "Rootfinder.hh"
namespace SA {
class Integrator {
protected:
double indyVar; // Independent Variable
double default_h; // Default step-size
void * udata; // User data
double X_in; // Independent Variable In
double X_out; // Independent Variable Out
double default_h; // Default step-size
void* user_data; // User data
public:
Integrator(double h, void* user_data): indyVar(0.0), default_h(h), udata(user_data) {};
Integrator(double h, void* udata): X_in(0.0), X_out(0.0), default_h(h), user_data(udata) {};
virtual ~Integrator() {}
virtual void step() { indyVar += default_h; }; // Integrate over default step-size (default_h)
virtual void load() = 0;
virtual void unload() = 0;
double getIndyVar() {return indyVar;}
void setIndyVar(double t) {indyVar = t;}
virtual void step();
virtual void load();
virtual void unload();
double getIndyVar();
void setIndyVar(double v);
};
typedef void (*derivsFunc)( double x, double state[], double derivs[], void* user_data);
typedef void (*DerivsFunc)( double x, double state[], double derivs[], void* udata);
class FirstOrderODEIntegrator : public Integrator {
protected:
unsigned int state_size; // size of the state vector.
double* istate;
double* ostate;
double** orig_vars;
double** dest_vars;
derivsFunc derivs_func;
bool reset;
double* inState;
double* outState;
double** inVars;
double** outVars;
DerivsFunc derivs_func;
double last_h; // What was the last step-size? For undo_step().
public:
FirstOrderODEIntegrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data);
~FirstOrderODEIntegrator();
virtual void undo_step();
FirstOrderODEIntegrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
virtual ~FirstOrderODEIntegrator();
virtual double undo_step();
void load();
void unload();
void load_from_ostate();
void set_in_vars( double* in_vars[]);
void set_out_vars( double* out_vars[]);
void load_from_outState();
double** set_in_vars( double* in_vars[]);
double** set_out_vars( double* out_vars[]);
};
class FirstOrderODEVariableStepIntegrator : public FirstOrderODEIntegrator {
typedef double (*RootErrorFunc)( double x, double state[], RootFinder* root_finder, void* udata);
class FirstOrderODEVariableStepIntegrator : public FirstOrderODEIntegrator {
RootFinder* root_finder;
RootErrorFunc root_error_func;
public:
FirstOrderODEVariableStepIntegrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data);
using Integrator::step;
virtual void step( double h );
FirstOrderODEVariableStepIntegrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~FirstOrderODEVariableStepIntegrator();
void add_Rootfinder( RootFinder* root_finder, RootErrorFunc rfunc);
// virtual void variable_step( double h)=0;
virtual void variable_step( double h);
void step();
protected:
void advanceIndyVar( double h );
private:
void find_roots(double h, unsigned int depth);
};
class EulerIntegrator : public FirstOrderODEVariableStepIntegrator {
public:
double *derivs;
EulerIntegrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
EulerIntegrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~EulerIntegrator();
void step( double h);
void step();
void variable_step( double h);
};
class HeunsMethod : public FirstOrderODEVariableStepIntegrator {
public:
double *wstate;
double *derivs[2];
HeunsMethod( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
HeunsMethod( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~HeunsMethod();
void step( double h);
void step();
void variable_step( double h);
};
class RK2Integrator : public FirstOrderODEVariableStepIntegrator {
public:
double *wstate;
double *derivs[2];
RK2Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
RK2Integrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc derivs_func, void* udata);
~RK2Integrator();
void step( double h);
void step();
void variable_step( double h);
};
class RK4Integrator : public FirstOrderODEVariableStepIntegrator {
public:
double *wstate[3];
double *derivs[4];
RK4Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
RK4Integrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~RK4Integrator();
void step( double h);
void step();
void variable_step( double h);
};
class RK3_8Integrator : public FirstOrderODEVariableStepIntegrator {
public:
double *wstate[4];
double *derivs[4];
RK3_8Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
RK3_8Integrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~RK3_8Integrator();
void step( double h);
void step();
void variable_step( double h);
};
class EulerCromerIntegrator : public Integrator {
@ -115,11 +113,11 @@ namespace SA {
double* v_out;
double* g_out;
double* f_out;
derivsFunc gderivs;
derivsFunc fderivs;
DerivsFunc gderivs;
DerivsFunc fderivs;
public:
EulerCromerIntegrator(double dt, int N, double* xp[], double* vp[], derivsFunc g, derivsFunc f , void* user_data);
EulerCromerIntegrator(double dt, int N, double* xp[], double* vp[], DerivsFunc g, DerivsFunc f , void* udata);
~EulerCromerIntegrator();
void step( double dt);
void step();
@ -137,7 +135,7 @@ namespace SA {
// unsigned int currentHistorySize;
//
// public:
// ABMIntegrator(int history_size, double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
// ABMIntegrator(int history_size, double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
// ~ABMIntegrator();
// void step();
// void undo_step();
@ -154,10 +152,10 @@ namespace SA {
FirstOrderODEVariableStepIntegrator* priming_integrator;
public:
ABM2Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
ABM2Integrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~ABM2Integrator();
void step();
void undo_step();
double undo_step();
};
// AdamsBashforthMoulton 4
@ -171,9 +169,9 @@ namespace SA {
FirstOrderODEVariableStepIntegrator* priming_integrator;
public:
ABM4Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
ABM4Integrator( double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata);
~ABM4Integrator();
void step();
void undo_step();
double undo_step();
};
}

3
trick_source/trick_utils/SAIntegrator/makefile
View File

@ -8,7 +8,8 @@ INCLUDE_DIRS = -Iinclude
OBJDIR = obj
LIBDIR = lib
LIBNAME = libSAInteg.a
LIBOBJS = ${OBJDIR}/SAIntegrator.o
LIBOBJS = ${OBJDIR}/SAIntegrator.o \
${OBJDIR}/RootFinder.o
all: test examples

87
trick_source/trick_utils/SAIntegrator/src/RootFinder.cpp Normal file
View File

@ -0,0 +1,87 @@
#include "Rootfinder.hh"
#include <math.h>
#include <float.h>
#include <iostream>
void RootFinder::init () {
f_error_tol = 0.00000000001;
iterations = 0;
prev_f_error = DBL_MAX;
slope_constraint = Unconstrained;
lower_set = false;
upper_set = false;
}
RootFinder::RootFinder () {
init();
}
RootFinder::RootFinder (double tolerance, SlopeConstraint constraint) {
init();
f_error_tol = tolerance;
slope_constraint = constraint;
}
// Given the values of the independent variable, and the function,
// estimate the distance of the independent variable from functions root.
double RootFinder::find_roots( double x, double f_error ) {
if ( ( fabs(f_error) < f_error_tol) ||
(( iterations > 0) && (fabs(prev_f_error - f_error) < f_error_tol))
) {
if (slope_constraint == Unconstrained) {
return (0.0);
} else if (slope_constraint == Positive && lower_set) {
return (0.0);
} else if (slope_constraint == Negative && upper_set) {
return (0.0);
}
}
if (f_error < 0.0) {
f_lower = f_error;
x_lower = x;
lower_set = true;
} else if (f_error > 0.0) {
f_upper = f_error;
x_upper = x;
upper_set = true;
}
iterations++;
if (upper_set && lower_set) {
double x_correction_estimate;
if (fabs(f_error) < f_error_tol)
x_correction_estimate = 0.0;
else {
x_correction_estimate = -f_error/((f_upper-f_lower)/(x_upper-x_lower));
if (iterations > 20)
x_correction_estimate = 0.0;
}
/* Check for Positive or Negative constraint */
switch (slope_constraint) {
case Unconstrained:
prev_f_error = f_error;
return (x_correction_estimate);
case Positive:
if (f_slope == Positive) {
prev_f_error = f_error;
return (x_correction_estimate);
} else {
lower_set = false;
f_slope = Positive;
}
break;
case Negative:
if (f_slope == Negative) {
prev_f_error = f_error;
return (x_correction_estimate);
} else {
upper_set = false;
f_slope = Negative;
}
break;
}
f_slope = Unconstrained;
} else if (lower_set == 1) {
f_slope = Positive;
} else if (upper_set == 1) {
f_slope = Negative;
}
iterations = 0;
return (DBL_MAX);
}

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

@ -3,97 +3,129 @@
#include <iostream>
#include "SAIntegrator.hh"
// ------------------------------------------------------------
void SA::Integrator::step() { X_out = X_in + default_h; }
void SA::Integrator::load() { X_in = X_out; }
void SA::Integrator::unload() {}
double SA::Integrator::getIndyVar() { return X_out; }
void SA::Integrator::setIndyVar(double v) { X_out = v; }
SA::FirstOrderODEIntegrator::FirstOrderODEIntegrator(double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: Integrator(h, user_data) {
// ------------------------------------------------------------
SA::FirstOrderODEIntegrator::FirstOrderODEIntegrator(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: Integrator(h, udata) {
state_size = N;
orig_vars = in_vars;
dest_vars = out_vars;
istate = new double[state_size];
ostate = new double[state_size];
derivs_func = func;
reset=false;
inVars = in_vars;
outVars = out_vars;
inState = new double[state_size];
outState = new double[state_size];
derivs_func = dfunc;
}
SA::FirstOrderODEIntegrator::~FirstOrderODEIntegrator() {
delete istate;
delete ostate;
delete inState;
delete outState;
}
void SA::FirstOrderODEIntegrator::load() {
reset = false;
if (orig_vars != NULL) {
if (inVars != NULL) {
for (unsigned int i=0 ; i<state_size; i++ ) {
istate[i] = *(orig_vars[i]);
inState[i] = *(inVars[i]);
}
Integrator::load();
} else {
std::cerr << "Error: SA::FirstOrderODEIntegrator::load(). orig_vars is not set." << std::endl;
std::cerr << "Error: SA::FirstOrderODEIntegrator::load(). inVars is not set." << std::endl;
}
}
void SA::FirstOrderODEIntegrator::unload() {
if (dest_vars != NULL) {
if (outVars != NULL) {
for (unsigned int i=0 ; i<state_size; i++ ) {
*(dest_vars[i]) = ostate[i];
*(outVars[i]) = outState[i];
}
} else {
std::cerr << "Error: SA::FirstOrderODEIntegrator::load(). dest_vars is not set." << std::endl;
std::cerr << "Error: SA::FirstOrderODEIntegrator::load(). outVars is not set." << std::endl;
}
}
void SA::FirstOrderODEIntegrator::load_from_ostate(){
void SA::FirstOrderODEIntegrator::load_from_outState(){
for (unsigned int i=0 ; i<state_size; i++ ) {
istate[i] = ostate[i];
inState[i] = outState[i];
}
Integrator::load();
}
void SA::FirstOrderODEIntegrator::set_in_vars( double* in_vars[]){
orig_vars = in_vars;
double** SA::FirstOrderODEIntegrator::set_in_vars( double* in_vars[]){
double **ret = inVars;
inVars = in_vars;
return (ret);
}
void SA::FirstOrderODEIntegrator::set_out_vars( double* out_vars[]){
dest_vars = out_vars;
double** SA::FirstOrderODEIntegrator::set_out_vars( double* out_vars[]){
double **ret = outVars;
outVars = out_vars;
return (ret);
}
void SA::FirstOrderODEIntegrator::undo_step() {
if (!reset) { // If we've not already reset the integrator, then reset it.
double SA::FirstOrderODEIntegrator::undo_step() {
for (unsigned int i=0 ; i<state_size; i++ ) {
*(orig_vars[i]) = istate[i]; // Copy istate values back to the state variables from whence they came.
*(inVars[i]) = inState[i];
}
X_out = X_in;
return (last_h);
}
// ------------------------------------------------------------
SA::FirstOrderODEVariableStepIntegrator::FirstOrderODEVariableStepIntegrator(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
root_finder = (RootFinder*) NULL;
root_error_func = (RootErrorFunc)NULL;
}
SA::FirstOrderODEVariableStepIntegrator::~FirstOrderODEVariableStepIntegrator() {
// User is responsible for deleting the root_finder.
}
void SA::FirstOrderODEVariableStepIntegrator::add_Rootfinder(
RootFinder* rootFinder, RootErrorFunc rfunc) {
root_finder = rootFinder;
root_error_func = rfunc;
}
void SA::FirstOrderODEVariableStepIntegrator::variable_step( double h) {
last_h = h; X_out = X_in + h;
}
void SA::FirstOrderODEVariableStepIntegrator::find_roots(double h, unsigned int depth) {
double new_h;
double h_correction = (*root_error_func)( getIndyVar(), outState, root_finder, user_data );
if (h_correction < 0.0) {
while (h_correction != 0.0) {
new_h = undo_step() + h_correction;
variable_step(new_h);
h_correction = (*root_error_func)( getIndyVar(), outState, root_finder, user_data );
}
load_from_outState();
variable_step(h-new_h);
if (depth > 0) {
find_roots(h-new_h, depth-1);
}
indyVar -= last_h; // Undo the last step.
}
reset = true;
}
// ------------------------------------------------------------
SA::FirstOrderODEVariableStepIntegrator::FirstOrderODEVariableStepIntegrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars, func, user_data) {}
void SA::FirstOrderODEVariableStepIntegrator::step( double h ) {
last_h = h; indyVar += h;
void SA::FirstOrderODEVariableStepIntegrator::step() {
variable_step( default_h );
if ((root_finder != NULL) && (root_error_func != NULL)) {
find_roots( default_h, 5 );
}
}
// ------------------------------------------------------------
SA::EulerIntegrator::EulerIntegrator(double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, func, user_data)
{
SA::EulerIntegrator::EulerIntegrator(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
derivs = new double[N];
}
SA::EulerIntegrator::~EulerIntegrator() {
delete derivs;
}
void SA::EulerIntegrator::step( double h) {
(*derivs_func)( indyVar, istate, derivs, udata);
void SA::EulerIntegrator::variable_step( double h) {
(*derivs_func)( X_in, inState, derivs, user_data);
for (unsigned int i=0; i<state_size; i++) {
ostate[i] = istate[i] + derivs[i] * h;
outState[i] = inState[i] + derivs[i] * h;
}
SA::FirstOrderODEVariableStepIntegrator::step(h);
SA::FirstOrderODEVariableStepIntegrator::variable_step(h);
}
void SA::EulerIntegrator::step() {
step(default_h);
}
// ------------------------------------------------------------
SA::HeunsMethod::HeunsMethod( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, func, user_data)
{
SA::HeunsMethod::HeunsMethod(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
wstate = new double[N];
for (unsigned int i = 0; i < 2 ; i++) {
derivs[i] = new double[N];
@ -105,26 +137,21 @@ SA::HeunsMethod::~HeunsMethod() {
delete derivs[i];
}
}
void SA::HeunsMethod::step( double h) {
(*derivs_func)( indyVar, istate, derivs[0], udata);
void SA::HeunsMethod::variable_step( double h) {
(*derivs_func)( X_in, inState, derivs[0], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[i] = istate[i] + h * derivs[0][i];
wstate[i] = inState[i] + h * derivs[0][i];
}
(*derivs_func)( indyVar, wstate, derivs[1], udata);
(*derivs_func)( X_in, wstate, derivs[1], user_data);
for (unsigned int i=0; i<state_size; i++) {
ostate[i] = istate[i] + (h/2) * ( derivs[0][i] + derivs[1][i] );
outState[i] = inState[i] + (h/2) * ( derivs[0][i] + derivs[1][i] );
}
SA::FirstOrderODEVariableStepIntegrator::step(h);
SA::FirstOrderODEVariableStepIntegrator::variable_step(h);
}
void SA::HeunsMethod::step() {
step(default_h);
}
// ------------------------------------------------------------
SA::RK2Integrator::RK2Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, func, user_data)
{
SA::RK2Integrator::RK2Integrator(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
wstate = new double[N];
for (unsigned int i = 0; i < 2 ; i++) {
derivs[i] = new double[N];
@ -136,26 +163,21 @@ SA::RK2Integrator::~RK2Integrator() {
delete derivs[i];
}
}
void SA::RK2Integrator::step( double h) {
(*derivs_func)( indyVar, istate, derivs[0], udata);
void SA::RK2Integrator::variable_step( double h) {
(*derivs_func)( X_in, inState, derivs[0], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[i] = istate[i] + 0.5 * h * derivs[0][i];
wstate[i] = inState[i] + 0.5 * h * derivs[0][i];
}
(*derivs_func)( indyVar + 0.5 * h , wstate, derivs[1], udata);
(*derivs_func)( X_in + 0.5 * h , wstate, derivs[1], user_data);
for (unsigned int i=0; i<state_size; i++) {
ostate[i] = istate[i] + h * derivs[1][i];
outState[i] = inState[i] + h * derivs[1][i];
}
SA::FirstOrderODEVariableStepIntegrator::step(h);
SA::FirstOrderODEVariableStepIntegrator::variable_step(h);
}
void SA::RK2Integrator::step() {
step(default_h);
}
// ------------------------------------------------------------
SA::RK4Integrator::RK4Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, func, user_data)
{
SA::RK4Integrator::RK4Integrator(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
for (unsigned int i = 0; i < 3 ; i++) {
wstate[i] = new double[N];
}
@ -171,37 +193,32 @@ SA::RK4Integrator::~RK4Integrator() {
delete (derivs[i]);
}
}
void SA::RK4Integrator::step( double h) {
(*derivs_func)( indyVar, istate, derivs[0], udata);
void SA::RK4Integrator::variable_step( double h) {
(*derivs_func)( X_in, inState, derivs[0], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[0][i] = istate[i] + 0.5 * derivs[0][i] * h;
wstate[0][i] = inState[i] + 0.5 * derivs[0][i] * h;
}
(*derivs_func)( indyVar + 0.5 * h , wstate[0], derivs[1], udata);
(*derivs_func)( X_in + 0.5 * h , wstate[0], derivs[1], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[1][i] = istate[i] + 0.5 * derivs[1][i] * h;
wstate[1][i] = inState[i] + 0.5 * derivs[1][i] * h;
}
(*derivs_func)( indyVar + 0.5 * h , wstate[1], derivs[2], udata);
(*derivs_func)( X_in + 0.5 * h , wstate[1], derivs[2], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[2][i] = istate[i] + derivs[2][i] * h;
wstate[2][i] = inState[i] + derivs[2][i] * h;
}
(*derivs_func)( indyVar + h , wstate[2], derivs[3], udata);
(*derivs_func)( X_in + h , wstate[2], derivs[3], user_data);
for (unsigned int i=0; i<state_size; i++) {
ostate[i] = istate[i] + ((1/6.0)* derivs[0][i] +
outState[i] = inState[i] + ((1/6.0)* derivs[0][i] +
(1/3.0)* derivs[1][i] +
(1/3.0)* derivs[2][i] +
(1/6.0)* derivs[3][i]) * h;
}
SA::FirstOrderODEVariableStepIntegrator::step(h);
SA::FirstOrderODEVariableStepIntegrator::variable_step(h);
}
void SA::RK4Integrator::step() {
step(default_h);
}
// ------------------------------------------------------------
SA::RK3_8Integrator::RK3_8Integrator( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars ,func, user_data)
{
SA::RK3_8Integrator::RK3_8Integrator(
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEVariableStepIntegrator(h, N, in_vars, out_vars ,dfunc, udata) {
for (unsigned int i = 0; i < 3 ; i++) {
wstate[i] = new double[N];
}
@ -217,36 +234,32 @@ SA::RK3_8Integrator::~RK3_8Integrator() {
delete (derivs[i]);
}
}
void SA::RK3_8Integrator::step( double h) {
(*derivs_func)( indyVar, istate, derivs[0], udata);
void SA::RK3_8Integrator::variable_step( double h) {
(*derivs_func)( X_in, inState, derivs[0], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[0][i] = istate[i] + h * (1/3.0) * derivs[0][i];
wstate[0][i] = inState[i] + h * (1/3.0) * derivs[0][i];
}
(*derivs_func)( indyVar + (1/3.0) * h , wstate[0], derivs[1], udata);
(*derivs_func)( X_in + (1/3.0) * h , wstate[0], derivs[1], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[1][i] = istate[i] + h * ((-1/3.0) * derivs[0][i] + derivs[1][i]);
wstate[1][i] = inState[i] + h * ((-1/3.0) * derivs[0][i] + derivs[1][i]);
}
(*derivs_func)( indyVar + (2/3.0) * h , wstate[1], derivs[2], udata);
(*derivs_func)( X_in + (2/3.0) * h , wstate[1], derivs[2], user_data);
for (unsigned int i=0; i<state_size; i++) {
wstate[2][i] = istate[i] + h * (derivs[0][i] - derivs[1][i] + derivs[2][i]);
wstate[2][i] = inState[i] + h * (derivs[0][i] - derivs[1][i] + derivs[2][i]);
}
(*derivs_func)( indyVar + h , wstate[2], derivs[3], udata);
(*derivs_func)( X_in + h , wstate[2], derivs[3], user_data);
for (unsigned int i=0; i<state_size; i++) {
ostate[i] = istate[i] + ((1/8.0)* derivs[0][i] +
outState[i] = inState[i] + ((1/8.0)* derivs[0][i] +
(3/8.0)* derivs[1][i] +
(3/8.0)* derivs[2][i] +
(1/8.0)* derivs[3][i]) * h;
}
SA::FirstOrderODEVariableStepIntegrator::step(h);
SA::FirstOrderODEVariableStepIntegrator::variable_step(h);
}
void SA::RK3_8Integrator::step() {
step(default_h);
}
// ------------------------------------------------------------
SA::EulerCromerIntegrator::EulerCromerIntegrator(double dt, int N, double* xp[], double* vp[], derivsFunc gfunc, derivsFunc ffunc, void* user_data)
: Integrator(dt, user_data) {
SA::EulerCromerIntegrator::EulerCromerIntegrator(
double dt, int N, double* xp[], double* vp[], DerivsFunc gfunc, DerivsFunc ffunc, void* udata)
:Integrator(dt, udata) {
state_size = N;
x_p = new double*[N];
v_p = new double*[N];
@ -274,15 +287,15 @@ SA::EulerCromerIntegrator::~EulerCromerIntegrator() {
delete v_out;
}
void SA::EulerCromerIntegrator::step( double dt) {
(*gderivs)( indyVar, x_in, g_out, udata);
(*gderivs)( X_in, x_in, g_out, user_data);
for (int i=0 ; i<state_size; i++ ) {
v_out[i] = v_in[i] + g_out[i] * dt;
}
(*fderivs)( indyVar, v_in, f_out, udata);
(*fderivs)( X_in, v_in, f_out, user_data);
for (int i=0 ; i<state_size; i++ ) {
x_out[i] = x_in[i] + f_out[i] * dt;
}
indyVar += dt;
X_out = X_in + dt;
}
void SA::EulerCromerIntegrator::step() {
step(default_h);
@ -317,8 +330,8 @@ void SA::EulerCromerIntegrator::unload(){
// {(9/24.0), (19/24.0), (-5/24.0), (1/24.0), 0.0},
// {(251/720.0), (646/720.0), (-264/720.0), (106/720.0), (-19/720.0)}
// };
// SA::ABMIntegrator::ABMIntegrator ( int history_size, double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
// : FirstOrderODEIntegrator(h, N, in_vars, out_vars ,func, user_data) {
// SA::ABMIntegrator::ABMIntegrator ( int history_size, double h, int N, double* in_vars[], double* out_vars[], DerivsFunc func, void* udata)
// : FirstOrderODEIntegrator(h, N, in_vars, out_vars ,func, udata) {
//
// algorithmHistorySize=history_size; // The amount of history that we intend to use in this ABM integrator.
// bufferSize=algorithmHistorySize+1; // The size of the buffer needs to be one more than the history so that an integration step can be reset.
@ -340,7 +353,8 @@ void SA::EulerCromerIntegrator::unload(){
// void SA::ABMIntegrator::step() {
//
// hix = (hix+1)%bufferSize; // Move history index forward
// (*derivs_func)( indyVar, istate, deriv_history[hix], udata); // Calculated and store the deriv for current, corrected state.
// // (*derivs_func)( indyVar, inState, deriv_history[hix], user_data);
// (*derivs_func)( X_in, inState, deriv_history[hix], user_data); // Calculated and store the deriv for current, corrected state.
// // Increase the size of the stored history, up to the limit specified by the user.
// currentHistorySize = (currentHistorySize < algorithmHistorySize) ? currentHistorySize+1 : algorithmHistorySize;
// // (Predictor) Predict the next state using the Adams-Bashforth explicit method.
@ -349,36 +363,37 @@ void SA::EulerCromerIntegrator::unload(){
// for (int n=0,j=hix; n<currentHistorySize ; n++,j=(j+bufferSize-1)%bufferSize) {
// composite_deriv[i] += ABCoeffs[currentHistorySize-1][n] * deriv_history[j][i];
// }
// ostate[i] = istate[i] + default_h * composite_deriv[i];
// outState[i] = inState[i] + default_h * composite_deriv[i];
// }
// // Move history index forward, so we can temporarily store the derivative of the predicted next state.
// // We do not increase the currentHistorySize.
// hix = (hix+1)%bufferSize;
// (*derivs_func)( indyVar, ostate, deriv_history[hix], udata); // Calc deriv for predicted next state.
// (*derivs_func)( X_out, outState, deriv_history[hix], user_data); // Calc deriv for predicted next state.
// // (Corrector) Refine the next state using the Adams-Moulton implicit method. This is the corrected next state.
// for (int i=0; i<state_size; i++) {
// composite_deriv[i] = 0.0;
// for (int n=0,j=hix; n<currentHistorySize ; n++,j=(j+bufferSize-1)%bufferSize) {
// composite_deriv[i] += AMCoeffs[currentHistorySize-1][n] * deriv_history[j][i];
// }
// ostate[i] = istate[i] + default_h * composite_deriv[i];
// outState[i] = inState[i] + default_h * composite_deriv[i];
// }
// // Move history index backward, so we over-write the predicted state with the corrected state on our next step().
// hix = (hix+bufferSize-1)%bufferSize;
// SA::Integrator::step();
// }
//
// void SA::ABMIntegrator::undo_step() {
// double SA::ABMIntegrator::undo_step() {
// hix = (hix+bufferSize-1)%bufferSize;
// FirstOrderODEIntegrator::undo_step();
// return (FirstOrderODEIntegrator::undo_step());
// }
// =======================================================================
static const double AB2Coeffs[2] = {(3/2.0), (-1/2.0)};
static const double AM2Coeffs[2] = {(1/2.0), (1/2.0)};
SA::ABM2Integrator::ABM2Integrator ( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars ,func, user_data) {
SA::ABM2Integrator::ABM2Integrator (
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
// The amount of history that we intend to use in this ABM integrator.
bufferSize=2+1; // The size of the buffer needs to be one more than the history so that an integration step can be reset.
hix=0;
@ -392,13 +407,13 @@ SA::ABM2Integrator::ABM2Integrator ( double h, int N, double* in_vars[], double*
// NEW: Create a priming integrator.
double** priming_integrator_in_vars = new double*[state_size];
for (unsigned int i=0; i<N ; i++) {
priming_integrator_in_vars[i] = &(istate[i]);
priming_integrator_in_vars[i] = &(inState[i]);
}
double** priming_integrator_out_vars = new double*[state_size];
for (unsigned int i=0; i<N ; i++) {
priming_integrator_out_vars[i] = &(ostate[i]);
priming_integrator_out_vars[i] = &(outState[i]);
}
priming_integrator = new SA::RK2Integrator(h, N, priming_integrator_in_vars, priming_integrator_out_vars, func, user_data);
priming_integrator = new SA::RK2Integrator(h, N, priming_integrator_in_vars, priming_integrator_out_vars, dfunc, udata);
}
SA::ABM2Integrator::~ABM2Integrator() {
for (int i=0; i<bufferSize ; i++) {
@ -411,7 +426,7 @@ SA::ABM2Integrator::~ABM2Integrator() {
void SA::ABM2Integrator::step() {
hix = (hix+1)%bufferSize; // Move history index forward
(*derivs_func)( indyVar, istate, deriv_history[hix], udata); // Calculated and store the deriv for current, corrected state.
(*derivs_func)( X_in, inState, deriv_history[hix], user_data); // Calculated and store the deriv for current, corrected state.
// Increase the size of the stored history, up to the limit specified by the user.
currentHistorySize = (currentHistorySize < 2) ? currentHistorySize+1 : 2;
@ -426,36 +441,36 @@ void SA::ABM2Integrator::step() {
for (int n=0,j=hix; n<2 ; n++,j=(j+bufferSize-1)%bufferSize) {
composite_deriv[i] += AB2Coeffs[n] * deriv_history[j][i];
}
ostate[i] = istate[i] + default_h * composite_deriv[i];
outState[i] = inState[i] + default_h * composite_deriv[i];
}
// Move history index forward, so we can temporarily store the derivative of the predicted next state.
// We do not increase the currentHistorySize.
hix = (hix+1)%bufferSize;
(*derivs_func)( indyVar, ostate, deriv_history[hix], udata); // Calc deriv for predicted next state.
(*derivs_func)( X_in, outState, deriv_history[hix], user_data); // Calc deriv for predicted next state.
// (Corrector) Refine the next state using the Adams-Moulton implicit method. This is the corrected next state.
for (int i=0; i<state_size; i++) {
composite_deriv[i] = 0.0;
for (int n=0,j=hix; n<2 ; n++,j=(j+bufferSize-1)%bufferSize) {
composite_deriv[i] += AM2Coeffs[n] * deriv_history[j][i];
}
ostate[i] = istate[i] + default_h * composite_deriv[i];
outState[i] = inState[i] + default_h * composite_deriv[i];
}
// Move history index backward, so we over-write the predicted state with the corrected state on our next step().
hix = (hix+bufferSize-1)%bufferSize;
}
SA::Integrator::step();
}
void SA::ABM2Integrator::undo_step() {
double SA::ABM2Integrator::undo_step() {
hix = (hix+bufferSize-1)%bufferSize;
FirstOrderODEIntegrator::undo_step();
return ( FirstOrderODEIntegrator::undo_step());
}
// =======================================================================
static const double AB4Coeffs[4] = {(55/24.0), (-59/24.0), (37/24.0), (-9/24.0)};
static const double AM4Coeffs[4] = {(9/24.0), (19/24.0), (-5/24.0), (1/24.0)};
SA::ABM4Integrator::ABM4Integrator ( double h, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars ,func, user_data) {
SA::ABM4Integrator::ABM4Integrator (
double h, int N, double* in_vars[], double* out_vars[], DerivsFunc dfunc, void* udata)
: FirstOrderODEIntegrator(h, N, in_vars, out_vars, dfunc, udata) {
// The amount of history that we intend to use in this ABM integrator.
bufferSize=4+1; // The size of the buffer needs to be one more than the history so that an integration step can be reset.
@ -466,19 +481,17 @@ SA::ABM4Integrator::ABM4Integrator ( double h, int N, double* in_vars[], double*
deriv_history[i] = new double[state_size];
}
composite_deriv = new double[state_size];
// NEW: Create a priming integrator.
double** priming_integrator_in_vars = new double*[state_size];
for (unsigned int i=0; i<N ; i++) {
priming_integrator_in_vars[i] = &(istate[i]);
priming_integrator_in_vars[i] = &(inState[i]);
}
double** priming_integrator_out_vars = new double*[state_size];
for (unsigned int i=0; i<N ; i++) {
priming_integrator_out_vars[i] = &(ostate[i]);
priming_integrator_out_vars[i] = &(outState[i]);
}
priming_integrator = new SA::RK4Integrator(h, N, priming_integrator_in_vars, priming_integrator_out_vars, func, user_data);
priming_integrator = new SA::RK4Integrator(h, N, priming_integrator_in_vars, priming_integrator_out_vars, dfunc, udata);
}
SA::ABM4Integrator::~ABM4Integrator() {
for (int i=0; i<bufferSize ; i++) {
delete (deriv_history[i]);
@ -487,11 +500,10 @@ SA::ABM4Integrator::~ABM4Integrator() {
delete(composite_deriv);
delete priming_integrator;
}
void SA::ABM4Integrator::step() {
hix = (hix+1)%bufferSize; // Move history index forward
(*derivs_func)( indyVar, istate, deriv_history[hix], udata); // Calculated and store the deriv for current, corrected state.
(*derivs_func)( X_in, inState, deriv_history[hix], user_data); // Calculated and store the deriv for current, corrected state.
// Increase the size of the stored history, up to the limit specified by the user.
currentHistorySize = (currentHistorySize < 4) ? currentHistorySize+1 : 4;
if ( currentHistorySize < 4 ) {
@ -505,26 +517,26 @@ void SA::ABM4Integrator::step() {
for (int n=0,j=hix; n<4 ; n++,j=(j+bufferSize-1)%bufferSize) {
composite_deriv[i] += AB4Coeffs[n] * deriv_history[j][i];
}
ostate[i] = istate[i] + default_h * composite_deriv[i];
outState[i] = inState[i] + default_h * composite_deriv[i];
}
// Move history index forward, so we can temporarily store the derivative of the predicted next state.
// We do not increase the currentHistorySize.
hix = (hix+1)%bufferSize;
(*derivs_func)( indyVar, ostate, deriv_history[hix], udata); // Calc deriv for predicted next state.
(*derivs_func)( X_out, outState, deriv_history[hix], user_data); // Calc deriv for predicted next state.
// (Corrector) Refine the next state using the Adams-Moulton implicit method. This is the corrected next state.
for (int i=0; i<state_size; i++) {
composite_deriv[i] = 0.0;
for (int n=0,j=hix; n<4 ; n++,j=(j+bufferSize-1)%bufferSize) {
composite_deriv[i] += AM4Coeffs[n] * deriv_history[j][i];
}
ostate[i] = istate[i] + default_h * composite_deriv[i];
outState[i] = inState[i] + default_h * composite_deriv[i];
}
// Move history index backward, so we over-write the predicted state with the corrected state on our next step().
hix = (hix+bufferSize-1)%bufferSize;
}
SA::Integrator::step();
}
void SA::ABM4Integrator::undo_step() {
double SA::ABM4Integrator::undo_step() {
hix = (hix+bufferSize-1)%bufferSize;
FirstOrderODEIntegrator::undo_step();
return (FirstOrderODEIntegrator::undo_step());
}

11
trick_source/trick_utils/SAIntegrator/unittest/Makefile
View File

@ -13,10 +13,11 @@ SAI_LIBOBJS = ${SAI_OBJDIR}/Integrator.o
LIBDIRS = -L${SAI_LIBDIR} -L${GTEST_HOME}/lib64 -L${GTEST_HOME}/lib
all: test
all: test
test: SAIntegrator_unittest
test: SAIntegrator_unittest RootFinder_unittest
./SAIntegrator_unittest --gtest_output=xml:${TRICK_HOME}/trick_test/SAIntegrator_unittest.xml
./RootFinder_unittest --gtest_output=xml:${TRICK_HOME}/trick_test/RootFinder_unittest.xml
SAIntegrator_unittest.o : SAIntegrator_unittest.cc
$(TRICK_CXX) $(TRICK_CPPFLAGS) $(INCLUDE_DIRS) -c $<
@ -24,6 +25,12 @@ SAIntegrator_unittest.o : SAIntegrator_unittest.cc
SAIntegrator_unittest : ${SAI_LIBDIR}/${SAI_LIBNAME} SAIntegrator_unittest.o
$(TRICK_CXX) $(TRICK_CPPFLAGS) -o $@ $^ ${LIBDIRS} -lSAInteg -lgtest -lgtest_main -lpthread
RootFinder_unittest.o : RootFinder_unittest.cc
$(TRICK_CXX) $(TRICK_CPPFLAGS) $(INCLUDE_DIRS) -c $<
RootFinder_unittest : ${SAI_LIBDIR}/${SAI_LIBNAME} RootFinder_unittest.o
$(TRICK_CXX) $(TRICK_CPPFLAGS) -o $@ $^ ${LIBDIRS} -lSAInteg -lgtest -lgtest_main -lpthread
${SAI_LIBDIR}/${SAI_LIBNAME} :
$(MAKE) -C ..

80
trick_source/trick_utils/SAIntegrator/unittest/RootFinder_unittest.cc Normal file
View File

@ -0,0 +1,80 @@
#include <gtest/gtest.h>
#include <iostream>
#include "Rootfinder.hh"
#include <math.h>
#define EXCEPTABLE_ERROR 0.00000000001
double func_negative_slope(double x) {
// Root: x = 8.5
return ( -0.5 * x + 4.25);
}
double func_positive_slope(double x) {
// Root: x = 8.5
return ( 0.5 * x + -4.25);
}
// If we immediately hit a root, without any history, we can't tell whether the
// slope is positive or negative. So, we can only "detect" the root if the rootfinder
// is unconstrained.
TEST(RootFinder_unittest, Root_with_no_history_unconstrained) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Unconstrained );
double root_error;
root_error = root_finder->find_roots(8.5, func_negative_slope(8.5));
EXPECT_NEAR(root_error, 0.0, EXCEPTABLE_ERROR);
}
TEST(RootFinder_unittest, Unconstrained_with_negative_slope_function) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Unconstrained );
double root_error;
root_error = root_finder->find_roots(8, func_negative_slope(8));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
root_error = root_finder->find_roots(9, func_negative_slope(9));
EXPECT_NEAR(root_error, -0.5, EXCEPTABLE_ERROR);
}
TEST(RootFinder_unittest, Unconstrained_with_positive_slope_function) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Unconstrained );
double root_error;
root_error = root_finder->find_roots(8, func_positive_slope(8));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
root_error = root_finder->find_roots(9, func_positive_slope(9));
EXPECT_NEAR(root_error, -0.5, EXCEPTABLE_ERROR);
}
TEST(RootFinder_unittest, Positive_constraint_with_negative_slope_function) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Positive );
double root_error;
root_error = root_finder->find_roots(8, func_negative_slope(8));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
root_error = root_finder->find_roots(9, func_negative_slope(9));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
}
TEST(RootFinder_unittest, Positive_constraint_with_positive_slope_function) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Positive );
double root_error;
root_error = root_finder->find_roots(8, func_positive_slope(8));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
root_error = root_finder->find_roots(9, func_positive_slope(9));
EXPECT_NEAR(root_error, -0.5, EXCEPTABLE_ERROR);
}
TEST(RootFinder_unittest, Negative_constraint_with_negative_slope_function) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Negative );
double root_error;
root_error = root_finder->find_roots(8, func_negative_slope(8));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
root_error = root_finder->find_roots(9, func_negative_slope(9));
EXPECT_NEAR(root_error, -0.5, EXCEPTABLE_ERROR);
}
TEST(RootFinder_unittest, Negative_constraint_with_positive_slope_function) {
RootFinder* root_finder = new RootFinder ( 0.00000000001, Negative );
double root_error;
root_error = root_finder->find_roots(8, func_positive_slope(8));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
root_error = root_finder->find_roots(9, func_positive_slope(9));
EXPECT_NEAR(root_error, DBL_MAX, EXCEPTABLE_ERROR);
}

49
trick_source/trick_utils/SAIntegrator/unittest/SAIntegrator_unittest.cc
View File

@ -121,3 +121,52 @@ TEST(SAIntegrator_unittest, RungeKutta38_1) {
EXPECT_NEAR(state[2], (24+2/3.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[3], 20.0, EXCEPTABLE_ERROR);
}
TEST(SAIntegrator_unittest, EulerIntegrator_undo_step) {
double state[4] = {0.0, 0.0, 0.0, 0.0};
double* state_var_p[4] = { &(state[0]), &(state[1]), &(state[2]), &(state[3])};
double dx = 0.1;
double x;
SA::EulerIntegrator integ(dx, 4, state_var_p, state_var_p, deriv1, NULL);
integ.load();
integ.step();
integ.unload();
x = integ.getIndyVar();
EXPECT_NEAR(state[0], (4.0/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[1], (-4.0/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[2], (M_PI/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[3], (-M_PI/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(x, 0.1, EXCEPTABLE_ERROR);
integ.undo_step();
x = integ.getIndyVar();
EXPECT_NEAR(state[0], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(state[1], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(state[2], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(state[3], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(x, 0.0, EXCEPTABLE_ERROR);
integ.undo_step();
x = integ.getIndyVar();
EXPECT_NEAR(state[0], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(state[1], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(state[2], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(state[3], 0.0, EXCEPTABLE_ERROR);
EXPECT_NEAR(x, 0.0, EXCEPTABLE_ERROR);
integ.load();
integ.step();
integ.unload();
x = integ.getIndyVar();
EXPECT_NEAR(state[0], (4.0/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[1], (-4.0/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[2], (M_PI/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(state[3], (-M_PI/10.0), EXCEPTABLE_ERROR);
EXPECT_NEAR(x, 0.1, EXCEPTABLE_ERROR);
}