Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers geometryfuncs.h Source File

geometryfuncs.h

00001 #ifndef GEOMETRYFUNCS_H
00002 #define GEOMETRYFUNCS_H
00003 
00004 #include <tvmet/Matrix.h>
00005 
00006 template <typename T>
00007 Matrix <T, 2, 2> Rotmatrix(T theta) {
00008      Matrix <T, 2, 2> outmatrix;
00009      outmatrix = cos(theta), -sin(theta),
00010                  sin(theta), cos(theta);
00011      return outmatrix;
00012 }
00013 
00014 // rectifies angle to range -PI to PI
00015 template <typename T>
00016 T rectifyAng (T ang_in) {
00017     ang_in -= (floor(ang_in/(2*PI)))*2*PI;
00018     if (ang_in < -PI) {
00019         ang_in += 2*PI;
00020     }
00021     if (ang_in > PI) {
00022         ang_in -= 2*PI;
00023     }
00024 
00025     return ang_in;
00026 }
00027 
00028 #endif //GEOMETRYFUNCS_H
Generated on Tue Jul 12 2022 21:49:57 by  doxygen 1.7.2