Shuto Naruse
/
Eurobot_2012_Secondary
Eurobot_2012_Secondary
Embed:
(wiki syntax)
Show/hide line numbers
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 Sat Jul 16 2022 01:26:17 by 1.7.2