mirror of
https://github.com/nasa/trick.git
synced 2025-03-21 11:35:19 +00:00
This commit is contained in:
Penn, John M 047828115
parent
b2ec2313e8
commit
cb3869a36c
255
trick_source/trick_utils/SAIntegrator/README.md
Normal file
255
trick_source/trick_utils/SAIntegrator/README.md
Normal file
@ -0,0 +1,255 @@
|
||||
# Stand-Alone Integration Library
|
||||
|
||||
## Contents
|
||||
|
||||
* [Introduction](#Introduction)
|
||||
* [class Integrator](#class-Integrator)
|
||||
* [typedef derivsFunc](#typedef-derivsFunc)
|
||||
* [class FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator)
|
||||
* [class FirstOrderODEVariableStepIntegrator](#class-FirstOrderODEVariableStepIntegrator)
|
||||
* [class EulerIntegrator](#class-EulerIntegrator)
|
||||
* [class HeunsMethod](#class-HeunsMethod)
|
||||
* [class RK2Integrator](#class-RK2Integrator)
|
||||
* [class RK4Integrator](#class-RK4Integrator)
|
||||
* [class RK3_8Integrator](#class-RK3_8Integrator)
|
||||
* [class EulerCromerIntegrator](#class-EulerCromerIntegrator)
|
||||
* [class ABM2Integrator](#class-ABM2Integrator)
|
||||
* [class ABM4Integrator](#class-ABM4Integrator)
|
||||
|
||||
<a id=Introduction></a>
|
||||
## Introduction
|
||||
The Stand-Alone Integration Library can be used within a Trick simulation, or independent of it.
|
||||
|
||||
Some examples of using these integrators can be found in the **examples/** directory.
|
||||
|
||||
* [CannonBall](examples/CannonBall/README.md) uses the RK2Integrator.
|
||||
* [MassSpringDamper](examples/MassSpringDamper/README.md) uses the EulerCromerIntegrator.
|
||||
* [Orbit](examples/Orbit/README.md) uses the EulerCromerIntegrator.
|
||||
|
||||
<a id=class-Integrator></a>
|
||||
## class Integrator
|
||||
|
||||
### Description
|
||||
This class represents an **integrator**.
|
||||
|
||||
### Constructor
|
||||
|
||||
#### ```Integrator(double dt, void* user_data);```
|
||||
|
||||
|
||||
|Parameter|Type |Description|
|
||||
|---------|------------|--------------------|
|
||||
|dt |```double```|Default time step value|
|
||||
|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 time by the default time-step.
|
||||
|
||||
#### ```virtual void load() = 0;```
|
||||
|
||||
Load the input state.
|
||||
|
||||
#### ```virtual void unload() = 0;```
|
||||
|
||||
Load the output state.
|
||||
|
||||
#### ```double getTime();```
|
||||
Return the integrator time.
|
||||
|
||||
<a id=typedef-derivsFunc></a>
|
||||
## typedef derivsFunc
|
||||
|
||||
```
|
||||
typedef void (*derivsFunc)( double t, double state[], double derivs[], void* user_data);
|
||||
```
|
||||
where:
|
||||
|
||||
|Parameter|Type|Description|
|
||||
|---|---|---|
|
||||
|t|```double```| time|
|
||||
|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 |
|
||||
|
||||
#### Example
|
||||
```
|
||||
void my_derivs( double t, double state[], double deriv[], void* udata) { ... }
|
||||
```
|
||||
|
||||
<a id=class-FirstOrderODEIntegrator></a>
|
||||
## class FirstOrderODEIntegrator
|
||||
Derived from [Integrator](#class-Integrator).
|
||||
|
||||
### Description
|
||||
This class represents an integrator for a first order [Ordinary Differential Equation (ODE)]([https://en.wikipedia.org/wiki/Ordinary_differential_equation).
|
||||
|
||||
### Constructor
|
||||
|
||||
```
|
||||
FirstOrderODEIntegrator( double dt,
|
||||
int N,
|
||||
double* in_vars[],
|
||||
double* out_vars[],
|
||||
derivsFunc func,
|
||||
void* user_data);
|
||||
```
|
||||
where:
|
||||
|
||||
<a id=FOODEConstructorParameters></a>
|
||||
|
||||
|Parameter|Type|Description|
|
||||
|---|---|---|
|
||||
|dt|```double```|Default time step value|
|
||||
|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. |
|
||||
|
||||
### Member Functions
|
||||
|
||||
#### void step( double dt )
|
||||
Integrate over time step specified by **dt**.
|
||||
|
||||
|Parameter|Type|Description|
|
||||
|---|---|---|
|
||||
|dt | double| A variable time-step |
|
||||
|
||||
#### virtual void undo_step()
|
||||
|
||||
If the integrator has not already been reset then :
|
||||
|
||||
1. Decrement time by the last time-step dt.
|
||||
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 time-step.
|
||||
|
||||
#### void load()
|
||||
Load the integrator's initial state from the variables specified by **in_vars**.
|
||||
|
||||
#### void unload();
|
||||
|
||||
Unload the integrator's result state to the variables specified by **out_vars**.
|
||||
|
||||
<a id=class-FirstOrderODEVariableStepIntegrator></a>
|
||||
## class FirstOrderODEVariableStepIntegrator
|
||||
|
||||
### Description
|
||||
This class represents an integrator for a first order ODE, that be
|
||||
|
||||
### Constructor
|
||||
### Member Functions
|
||||
#### virtual void step( double dt );
|
||||
|
||||
<a id=class-EulerIntegrator></a>
|
||||
## class EulerIntegrator
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
### Description
|
||||
The Euler method is a first order numerical integration method. It is the simplest, explicit Runge–Kutta method.
|
||||
### Constructor
|
||||
```
|
||||
EulerIntegrator(double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-HeunsMethod></a>
|
||||
## class HeunsMethod
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
### Description
|
||||
This integrator implements
|
||||
[Heun's Method](https://en.wikipedia.org/wiki/Heun%27s_method).
|
||||
|
||||
### Constructor
|
||||
```
|
||||
HeunsMethod( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-RK2Integrator></a>
|
||||
## class RK2Integrator
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
### Description
|
||||
The Runga-Kutta-2 method is a second order, explicit, numerical integration method.
|
||||
### Constructor
|
||||
```
|
||||
RK2Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-RK4Integrator></a>
|
||||
## class RK4Integrator
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
### Description
|
||||
The Runga-Kutta-4 method is a fourth order, explicit, numerical integration method.
|
||||
### Constructor
|
||||
```
|
||||
RK4Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-RK3_8Integrator></a>
|
||||
## class RK3_8Integrator
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
### Description
|
||||
The Runga-Kutta-3/8 method is a fourth order, explicit, numerical integration method.
|
||||
### Constructor
|
||||
```
|
||||
RK3_8Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-ABM2Integrator></a>
|
||||
## class ABM2Integrator
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
### Description
|
||||
|
||||
The ABM2Integrator implements the second-order Adams-Bashforth-Moulton predictor/corrector method. Adams methods maintain a history of derivatives rather than calculating intermediate values like Runga-Kutta methods.
|
||||
|
||||
### Constructor
|
||||
```
|
||||
ABM2Integrator ( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-ABM4Integrator></a>
|
||||
## class ABM4Integrator
|
||||
Derived from [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
### Description
|
||||
The ABM2Integrator implements the second-order Adams-Bashforth-Moulton predictor/corrector method. Adams methods maintain a history of derivatives rather than calculating intermediate values like Runga-Kutta methods.
|
||||
|
||||
### Constructor
|
||||
```
|
||||
ABM4Integrator ( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
```
|
||||
|
||||
[Constructor Parameters](#FOODEConstructorParameters) are those [FirstOrderODEIntegrator](#class-FirstOrderODEIntegrator).
|
||||
|
||||
<a id=class-EulerCromerIntegrator></a>
|
||||
## EulerCromerIntegrator
|
||||
Derived from [class-Integrator](#class-Integrator).
|
||||
|
||||
### Description
|
||||
EulerCromer is integration method that conserves energy in oscillatory systems better than Runge-Kutta. So, it's good for mass-spring-damper systems, and orbital systems.
|
||||
|
||||
### Constructor
|
||||
```
|
||||
SemiImplicitEuler(double dt, int N, double* xp[], double* vp[], derivsFunc gfunc, derivsFunc ffunc, void* user_data)
|
||||
```
|
||||
|
||||
|Parameter|Type |Description|
|
||||
|-----------|-------------|-----------------------|
|
||||
| dt |```double``` |Default time step value|
|
||||
| N |```int``` |Number of state variables to be integrated|
|
||||
| xp |```double*```|Array of pointers to the variables from which we ```load()``` and to which we ```unload()``` the integrator's position values .|
|
||||
| vp |```double*```|Array of pointers to the variables from which we ```load()``` and to which we ```unload()``` the integrator's velocity values .|
|
||||
| gfunc |[derivsFunc](#typedef-derivsFunc)| A function that returns acceleration |
|
||||
| ffunc |[derivsFunc](#typedef-derivsFunc)| A function that returns velocity |
|
||||
|user_data |```void*```| A pointer to user defined data that will be passed to a derivsFunc when called by the Integrator. |
|
||||
@ -0,0 +1,45 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include "SAIntegrator.hh"
|
||||
|
||||
struct Cannon {
|
||||
double pos[2];
|
||||
double vel[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);
|
||||
}
|
||||
void deriv( 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
|
||||
}
|
||||
void print_header() {
|
||||
printf ("t, cannon.pos[0], cannon.pos[1], cannon.vel[0], cannon.vel[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]);
|
||||
}
|
||||
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 dt = 0.01;
|
||||
double t = count * dt;
|
||||
SA::RK2Integrator integ(dt, 4, state_var_p, state_var_p, deriv, NULL);
|
||||
init_state(cannon);
|
||||
print_header();
|
||||
print_state(t, cannon );
|
||||
while (t < 5.1) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
t = count * dt;
|
||||
print_state(t, cannon );
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,11 @@
|
||||
# CannonBall
|
||||
|
||||
This is an example of using the RK2Integrator to create a simple cannon ball simulation.
|
||||
|
||||
'''
|
||||
$ make
|
||||
$ ./CannonBall > cannon.csv
|
||||
$ python plot_trajectory.py
|
||||
'''
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 397 KiB |
@ -0,0 +1,20 @@
|
||||
|
||||
RM = rm -rf
|
||||
CC = cc
|
||||
CPP = c++
|
||||
|
||||
CXXFLAGS = -g -Wall
|
||||
INCLUDE_DIRS = -I../../include
|
||||
LIBDIR = ../../lib
|
||||
|
||||
all: CannonBall
|
||||
|
||||
CannonBall:
|
||||
$(CPP) $(CXXFLAGS) CannonBall.cpp ${INCLUDE_DIRS} -L${LIBDIR} -lSAInteg -o CannonBall
|
||||
|
||||
clean:
|
||||
${RM} CannonBall.dSYM
|
||||
|
||||
spotless: clean
|
||||
${RM} CannonBall
|
||||
${RM} cannon.csv
|
||||
18
trick_source/trick_utils/SAIntegrator/examples/CannonBall/plot_trajectory.py
Executable file
18
trick_source/trick_utils/SAIntegrator/examples/CannonBall/plot_trajectory.py
Executable file
@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
data = np.genfromtxt('cannon.csv',
|
||||
delimiter=',',
|
||||
skip_header=1,
|
||||
skip_footer=1,
|
||||
names=['t', 'px', 'py', 'vx', 'vx'],
|
||||
dtype=(float, float, float, float, float)
|
||||
)
|
||||
|
||||
curve1 = plt.plot(data['px'], data['py'], 'C1-')
|
||||
plt.title('Cannonball Trajectory')
|
||||
plt.xlabel('pos-x')
|
||||
plt.ylabel('pos-y')
|
||||
plt.grid(True)
|
||||
plt.show()
|
||||
@ -0,0 +1,52 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include "SAIntegrator.hh"
|
||||
#define N_DIMENSIONS 1
|
||||
struct MassSpringDamper {
|
||||
double pos;
|
||||
double vel;
|
||||
double k; // Spring constant
|
||||
double c; // Damping constant
|
||||
double mass;
|
||||
};
|
||||
void init_state ( MassSpringDamper& msd ) {
|
||||
msd.pos = 2.0;
|
||||
msd.vel = 0.0;
|
||||
msd.k = 1.0;
|
||||
msd.c = 0.0;
|
||||
msd.mass = 5.0;
|
||||
}
|
||||
void print_header() {
|
||||
printf ("time, msd.pos, msd.vel\n");
|
||||
}
|
||||
void print_state( double t, MassSpringDamper& msd ) {
|
||||
printf ("%10.10f, %10.10f, %10.10f\n", t, msd.pos, msd.vel);
|
||||
}
|
||||
void G( double t, double x[], double g_out[], void* udata) {
|
||||
MassSpringDamper* msd = (MassSpringDamper*)udata;
|
||||
g_out[0] = -(msd->k/msd->mass) * x[0];
|
||||
}
|
||||
void F( double t, double v[], double f_out[], void* udata) {
|
||||
f_out[0] = v[0];
|
||||
f_out[1] = v[1];
|
||||
}
|
||||
int main ( int argc, char* argv[]) {
|
||||
MassSpringDamper msd;
|
||||
double* x_p[N_DIMENSIONS] = { &msd.pos };
|
||||
double* v_p[N_DIMENSIONS] = { &msd.vel };
|
||||
unsigned count = 0;
|
||||
double dt = 0.001;
|
||||
double time = count * dt;
|
||||
init_state(msd);
|
||||
print_header();
|
||||
print_state(time, msd);
|
||||
SA::EulerCromerIntegrator integ(dt, N_DIMENSIONS, x_p, v_p, G, F, &msd);
|
||||
while (time < 50) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
time = count * dt;
|
||||
print_state(time, msd);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,22 @@
|
||||
# MassSpringDamper
|
||||
|
||||
This program uses the SemiImplicitEuler integrator to simulate a simple mass-spring-damper.
|
||||
|
||||
Generate the results as follows:
|
||||
'''
|
||||
$ make
|
||||
$ ./MassSpringDamper > msd.csv
|
||||
'''
|
||||
|
||||
Plot the results as follows:
|
||||
|
||||
'''
|
||||
$ python plot_position.py
|
||||
'''
|
||||
|
||||
and
|
||||
|
||||
'''
|
||||
$ python plot_velocity.py
|
||||
'''
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 452 KiB |
@ -0,0 +1,20 @@
|
||||
|
||||
RM = rm -rf
|
||||
CC = cc
|
||||
CPP = c++
|
||||
|
||||
CXXFLAGS = -g -Wall
|
||||
INCLUDE_DIRS = -I../../include
|
||||
LIBDIR = ../../lib
|
||||
|
||||
all: MassSpringDamper
|
||||
|
||||
MassSpringDamper: MassSpringDamper.cpp
|
||||
$(CPP) $(CXXFLAGS) MassSpringDamper.cpp ${INCLUDE_DIRS} -L${LIBDIR} -lSAInteg -o MassSpringDamper
|
||||
|
||||
clean:
|
||||
${RM} MassSpringDamper.dSYM
|
||||
|
||||
spotless: clean
|
||||
${RM} MassSpringDamper
|
||||
${RM} msd.csv
|
||||
@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
data = np.genfromtxt('msd.csv',
|
||||
delimiter=',',
|
||||
skip_header=1,
|
||||
skip_footer=1,
|
||||
names=['t', 'pos', 'vel'],
|
||||
dtype=(float, float, float)
|
||||
)
|
||||
|
||||
curve1 = plt.plot(data['t'], data['pos'], 'C1-')
|
||||
plt.title('MSD Position')
|
||||
plt.xlabel('position')
|
||||
plt.ylabel('time')
|
||||
plt.grid(True)
|
||||
plt.show()
|
||||
@ -0,0 +1,57 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include "SAIntegrator.hh"
|
||||
|
||||
#define GRAVITATIONAL_CONSTANT 6.674e-11
|
||||
#define EARTH_MASS 5.9723e24
|
||||
#define EARTH_RADIUS 6367500.0
|
||||
|
||||
struct Orbit {
|
||||
double pos[2];
|
||||
double vel[2];
|
||||
double planet_mass;
|
||||
};
|
||||
void init_state ( Orbit& orbit ) {
|
||||
orbit.pos[0] = 0.0;
|
||||
orbit.pos[1] = 6578000.0;
|
||||
orbit.vel[0] = 7905.0;
|
||||
orbit.vel[1] = 0.0;
|
||||
orbit.planet_mass = EARTH_MASS;
|
||||
}
|
||||
void print_header() {
|
||||
printf ("time, orbit.pos[0], orbit.pos[1], orbit.vel[0], orbit.vel[1]\n");
|
||||
}
|
||||
void print_state( double t, Orbit& orbit ) {
|
||||
printf ("%10.10f, %10.10f, %10.10f, %10.10f, %10.10f\n",
|
||||
t, orbit.pos[0], orbit.pos[1], orbit.vel[0], orbit.vel[1]);
|
||||
}
|
||||
void G( double t, double x[], double g_out[], void* udata) {
|
||||
Orbit* orbit = (Orbit*)udata;
|
||||
double d = sqrt( x[0]*x[0] + x[1]*x[1]);
|
||||
g_out[0] = -x[0] * GRAVITATIONAL_CONSTANT * orbit->planet_mass / (d*d*d);
|
||||
g_out[1] = -x[1] * GRAVITATIONAL_CONSTANT * orbit->planet_mass / (d*d*d);
|
||||
}
|
||||
void F( double t, double v[], double f_out[], void* udata) {
|
||||
f_out[0] = v[0];
|
||||
f_out[1] = v[1];
|
||||
}
|
||||
int main ( int argc, char* argv[]) {
|
||||
Orbit orbit;
|
||||
double* x_p[2] = { &orbit.pos[0], &orbit.pos[1] };
|
||||
double* v_p[2] = { &orbit.vel[0], &orbit.vel[1] };
|
||||
unsigned count = 0;
|
||||
double dt = 0.1;
|
||||
double time = count * dt;
|
||||
init_state(orbit);
|
||||
print_header();
|
||||
print_state(time, orbit);
|
||||
SA::EulerCromerIntegrator integ(dt, 2, x_p, v_p, G, F, &orbit);
|
||||
while (time < 6000) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
time = count * dt;
|
||||
print_state(time, orbit);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,17 @@
|
||||
# Orbit
|
||||
|
||||
This program uses the *SemiImplicitEuler* integrator to simulate the orbit of an Earth satellite.
|
||||
|
||||
Generate the results as follows:
|
||||
'''
|
||||
$ make
|
||||
$ ./Orbit > orbit.csv
|
||||
'''
|
||||
|
||||
Plot the results as follows:
|
||||
|
||||
'''
|
||||
$ python plot_position.py
|
||||
'''
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 414 KiB |
@ -0,0 +1,20 @@
|
||||
|
||||
RM = rm -rf
|
||||
CC = cc
|
||||
CPP = c++
|
||||
|
||||
CXXFLAGS = -g -Wall
|
||||
INCLUDE_DIRS = -I../../include
|
||||
LIBDIR = ../../lib
|
||||
|
||||
all: Orbit
|
||||
|
||||
Orbit: Orbit.cpp
|
||||
$(CPP) $(CXXFLAGS) Orbit.cpp ${INCLUDE_DIRS} -L${LIBDIR} -lSAInteg -o Orbit
|
||||
|
||||
clean:
|
||||
${RM} Orbit.dSYM
|
||||
|
||||
spotless: clean
|
||||
${RM} Orbit
|
||||
${RM} orbit.csv
|
||||
18
trick_source/trick_utils/SAIntegrator/examples/Orbit/plot_position.py
Executable file
18
trick_source/trick_utils/SAIntegrator/examples/Orbit/plot_position.py
Executable file
@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
data = np.genfromtxt('orbit.csv',
|
||||
delimiter=',',
|
||||
skip_header=1,
|
||||
skip_footer=1,
|
||||
names=['t', 'posx','posy','velx','vely'],
|
||||
dtype=(float, float, float, float, float)
|
||||
)
|
||||
|
||||
curve1 = plt.plot(data['posx'], data['posy'], 'C1-')
|
||||
plt.title('Orbit')
|
||||
plt.xlabel('position')
|
||||
plt.ylabel('position')
|
||||
plt.grid(True)
|
||||
plt.show()
|
||||
15
trick_source/trick_utils/SAIntegrator/examples/makefile
Normal file
15
trick_source/trick_utils/SAIntegrator/examples/makefile
Normal file
@ -0,0 +1,15 @@
|
||||
|
||||
examples:
|
||||
@make -C CannonBall
|
||||
@make -C MassSpringDamper
|
||||
@make -C Orbit
|
||||
|
||||
clean:
|
||||
@make -C CannonBall clean
|
||||
@make -C MassSpringDamper clean
|
||||
@make -C Orbit clean
|
||||
|
||||
spotless:
|
||||
@make -C CannonBall spotless
|
||||
@make -C MassSpringDamper spotless
|
||||
@make -C Orbit spotless
|
||||
177
trick_source/trick_utils/SAIntegrator/include/SAIntegrator.hh
Normal file
177
trick_source/trick_utils/SAIntegrator/include/SAIntegrator.hh
Normal file
@ -0,0 +1,177 @@
|
||||
|
||||
namespace SA {
|
||||
|
||||
class Integrator {
|
||||
protected:
|
||||
double time;
|
||||
double default_dt;
|
||||
void * udata;
|
||||
public:
|
||||
Integrator(double dt, void* user_data): time(0.0), default_dt(dt), udata(user_data) {};
|
||||
virtual ~Integrator() {}
|
||||
virtual void step() { time += default_dt; }; // Integrate over implicit (default) time step (default: default_dt)
|
||||
virtual void load() = 0;
|
||||
virtual void unload() = 0;
|
||||
double getTime() {return time;}
|
||||
};
|
||||
|
||||
typedef void (*derivsFunc)( double t, double state[], double derivs[], void* user_data);
|
||||
|
||||
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 last_dt; // What was the last step-size? For undo_step().
|
||||
|
||||
public:
|
||||
FirstOrderODEIntegrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data);
|
||||
~FirstOrderODEIntegrator();
|
||||
// void init(double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func);
|
||||
virtual void undo_step();
|
||||
void load();
|
||||
void unload();
|
||||
};
|
||||
|
||||
class FirstOrderODEVariableStepIntegrator : public FirstOrderODEIntegrator {
|
||||
|
||||
public:
|
||||
FirstOrderODEVariableStepIntegrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data);
|
||||
using Integrator::step;
|
||||
virtual void step( double dt );
|
||||
};
|
||||
|
||||
class EulerIntegrator : public FirstOrderODEVariableStepIntegrator {
|
||||
public:
|
||||
double *derivs;
|
||||
EulerIntegrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~EulerIntegrator();
|
||||
void step( double dt);
|
||||
void step();
|
||||
};
|
||||
|
||||
class HeunsMethod : public FirstOrderODEVariableStepIntegrator {
|
||||
public:
|
||||
double *wstate;
|
||||
double *derivs[2];
|
||||
|
||||
HeunsMethod( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~HeunsMethod();
|
||||
void step( double dt);
|
||||
void step();
|
||||
};
|
||||
|
||||
class RK2Integrator : public FirstOrderODEVariableStepIntegrator {
|
||||
public:
|
||||
double *wstate;
|
||||
double *derivs[2];
|
||||
|
||||
RK2Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~RK2Integrator();
|
||||
void step( double dt);
|
||||
void step();
|
||||
};
|
||||
|
||||
class RK4Integrator : public FirstOrderODEVariableStepIntegrator {
|
||||
public:
|
||||
double *wstate[3];
|
||||
double *derivs[4];
|
||||
|
||||
RK4Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~RK4Integrator();
|
||||
void step( double dt);
|
||||
void step();
|
||||
|
||||
};
|
||||
|
||||
class RK3_8Integrator : public FirstOrderODEVariableStepIntegrator {
|
||||
public:
|
||||
double *wstate[4];
|
||||
double *derivs[4];
|
||||
|
||||
RK3_8Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~RK3_8Integrator();
|
||||
void step( double dt);
|
||||
void step();
|
||||
|
||||
};
|
||||
|
||||
class EulerCromerIntegrator : public Integrator {
|
||||
protected:
|
||||
unsigned int state_size;
|
||||
double** x_p;
|
||||
double** v_p;
|
||||
double* x_in;
|
||||
double* v_in;
|
||||
double* x_out;
|
||||
double* v_out;
|
||||
double* g_out;
|
||||
double* f_out;
|
||||
derivsFunc gderivs;
|
||||
derivsFunc fderivs;
|
||||
|
||||
public:
|
||||
EulerCromerIntegrator(double dt, int N, double* xp[], double* vp[], derivsFunc g, derivsFunc f , void* user_data);
|
||||
~EulerCromerIntegrator();
|
||||
void step( double dt);
|
||||
void step();
|
||||
void load();
|
||||
void unload();
|
||||
};
|
||||
|
||||
// AdamsBashforthMoulton 4
|
||||
// class ABMIntegrator : public FirstOrderODEIntegrator {
|
||||
// protected:
|
||||
// double ** deriv_history;
|
||||
// double * composite_deriv;
|
||||
// unsigned int bufferSize;
|
||||
// unsigned int algorithmHistorySize;
|
||||
// unsigned int hix;
|
||||
// unsigned int currentHistorySize;
|
||||
//
|
||||
// public:
|
||||
// ABMIntegrator(int history_size, double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
// ~ABMIntegrator();
|
||||
// void step();
|
||||
// void undo_step();
|
||||
// };
|
||||
|
||||
// AdamsBashforthMoulton 2
|
||||
class ABM2Integrator : public FirstOrderODEIntegrator {
|
||||
protected:
|
||||
double ** deriv_history;
|
||||
double * composite_deriv;
|
||||
unsigned int bufferSize;
|
||||
unsigned int hix;
|
||||
unsigned int currentHistorySize;
|
||||
FirstOrderODEVariableStepIntegrator* priming_integrator;
|
||||
|
||||
public:
|
||||
ABM2Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~ABM2Integrator();
|
||||
void step();
|
||||
void undo_step();
|
||||
};
|
||||
|
||||
// AdamsBashforthMoulton 4
|
||||
class ABM4Integrator : public FirstOrderODEIntegrator {
|
||||
protected:
|
||||
double ** deriv_history;
|
||||
double * composite_deriv;
|
||||
unsigned int bufferSize;
|
||||
unsigned int hix;
|
||||
unsigned int currentHistorySize;
|
||||
FirstOrderODEVariableStepIntegrator* priming_integrator;
|
||||
|
||||
public:
|
||||
ABM4Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc derivs_func, void* user_data);
|
||||
~ABM4Integrator();
|
||||
void step();
|
||||
void undo_step();
|
||||
};
|
||||
}
|
||||
44
trick_source/trick_utils/SAIntegrator/makefile
Normal file
44
trick_source/trick_utils/SAIntegrator/makefile
Normal file
@ -0,0 +1,44 @@
|
||||
|
||||
RM = rm -rf
|
||||
CC = cc
|
||||
CPP = c++
|
||||
|
||||
CFLAGS = -g -Wall
|
||||
INCLUDE_DIRS = -Iinclude
|
||||
OBJDIR = obj
|
||||
LIBDIR = lib
|
||||
LIBNAME = libSAInteg.a
|
||||
LIBOBJS = ${OBJDIR}/SAIntegrator.o
|
||||
|
||||
all: test examples
|
||||
|
||||
test: ${LIBDIR}/${LIBNAME}
|
||||
${MAKE} -C test
|
||||
|
||||
examples: ${LIBDIR}/${LIBNAME}
|
||||
${MAKE} -C examples
|
||||
|
||||
$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
|
||||
$(CPP) $(CFLAGS) ${INCLUDE_DIRS} -c $< -o $@
|
||||
|
||||
${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
|
||||
ar crs $@ ${LIBOBJS}
|
||||
|
||||
${OBJDIR}:
|
||||
mkdir -p ${OBJDIR}
|
||||
|
||||
${LIBDIR}:
|
||||
mkdir -p ${LIBDIR}
|
||||
|
||||
clean:
|
||||
${RM} *~
|
||||
${RM} ${OBJDIR}
|
||||
${MAKE} -C test clean
|
||||
${MAKE} -C examples clean
|
||||
|
||||
spotless:
|
||||
${RM} *~
|
||||
${RM} ${OBJDIR}
|
||||
${RM} ${LIBDIR}
|
||||
${MAKE} -C test spotless
|
||||
${MAKE} -C examples spotless
|
||||
511
trick_source/trick_utils/SAIntegrator/src/SAIntegrator.cpp
Normal file
511
trick_source/trick_utils/SAIntegrator/src/SAIntegrator.cpp
Normal file
@ -0,0 +1,511 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <iostream>
|
||||
#include "SAIntegrator.hh"
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::FirstOrderODEIntegrator::FirstOrderODEIntegrator(double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: Integrator(dt, user_data) {
|
||||
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;
|
||||
}
|
||||
SA::FirstOrderODEIntegrator::~FirstOrderODEIntegrator() {
|
||||
delete istate;
|
||||
delete ostate;
|
||||
}
|
||||
void SA::FirstOrderODEIntegrator::load() {
|
||||
reset = false;
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
istate[i] = *(orig_vars[i]);
|
||||
}
|
||||
}
|
||||
void SA::FirstOrderODEIntegrator::unload() {
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
*(dest_vars[i]) = ostate[i];
|
||||
}
|
||||
}
|
||||
void SA::FirstOrderODEIntegrator::undo_step() {
|
||||
if (!reset) { // If we've not already reset the integrator, then reset it.
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
*(orig_vars[i]) = istate[i]; // Copy istate values back to the state variables from whence they came.
|
||||
}
|
||||
time -= last_dt; // Undo the last time-step.
|
||||
}
|
||||
reset = true;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::FirstOrderODEVariableStepIntegrator::FirstOrderODEVariableStepIntegrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEIntegrator(dt, N, in_vars, out_vars, func, user_data) {}
|
||||
|
||||
void SA::FirstOrderODEVariableStepIntegrator::step( double dt ) {
|
||||
last_dt = dt; time += dt;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::EulerIntegrator::EulerIntegrator(double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEVariableStepIntegrator(dt, N, in_vars, out_vars, func, user_data)
|
||||
{
|
||||
derivs = new double[N];
|
||||
}
|
||||
SA::EulerIntegrator::~EulerIntegrator() {
|
||||
delete derivs;
|
||||
}
|
||||
void SA::EulerIntegrator::step( double dt) {
|
||||
(*derivs_func)( time, istate, derivs, udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
ostate[i] = istate[i] + derivs[i] * dt;
|
||||
}
|
||||
SA::FirstOrderODEVariableStepIntegrator::step(dt);
|
||||
}
|
||||
void SA::EulerIntegrator::step() {
|
||||
step(default_dt);
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::HeunsMethod::HeunsMethod( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEVariableStepIntegrator(dt, N, in_vars, out_vars, func, user_data)
|
||||
{
|
||||
wstate = new double[N];
|
||||
for (unsigned int i = 0; i < 2 ; i++) {
|
||||
derivs[i] = new double[N];
|
||||
}
|
||||
}
|
||||
SA::HeunsMethod::~HeunsMethod() {
|
||||
delete wstate;
|
||||
for (unsigned int i = 0; i < 2 ; i++) {
|
||||
delete derivs[i];
|
||||
}
|
||||
}
|
||||
void SA::HeunsMethod::step( double dt) {
|
||||
(*derivs_func)( time, istate, derivs[0], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[i] = istate[i] + dt * derivs[0][i];
|
||||
}
|
||||
(*derivs_func)( time, wstate, derivs[1], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
ostate[i] = istate[i] + (dt/2) * ( derivs[0][i] + derivs[1][i] );
|
||||
}
|
||||
SA::FirstOrderODEVariableStepIntegrator::step(dt);
|
||||
}
|
||||
void SA::HeunsMethod::step() {
|
||||
step(default_dt);
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::RK2Integrator::RK2Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEVariableStepIntegrator(dt, N, in_vars, out_vars, func, user_data)
|
||||
{
|
||||
wstate = new double[N];
|
||||
for (unsigned int i = 0; i < 2 ; i++) {
|
||||
derivs[i] = new double[N];
|
||||
}
|
||||
}
|
||||
SA::RK2Integrator::~RK2Integrator() {
|
||||
delete (wstate);
|
||||
for (unsigned int i = 0; i < 2 ; i++) {
|
||||
delete derivs[i];
|
||||
}
|
||||
}
|
||||
void SA::RK2Integrator::step( double dt) {
|
||||
(*derivs_func)( time, istate, derivs[0], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[i] = istate[i] + 0.5 * dt * derivs[0][i];
|
||||
}
|
||||
(*derivs_func)( time + 0.5 * dt , wstate, derivs[1], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
ostate[i] = istate[i] + dt * derivs[1][i];
|
||||
}
|
||||
SA::FirstOrderODEVariableStepIntegrator::step(dt);
|
||||
}
|
||||
void SA::RK2Integrator::step() {
|
||||
step(default_dt);
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::RK4Integrator::RK4Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEVariableStepIntegrator(dt, N, in_vars, out_vars, func, user_data)
|
||||
{
|
||||
for (unsigned int i = 0; i < 3 ; i++) {
|
||||
wstate[i] = new double[N];
|
||||
}
|
||||
for (unsigned int i = 0; i < 4 ; i++) {
|
||||
derivs[i] = new double[N];
|
||||
}
|
||||
}
|
||||
SA::RK4Integrator::~RK4Integrator() {
|
||||
for (unsigned int i = 0; i < 3 ; i++) {
|
||||
delete (wstate[i]);
|
||||
}
|
||||
for (unsigned int i = 0; i < 4 ; i++) {
|
||||
delete (derivs[i]);
|
||||
}
|
||||
}
|
||||
void SA::RK4Integrator::step( double dt) {
|
||||
(*derivs_func)( time, istate, derivs[0], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[0][i] = istate[i] + 0.5 * derivs[0][i] * dt;
|
||||
}
|
||||
(*derivs_func)( time + 0.5 * dt , wstate[0], derivs[1], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[1][i] = istate[i] + 0.5 * derivs[1][i] * dt;
|
||||
}
|
||||
(*derivs_func)( time + 0.5 * dt , wstate[1], derivs[2], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[2][i] = istate[i] + derivs[2][i] * dt;
|
||||
}
|
||||
(*derivs_func)( time + dt , wstate[2], derivs[3], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
ostate[i] = istate[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]) * dt;
|
||||
}
|
||||
SA::FirstOrderODEVariableStepIntegrator::step(dt);
|
||||
}
|
||||
void SA::RK4Integrator::step() {
|
||||
step(default_dt);
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::RK3_8Integrator::RK3_8Integrator( double dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEVariableStepIntegrator(dt, N, in_vars, out_vars ,func, user_data)
|
||||
{
|
||||
for (unsigned int i = 0; i < 3 ; i++) {
|
||||
wstate[i] = new double[N];
|
||||
}
|
||||
for (unsigned int i = 0; i < 4 ; i++) {
|
||||
derivs[i] = new double[N];
|
||||
}
|
||||
}
|
||||
SA::RK3_8Integrator::~RK3_8Integrator() {
|
||||
for (unsigned int i = 0; i < 3 ; i++) {
|
||||
delete (wstate[i]);
|
||||
}
|
||||
for (unsigned int i = 0; i < 4 ; i++) {
|
||||
delete (derivs[i]);
|
||||
}
|
||||
}
|
||||
void SA::RK3_8Integrator::step( double dt) {
|
||||
(*derivs_func)( time, istate, derivs[0], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[0][i] = istate[i] + dt * (1/3.0) * derivs[0][i];
|
||||
}
|
||||
(*derivs_func)( time + (1/3.0) * dt , wstate[0], derivs[1], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[1][i] = istate[i] + dt * ((-1/3.0) * derivs[0][i] + derivs[1][i]);
|
||||
}
|
||||
(*derivs_func)( time + (2/3.0) * dt , wstate[1], derivs[2], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
wstate[2][i] = istate[i] + dt * (derivs[0][i] - derivs[1][i] + derivs[2][i]);
|
||||
}
|
||||
(*derivs_func)( time + dt , wstate[2], derivs[3], udata);
|
||||
for (unsigned int i=0; i<state_size; i++) {
|
||||
ostate[i] = istate[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]) * dt;
|
||||
}
|
||||
SA::FirstOrderODEVariableStepIntegrator::step(dt);
|
||||
}
|
||||
void SA::RK3_8Integrator::step() {
|
||||
step(default_dt);
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------
|
||||
|
||||
SA::EulerCromerIntegrator::EulerCromerIntegrator(double dt, int N, double* xp[], double* vp[], derivsFunc gfunc, derivsFunc ffunc, void* user_data)
|
||||
: Integrator(dt, user_data) {
|
||||
state_size = N;
|
||||
x_p = new double*[N];
|
||||
v_p = new double*[N];
|
||||
x_in = new double[N];
|
||||
v_in = new double[N];
|
||||
g_out = new double[N];
|
||||
f_out = new double[N];
|
||||
x_out = new double[N];
|
||||
v_out = new double[N];
|
||||
for (int i=0 ; i<N; i++ ) {
|
||||
x_p[i] = xp[i];
|
||||
v_p[i] = vp[i];
|
||||
}
|
||||
gderivs = gfunc;
|
||||
fderivs = ffunc;
|
||||
}
|
||||
SA::EulerCromerIntegrator::~EulerCromerIntegrator() {
|
||||
delete x_p;
|
||||
delete v_p;
|
||||
delete x_in;
|
||||
delete v_in;
|
||||
delete g_out;
|
||||
delete f_out;
|
||||
delete x_out;
|
||||
delete v_out;
|
||||
}
|
||||
void SA::EulerCromerIntegrator::step( double dt) {
|
||||
(*gderivs)( time, x_in, g_out, udata);
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
v_out[i] = v_in[i] + g_out[i] * dt;
|
||||
}
|
||||
(*fderivs)( time, v_in, f_out, udata);
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
x_out[i] = x_in[i] + f_out[i] * dt;
|
||||
}
|
||||
time += dt;
|
||||
}
|
||||
void SA::EulerCromerIntegrator::step() {
|
||||
step(default_dt);
|
||||
}
|
||||
void SA::EulerCromerIntegrator::load() {
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
x_in[i] = *(x_p[i]);
|
||||
v_in[i] = *(v_p[i]);
|
||||
}
|
||||
}
|
||||
void SA::EulerCromerIntegrator::unload(){
|
||||
for (int i=0 ; i<state_size; i++ ) {
|
||||
*(x_p[i]) = x_out[i];
|
||||
*(v_p[i]) = v_out[i];
|
||||
}
|
||||
}
|
||||
|
||||
// // Adams-Bashforth Coefficients
|
||||
// static const double ABCoeffs[5][5] = {
|
||||
// {1.0, 0.0, 0.0, 0.0, 0.0},
|
||||
// {(3/2.0), (-1/2.0), 0.0, 0.0, 0.0},
|
||||
// {(23/12.0), (-16/12.0), (5/12.0), 0.0, 0.0},
|
||||
// {(55/24.0), (-59/24.0), (37/24.0), (-9/24.0), 0.0},
|
||||
// {(1901/720.0), (-2774/720.0), (2616/720.0), (-1274/720.0), (251/720.0)}
|
||||
// };
|
||||
//
|
||||
// // Adams-Moulton Coefficients
|
||||
// static const double AMCoeffs[5][5] = {
|
||||
// {1.0, 0.0, 0.0, 0.0, 0.0},
|
||||
// {(1/2.0), (1/2.0), 0.0, 0.0, 0.0},
|
||||
// {(5/12.0), (8/12.0), (-1/12.0), 0.0, 0.0},
|
||||
// {(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 dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
// : FirstOrderODEIntegrator(dt, N, in_vars, out_vars ,func, user_data) {
|
||||
//
|
||||
// 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.
|
||||
// hix=0;
|
||||
// currentHistorySize=0; // How much derivative history is currently stored int the history buffer. Initially there will be none until we've primed the integrator.
|
||||
// deriv_history = new double*[bufferSize];
|
||||
// for (unsigned int i=0; i<bufferSize ; i++) {
|
||||
// deriv_history[i] = new double[state_size];
|
||||
// }
|
||||
// composite_deriv = new double[state_size];
|
||||
// }
|
||||
// SA::ABMIntegrator::~ABMIntegrator() {
|
||||
// for (int i=0; i<bufferSize ; i++) {
|
||||
// delete (deriv_history[i]);
|
||||
// }
|
||||
// delete(deriv_history);
|
||||
// delete(composite_deriv);
|
||||
// }
|
||||
// void SA::ABMIntegrator::step() {
|
||||
//
|
||||
// hix = (hix+1)%bufferSize; // Move history index forward
|
||||
// (*derivs_func)( time, istate, deriv_history[hix], udata); // 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.
|
||||
// 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] += ABCoeffs[currentHistorySize-1][n] * deriv_history[j][i];
|
||||
// }
|
||||
// ostate[i] = istate[i] + default_dt * 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)( time, ostate, deriv_history[hix], udata); // 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_dt * 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() {
|
||||
// hix = (hix+bufferSize-1)%bufferSize;
|
||||
// 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 dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEIntegrator(dt, N, in_vars, out_vars ,func, user_data) {
|
||||
// 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;
|
||||
currentHistorySize=0; // How much derivative history is currently stored int the history buffer. Initially there will be none until we've primed the integrator.
|
||||
deriv_history = new double*[bufferSize];
|
||||
for (unsigned int i=0; i<bufferSize ; i++) {
|
||||
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]);
|
||||
}
|
||||
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 = new SA::RK2Integrator(dt, N, priming_integrator_in_vars, priming_integrator_out_vars, func, user_data);
|
||||
}
|
||||
SA::ABM2Integrator::~ABM2Integrator() {
|
||||
for (int i=0; i<bufferSize ; i++) {
|
||||
delete (deriv_history[i]);
|
||||
}
|
||||
delete(deriv_history);
|
||||
delete(composite_deriv);
|
||||
delete priming_integrator;
|
||||
}
|
||||
void SA::ABM2Integrator::step() {
|
||||
|
||||
hix = (hix+1)%bufferSize; // Move history index forward
|
||||
(*derivs_func)( time, istate, deriv_history[hix], udata); // 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;
|
||||
|
||||
if ( currentHistorySize < 2 ) {
|
||||
priming_integrator->load();
|
||||
priming_integrator->step();
|
||||
priming_integrator->unload();
|
||||
} else {
|
||||
// (Predictor) Predict the next state using the Adams-Bashforth explicit method.
|
||||
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] += AB2Coeffs[n] * deriv_history[j][i];
|
||||
}
|
||||
ostate[i] = istate[i] + default_dt * 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)( time, ostate, deriv_history[hix], udata); // 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_dt * 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() {
|
||||
hix = (hix+bufferSize-1)%bufferSize;
|
||||
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 dt, int N, double* in_vars[], double* out_vars[], derivsFunc func, void* user_data)
|
||||
: FirstOrderODEIntegrator(dt, N, in_vars, out_vars ,func, user_data) {
|
||||
|
||||
// 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.
|
||||
hix=0;
|
||||
currentHistorySize=0; // How much derivative history is currently stored int the history buffer. Initially there will be none until we've primed the integrator.
|
||||
deriv_history = new double*[bufferSize];
|
||||
for (unsigned int i=0; i<bufferSize ; i++) {
|
||||
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]);
|
||||
}
|
||||
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 = new SA::RK4Integrator(dt, N, priming_integrator_in_vars, priming_integrator_out_vars, func, user_data);
|
||||
}
|
||||
|
||||
SA::ABM4Integrator::~ABM4Integrator() {
|
||||
for (int i=0; i<bufferSize ; i++) {
|
||||
delete (deriv_history[i]);
|
||||
}
|
||||
delete(deriv_history);
|
||||
delete(composite_deriv);
|
||||
delete priming_integrator;
|
||||
}
|
||||
|
||||
void SA::ABM4Integrator::step() {
|
||||
|
||||
hix = (hix+1)%bufferSize; // Move history index forward
|
||||
(*derivs_func)( time, istate, deriv_history[hix], udata); // 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 ) {
|
||||
priming_integrator->load();
|
||||
priming_integrator->step();
|
||||
priming_integrator->unload();
|
||||
} else {
|
||||
// (Predictor) Predict the next state using the Adams-Bashforth explicit method.
|
||||
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] += AB4Coeffs[n] * deriv_history[j][i];
|
||||
}
|
||||
ostate[i] = istate[i] + default_dt * 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)( time, ostate, deriv_history[hix], udata); // 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_dt * 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() {
|
||||
hix = (hix+bufferSize-1)%bufferSize;
|
||||
FirstOrderODEIntegrator::undo_step();
|
||||
}
|
||||
116
trick_source/trick_utils/SAIntegrator/test/Integ_unittest.cc
Normal file
116
trick_source/trick_utils/SAIntegrator/test/Integ_unittest.cc
Normal file
@ -0,0 +1,116 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <iostream>
|
||||
#include "SAIntegrator.hh"
|
||||
#include <math.h>
|
||||
|
||||
void deriv4( double t, double state[], double derivs[], void* udata) {
|
||||
double t2 = t*t;
|
||||
double t3 = t2*t;
|
||||
|
||||
derivs[0] = 3.0*t3 - 3.0*t2 - 3.0*t + 4.0;
|
||||
derivs[1] = 2.0*t3 + 1.0*t2 - 5.0*t + 7.0;
|
||||
derivs[2] = t3 + t2 + 5.0*t + 4.0;
|
||||
derivs[3] = t3 - 6.0*t2 + 4.0*t + 12.0;
|
||||
}
|
||||
|
||||
void deriv2( double t, double state[], double derivs[], void* udata) {
|
||||
|
||||
derivs[0] = -2.0*t + 3.0;
|
||||
derivs[1] = 5.0*t + 4.0;
|
||||
derivs[2] = 3.0*t - 3.0;
|
||||
derivs[3] = 5.0*t + 4.0;
|
||||
}
|
||||
|
||||
void deriv1( double t, double state[], double derivs[], void* udata) {
|
||||
|
||||
derivs[0] = 4.0;
|
||||
derivs[1] = -4.0;
|
||||
derivs[2] = M_PI;
|
||||
derivs[3] = -M_PI;
|
||||
}
|
||||
|
||||
#define EXCEPTABLE_ERROR 0.00000000001
|
||||
|
||||
TEST(Integ_unittest, EulerIntegrator_1) {
|
||||
|
||||
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 dt = 0.01;
|
||||
unsigned int count = 0;
|
||||
double t = count * dt;
|
||||
SA::EulerIntegrator integ(dt, 4, state_var_p, state_var_p, deriv1, NULL);
|
||||
while (t < 2.0) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
t = count * dt;
|
||||
}
|
||||
EXPECT_NEAR(state[0], 8.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[1], -8.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[2], (2*M_PI), EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[3], (-2*M_PI), EXCEPTABLE_ERROR);
|
||||
}
|
||||
|
||||
TEST(Integ_unittest, RungeKutta2_1) {
|
||||
|
||||
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 dt = 0.01;
|
||||
unsigned int count = 0;
|
||||
double t = count * dt;
|
||||
SA::RK2Integrator integ(dt, 4, state_var_p, state_var_p, deriv2, NULL);
|
||||
while (t < 2.0) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
t = count * dt;
|
||||
}
|
||||
EXPECT_NEAR(state[0], 2.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[1], 18.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[2], 0.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[3], 18.0, EXCEPTABLE_ERROR);
|
||||
}
|
||||
|
||||
TEST(Integ_unittest, RungeKutta4_1) {
|
||||
|
||||
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 dt = 0.01;
|
||||
unsigned int count = 0;
|
||||
double t = count * dt;
|
||||
SA::RK4Integrator integ(dt, 4, state_var_p, state_var_p, deriv4, NULL);
|
||||
while (t < 2.0) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
t = count * dt;
|
||||
}
|
||||
EXPECT_NEAR(state[0], 6.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[1], (14.0+2/3.0), EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[2], (24+2/3.0), EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[3], 20.0, EXCEPTABLE_ERROR);
|
||||
}
|
||||
|
||||
TEST(Integ_unittest, RungeKutta38_1) {
|
||||
|
||||
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 dt = 0.01;
|
||||
unsigned int count = 0;
|
||||
double t = count * dt;
|
||||
SA::RK3_8Integrator integ(dt, 4, state_var_p, state_var_p, deriv4, NULL);
|
||||
while (t < 2.0) {
|
||||
integ.load();
|
||||
integ.step();
|
||||
integ.unload();
|
||||
count ++;
|
||||
t = count * dt;
|
||||
}
|
||||
EXPECT_NEAR(state[0], 6.0, EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[1], (14.0+2/3.0), EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[2], (24+2/3.0), EXCEPTABLE_ERROR);
|
||||
EXPECT_NEAR(state[3], 20.0, EXCEPTABLE_ERROR);
|
||||
}
|
||||
28
trick_source/trick_utils/SAIntegrator/test/makefile
Normal file
28
trick_source/trick_utils/SAIntegrator/test/makefile
Normal file
@ -0,0 +1,28 @@
|
||||
|
||||
RM = rm -rf
|
||||
CC = cc
|
||||
CPP = c++
|
||||
|
||||
CFLAGS = -g -Wall
|
||||
INCLUDE_DIRS = -I../include
|
||||
OBJDIR = obj
|
||||
LIBDIR = ../lib
|
||||
LIBNAME = libSAInteg.a
|
||||
LIBOBJS = ${OBJDIR}/Integrator.o
|
||||
|
||||
all: Integ_unittest
|
||||
|
||||
Integ_unittest.o : Integ_unittest.cc
|
||||
$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -c $<
|
||||
|
||||
Integ_unittest : ${LIBDIR}/${LIBNAME} Integ_unittest.o
|
||||
$(CPP) $(CFLAGS) -o $@ $^ -lgtest -lgtest_main -lpthread
|
||||
|
||||
${LIBDIR}/${LIBNAME} :
|
||||
$(MAKE) -C ..
|
||||
|
||||
clean:
|
||||
${RM} *.o
|
||||
|
||||
spotless: clean
|
||||
${RM} Integ_unittest
|
||||
Loading…
x
Reference in New Issue
Block a user