Statistics
| Branch: | Revision:

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
}