Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

geometryfuncs.h@1:bbabbd997d21, 2012-04-20 (annotated)

Committer:
narshu
Date:
Fri Apr 20 21:56:15 2012 +0000
Revision:
1:bbabbd997d21
copied everything from secondary;

Who changed what in which revision?

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