You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
42 lines
978 B
42 lines
978 B
#pragma once |
|
|
|
#include "math.hpp" |
|
|
|
namespace matrix { |
|
|
|
template<typename Type, size_t M, size_t N> |
|
int integrate_rk4( |
|
Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> & u), |
|
const Matrix<Type, M, 1> & y0, |
|
const Matrix<Type, N, 1> & u, |
|
Type t0, |
|
Type tf, |
|
Type h0, |
|
Matrix<Type, M, 1> & y1 |
|
) |
|
{ |
|
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods |
|
Type t1 = t0; |
|
y1 = y0; |
|
Type h = h0; |
|
Vector<Type, M> k1, k2, k3, k4; |
|
if (tf < t0) return -1; // make sure t1 > t0 |
|
while (t1 < tf) { |
|
if (t1 + h0 < tf) { |
|
h = h0; |
|
} else { |
|
h = tf - t1; |
|
} |
|
k1 = f(t1, y1, u); |
|
k2 = f(t1 + h/2, y1 + k1*h/2, u); |
|
k3 = f(t1 + h/2, y1 + k2*h/2, u); |
|
k4 = f(t1 + h, y1 + k3*h, u); |
|
y1 += (k1 + k2*2 + k3*2 + k4)*(h/6); |
|
t1 += h; |
|
} |
|
return 0; |
|
} |
|
|
|
} // namespace matrix |
|
|
|
// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :
|
|
|