root / scout / libscout / src / helper_classes / RungaKutta.cpp @ 34a60a3b
History | View | Annotate | Download (1.22 KB)
1 |
#include "RungaKutta.h" |
---|---|
2 |
#include <math.h> |
3 |
|
4 |
using namespace std; |
5 |
|
6 |
vector<double> RungaKutta::integrate(vector<double> a, vector<double> b, double t) |
7 |
{ |
8 |
//Copy over the a vector.
|
9 |
vector<double> result(a);
|
10 |
//Add half a timestep to the starting conditions.
|
11 |
for(unsigned int i=0; i<a.size(); i++) |
12 |
{ |
13 |
result[i] += t*b[i]; |
14 |
} |
15 |
//Return the integrated result.
|
16 |
return result;
|
17 |
} |
18 |
|
19 |
vector<double> RungaKutta::diff_drive_robot(vector<double> x) |
20 |
{ |
21 |
vector<double> dx(5,0); |
22 |
dx[0] = x[3] * cos(x[2]); |
23 |
dx[1] = x[3] * sin(x[2]); |
24 |
dx[2] = x[4]; |
25 |
dx[3] = x[3]; |
26 |
dx[4] = x[4]; |
27 |
return dx;
|
28 |
} |
29 |
|
30 |
vector<double> RungaKutta::rk4(vector<double> start, |
31 |
vector<double> (*func)(vector<double>), |
32 |
double loop_time)
|
33 |
{ |
34 |
vector<double> k0 = func(start);
|
35 |
vector<double> temp_k0 = integrate(start, k0, loop_time/2); |
36 |
vector<double> k1 = func(temp_k0);
|
37 |
vector<double> temp_k1 = integrate(start, k1, loop_time/2); |
38 |
vector<double> k2 = func(temp_k1);
|
39 |
vector<double> temp_k2 = integrate(start, k2, loop_time);
|
40 |
vector<double> k3 = func(temp_k2);
|
41 |
|
42 |
// Average out each loop's result.
|
43 |
vector<double> result(start);
|
44 |
for(unsigned int i=0; i<result.size(); i++) |
45 |
{ |
46 |
result[i] = loop_time/6*(k0[i]+2*k1[i]+2*k2[i]+k3[i]); |
47 |
} |
48 |
return result;
|
49 |
} |