### Profile

Statistics
| Branch: | Revision:

## root / scout / libscout / src / helper_classes / RungaKutta.cpp @ 34a60a3b

 1 ```#include "RungaKutta.h" ``` ```#include ``` ```using namespace std; ``` ```vector RungaKutta::integrate(vector a, vector b, double t) ``` ```{ ``` ``` //Copy over the a vector. ``` ``` vector result(a); ``` ``` //Add half a timestep to the starting conditions. ``` ``` for(unsigned int i=0; i RungaKutta::diff_drive_robot(vector x) ``` ```{ ``` ``` vector dx(5,0); ``` ``` dx[0] = x[3] * cos(x[2]); ``` ``` dx[1] = x[3] * sin(x[2]); ``` ``` dx[2] = x[4]; ``` ``` dx[3] = x[3]; ``` ``` dx[4] = x[4]; ``` ``` return dx; ``` ```} ``` ```vector RungaKutta::rk4(vector start, ``` ``` vector (*func)(vector), ``` ``` double loop_time) ``` ```{ ``` ``` vector k0 = func(start); ``` ``` vector temp_k0 = integrate(start, k0, loop_time/2); ``` ``` vector k1 = func(temp_k0); ``` ``` vector temp_k1 = integrate(start, k1, loop_time/2); ``` ``` vector k2 = func(temp_k1); ``` ``` vector temp_k2 = integrate(start, k2, loop_time); ``` ``` vector k3 = func(temp_k2); ``` ``` ``` ``` // Average out each loop's result. ``` ``` vector result(start); ``` ``` for(unsigned int i=0; i