Shuto Naruse / Eurobot_2012_shared

Eurobot_shared pubulished from Eurobot Primary

geometryfuncs/geometryfuncs.h@0:434fd09723be, 2012-08-07 (annotated)

Committer:
narshu
Date:
Tue Aug 07 10:25:53 2012 +0000
Revision:
0:434fd09723be
[mbed] converted /Eurobot_2012_Primary/Eurobot_shared

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:434fd09723be 1 #ifndef GEOMETRYFUNCS_H
narshu 0:434fd09723be 2 #define GEOMETRYFUNCS_H
narshu 0:434fd09723be 3
narshu 0:434fd09723be 4 #include <tvmet/Matrix.h>
narshu 0:434fd09723be 5
narshu 0:434fd09723be 6 template <typename T>
narshu 0:434fd09723be 7 Matrix <T, 2, 2> Rotmatrix(T theta) {
narshu 0:434fd09723be 8 Matrix <T, 2, 2> outmatrix;
narshu 0:434fd09723be 9 outmatrix = cos(theta), -sin(theta),
narshu 0:434fd09723be 10 sin(theta), cos(theta);
narshu 0:434fd09723be 11 return outmatrix;
narshu 0:434fd09723be 12 }
narshu 0:434fd09723be 13
narshu 0:434fd09723be 14 // rectifies angle to range -PI to PI
narshu 0:434fd09723be 15 template <typename T>
narshu 0:434fd09723be 16 T rectifyAng (T ang_in) {
narshu 0:434fd09723be 17 ang_in -= (floor(ang_in/(2*PI)))*2*PI;
narshu 0:434fd09723be 18 if (ang_in < -PI) {
narshu 0:434fd09723be 19 ang_in += 2*PI;
narshu 0:434fd09723be 20 }
narshu 0:434fd09723be 21 if (ang_in > PI) {
narshu 0:434fd09723be 22 ang_in -= 2*PI;
narshu 0:434fd09723be 23 }
narshu 0:434fd09723be 24
narshu 0:434fd09723be 25 return ang_in;
narshu 0:434fd09723be 26 }
narshu 0:434fd09723be 27
narshu 0:434fd09723be 28 #endif //GEOMETRYFUNCS_H