Added global-to-local and local-to-global transforms

src/core/CMakeLists.txt

@@ -56,6 +56,7 @@ set(ARGOS3_HEADERS_UTILITY_MATH
   utility/math/ray3.h
   utility/math/rng.h
   utility/math/pose2.h
+  utility/math/transforms.h
   utility/math/vector2.h
   utility/math/vector3.h)
 # argos3/core/utility/math/matrix
@@ -155,6 +156,7 @@ set(ARGOS3_SOURCES_CORE
   utility/math/cylinder.cpp
   utility/math/convex_hull.cpp
   utility/math/pose2.cpp
+  utility/math/transforms.h
   utility/math/vector2.cpp
   utility/math/vector3.cpp
   utility/math/plane.cpp

src/core/utility/math/transforms.cpp

@@ -0,0 +1,29 @@
+#include "transforms.h"
+
+using namespace argos;
+
+/****************************************/
+/****************************************/
+
+CVector3 GlobalToLocal(const CVector3& c_global,
+                       const CVector3& c_translation,
+                       const CQuaternion& c_orientation) {
+   CVector3 cLocal = c_global - c_translation;
+   cLocal.Rotate(c_orientation.Inverse());
+   return cLocal;
+}
+
+/****************************************/
+/****************************************/
+
+CVector3 LocalToGlobal(const CVector3& c_local,
+                       const CVector3& c_translation,
+                       const CQuaternion& c_orientation) {
+   CVector cGlobal = c_local.Rotate(c_orientation);
+   cGlobal += c_translation;
+   return cGlobal;
+}
+
+/****************************************/
+/****************************************/
+

src/core/utility/math/transforms.h

@@ -0,0 +1,16 @@
+#ifndef TRANSFORMS_H
+#define TRANSFORMS_H
+
+namespace argos {
+
+   CVector3 GlobalToLocal(const CVector3& c_global,
+                          const CVector3& c_translation,
+                          const CQuaternion& c_orientation);
+
+   CVector3 LocalToGlobal(const CVector3& c_local,
+                          const CVector3& c_translation,
+                          const CQuaternion& c_orientation);
+
+}
+
+#endif // TRANSFORMS_H