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 
} 