Oskar Weigl / Mbed 2 deprecated Eurobot2013

We are going to win! wohoo

Dependencies:   mbed mbed-rtos

system/geometryfuncs.h@9:08552997b544, 2012-11-14 (annotated)

Committer:
madcowswe
Date:
Wed Nov 14 17:15:53 2012 +0000
Revision:
9:08552997b544
Parent:
6:5a52c046d8f7 
Added an important comment

Who changed what in which revision?

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