Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/geometryfuncs/geometryfuncs.h@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Parent:
9:377560539b74 
Eurobot 2012 Primary;

Who changed what in which revision?

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