Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Eurobot_shared/geometryfuncs/geometryfuncs.h
- Committer:
- narshu
- Date:
- 2012-10-17
- Revision:
- 26:0995f61cb7b8
- Parent:
- 9:377560539b74
File content as of revision 26:0995f61cb7b8:
#ifndef GEOMETRYFUNCS_H #define GEOMETRYFUNCS_H #include <tvmet/Matrix.h> template <typename T> Matrix <T, 2, 2> Rotmatrix(T theta) { Matrix <T, 2, 2> outmatrix; outmatrix = cos(theta), -sin(theta), sin(theta), cos(theta); return outmatrix; } // rectifies angle to range -PI to PI template <typename T> T rectifyAng (T ang_in) { ang_in -= (floor(ang_in/(2*PI)))*2*PI; if (ang_in < -PI) { ang_in += 2*PI; } if (ang_in > PI) { ang_in -= 2*PI; } return ang_in; } #endif //GEOMETRYFUNCS_H