|
@@ -2,71 +2,87 @@ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
namespace rack { |
|
|
namespace rack { |
|
|
|
|
|
namespace ode { |
|
|
|
|
|
|
|
|
/** The callback function `f` in each of these stepping functions must have the signature |
|
|
/** The callback function `f` in each of these stepping functions must have the signature |
|
|
void f(float x, const float y[], float dydt[]) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void f(float t, const float x[], float dxdt[]) |
|
|
|
|
|
|
|
|
A capturing lambda is ideal for this. |
|
|
A capturing lambda is ideal for this. |
|
|
|
|
|
For example, |
|
|
|
|
|
|
|
|
|
|
|
float x[2] = {1.f, 0.f}; |
|
|
|
|
|
float dt = 0.01f; |
|
|
|
|
|
for (float t = 0.f; t < 1.f; t += dt) { |
|
|
|
|
|
rack::ode::stepRK4(t, dt, x, 2, [&](float t, const float x[], float dxdt[]) { |
|
|
|
|
|
dxdt[0] = x[1]; |
|
|
|
|
|
dxdt[1] = -x[0]; |
|
|
|
|
|
}); |
|
|
|
|
|
printf("%f\n", x[0]); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
*/ |
|
|
*/ |
|
|
|
|
|
|
|
|
/** Solves an ODE system using the 1st order Euler method */ |
|
|
/** Solves an ODE system using the 1st order Euler method */ |
|
|
template<typename F> |
|
|
template<typename F> |
|
|
void stepEuler(float x, float dx, float y[], int len, F f) { |
|
|
|
|
|
|
|
|
void stepEuler(float t, float dt, float x[], int len, F f) { |
|
|
float k[len]; |
|
|
float k[len]; |
|
|
|
|
|
|
|
|
f(x, y, k); |
|
|
|
|
|
|
|
|
f(t, x, k); |
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
y[i] += dx * k[i]; |
|
|
|
|
|
|
|
|
x[i] += dt * k[i]; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
/** Solves an ODE system using the 2nd order Runge-Kutta method */ |
|
|
/** Solves an ODE system using the 2nd order Runge-Kutta method */ |
|
|
template<typename F> |
|
|
template<typename F> |
|
|
void stepRK2(float x, float dx, float y[], int len, F f) { |
|
|
|
|
|
|
|
|
void stepRK2(float t, float dt, float x[], int len, F f) { |
|
|
float k1[len]; |
|
|
float k1[len]; |
|
|
float k2[len]; |
|
|
float k2[len]; |
|
|
float yi[len]; |
|
|
float yi[len]; |
|
|
|
|
|
|
|
|
f(x, y, k1); |
|
|
|
|
|
|
|
|
f(t, x, k1); |
|
|
|
|
|
|
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
yi[i] = y[i] + k1[i] * dx / 2.f; |
|
|
|
|
|
|
|
|
yi[i] = x[i] + k1[i] * dt / 2.f; |
|
|
} |
|
|
} |
|
|
f(x + dx / 2.f, yi, k2); |
|
|
|
|
|
|
|
|
f(t + dt / 2.f, yi, k2); |
|
|
|
|
|
|
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
y[i] += dx * k2[i]; |
|
|
|
|
|
|
|
|
x[i] += dt * k2[i]; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
/** Solves an ODE system using the 4th order Runge-Kutta method */ |
|
|
/** Solves an ODE system using the 4th order Runge-Kutta method */ |
|
|
template<typename F> |
|
|
template<typename F> |
|
|
void stepRK4(float x, float dx, float y[], int len, F f) { |
|
|
|
|
|
|
|
|
void stepRK4(float t, float dt, float x[], int len, F f) { |
|
|
float k1[len]; |
|
|
float k1[len]; |
|
|
float k2[len]; |
|
|
float k2[len]; |
|
|
float k3[len]; |
|
|
float k3[len]; |
|
|
float k4[len]; |
|
|
float k4[len]; |
|
|
float yi[len]; |
|
|
float yi[len]; |
|
|
|
|
|
|
|
|
f(x, y, k1); |
|
|
|
|
|
|
|
|
f(t, x, k1); |
|
|
|
|
|
|
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
yi[i] = y[i] + k1[i] * dx / 2.f; |
|
|
|
|
|
|
|
|
yi[i] = x[i] + k1[i] * dt / 2.f; |
|
|
} |
|
|
} |
|
|
f(x + dx / 2.f, yi, k2); |
|
|
|
|
|
|
|
|
f(t + dt / 2.f, yi, k2); |
|
|
|
|
|
|
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
yi[i] = y[i] + k2[i] * dx / 2.f; |
|
|
|
|
|
|
|
|
yi[i] = x[i] + k2[i] * dt / 2.f; |
|
|
} |
|
|
} |
|
|
f(x + dx / 2.f, yi, k3); |
|
|
|
|
|
|
|
|
f(t + dt / 2.f, yi, k3); |
|
|
|
|
|
|
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
yi[i] = y[i] + k3[i] * dx; |
|
|
|
|
|
|
|
|
yi[i] = x[i] + k3[i] * dt; |
|
|
} |
|
|
} |
|
|
f(x + dx, yi, k4); |
|
|
|
|
|
|
|
|
f(t + dt, yi, k4); |
|
|
|
|
|
|
|
|
for (int i = 0; i < len; i++) { |
|
|
for (int i = 0; i < len; i++) { |
|
|
y[i] += dx * (k1[i] + 2.f * k2[i] + 2.f * k3[i] + k4[i]) / 6.f; |
|
|
|
|
|
|
|
|
x[i] += dt * (k1[i] + 2.f * k2[i] + 2.f * k3[i] + k4[i]) / 6.f; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
} // namespace ode |
|
|
} // namespace rack |
|
|
} // namespace rack |