1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
| #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 :
|
|
