Shuto Naruse
/
Eurobot2012_Secondary
Eurobot2012_Secondary
Fork of Eurobot_2012_Secondary by
Diff: geometryfuncs.h
- Revision:
- 1:cc2a9eb0bd55
- Parent:
- 0:fbfafa6bf5f9
diff -r fbfafa6bf5f9 -r cc2a9eb0bd55 geometryfuncs.h --- a/geometryfuncs.h Fri Apr 20 21:32:24 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,28 +0,0 @@ -#ifndef GEOMETRYFUNCS_H -#define GEOMETRYFUNCS_H - -#include <tvmet/Matrix.h> - -template <typename T> -Matrix <T, 2, 2> Rotmatrix(T theta) { - Matrix <T, 2, 2> outmatrix; - outmatrix = cos(theta), -sin(theta), - sin(theta), cos(theta); - return outmatrix; -} - -// rectifies angle to range -PI to PI -template <typename T> -T rectifyAng (T ang_in) { - ang_in -= (floor(ang_in/(2*PI)))*2*PI; - if (ang_in < -PI) { - ang_in += 2*PI; - } - if (ang_in > PI) { - ang_in -= 2*PI; - } - - return ang_in; -} - -#endif //GEOMETRYFUNCS_H \ No newline at end of file