Shuto Naruse / geometryfuncs

geometryfuncs for ICRS eurobot

Files at this revision

API Documentation at this revision

Revision 0:886118d8488d, committed 2012-04-26

Comitter:
narshu
Date:
Thu Apr 26 19:34:16 2012 +0000
Commit message:

Changed in this revision

geometryfuncs.h Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r 886118d8488d geometryfuncs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/geometryfuncs.h	Thu Apr 26 19:34:16 2012 +0000
@@ -0,0 +1,28 @@
+#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
\ No newline at end of file