move matrix manipulations to matrix and (new) utils modules

CMakeLists.txt

@@ -39,7 +39,7 @@ find_package(X11 REQUIRED)
 include_directories(${install_dir}/include ${X11_INCLUDE_DIR} ${cli11_SOURCE_DIR}/include)
 link_directories(${X11_LIBRARIES})
 
-add_executable(spnavigt src/spnavigt.cpp)
+add_executable(spnavigt src/utils.cpp src/spnavigt.cpp)
 target_link_libraries(spnavigt OpenIGTLink ${X11_LIBRARIES} lspnav)
 set_target_properties(spnavigt PROPERTIES OUTPUT_NAME spnavigt)
 

src/matrix.hpp

@@ -42,4 +42,11 @@ class QuadMatrix
     float m_data[N][N];
 };
 
+
+// Additional functions
+QuadMatrix<4> rotmatX(float angle);
+QuadMatrix<4> rotmatY(float angle);
+QuadMatrix<4> rotmatZ(float angle);
+
+
 #include "matrix.tpp"

src/matrix.tpp

@@ -1,3 +1,5 @@
+#include "utils.hpp"
+
 template <int N>
 void QuadMatrix<N>::copy(const QuadMatrix<N> &other)
 {
@@ -262,4 +264,37 @@ std::string QuadMatrix<N>::toString() const
         sstr << "\n";
     }
     return sstr.str();
+}
+
+QuadMatrix<4> rotmatX(float angle)
+{
+    float a = deg2rad(angle);
+    float arr[4][4] = {
+        {1.0f, 0.0f, 0.0f, 0.0f},
+        {0.0f, (float)cos(a), (float)-sin(a), 0.0f},
+        {0.0f, (float)sin(a), (float)cos(a), 0.0f},
+        {0.0f, 0.0f, 0.0f, 1.0f}};
+    return QuadMatrix<4>(arr);
+}
+
+QuadMatrix<4> rotmatY(float angle)
+{
+    float a = deg2rad(angle);
+    float arr[4][4] = {
+        {(float)cos(a), 0.0f, (float)sin(a), 0.0f},
+        {0.0f, 1.0f, 0.0f, 0.0f},
+        {(float)-sin(a), 0.0f, (float)cos(a), 0.0f},
+        {0.0f, 0.0f, 0.0f, 1.0f}};
+    return QuadMatrix<4>(arr);
+}
+
+QuadMatrix<4> rotmatZ(float angle)
+{
+    float a = deg2rad(angle);
+    float arr[4][4] = {
+        {(float)cos(a), (float)-sin(a), 0.0f, 0.0f},
+        {(float)sin(a), (float)cos(a), 0.0f, 0.0f},
+        {0.0f, 0.0f, 1.0f, 0.0f},
+        {0.0f, 0.0f, 0.0f, 1.0f}};
+    return QuadMatrix<4>(arr);
 }

src/spnavigt.cpp

@@ -13,6 +13,7 @@
 #include <igtlTransformMessage.h>
 
 #include "matrix.hpp"
+#include "utils.hpp"
 
 static bool running;
 igtl::Socket::Pointer client_socket;
@@ -22,39 +23,6 @@ void sig(int s)
     running = false;
 }
 
-QuadMatrix<4> rotmatX(float angle)
-{
-    double a = M_PI * angle / 180.0;
-    float arr[4][4] = {
-        {1.0f, 0.0f, 0.0f, 0.0f},
-        {0.0f, (float)cos(a), (float)-sin(a), 0.0f},
-        {0.0f, (float)sin(a), (float)cos(a), 0.0f},
-        {0.0f, 0.0f, 0.0f, 1.0f}};
-    return QuadMatrix<4>(arr);
-}
-
-QuadMatrix<4> rotmatY(float angle)
-{
-    double a = M_PI * angle / 180.0;
-    float arr[4][4] = {
-        {(float)cos(a), 0.0f, (float)sin(a), 0.0f},
-        {0.0f, 1.0f, 0.0f, 0.0f},
-        {(float)-sin(a), 0.0f, (float)cos(a), 0.0f},
-        {0.0f, 0.0f, 0.0f, 1.0f}};
-    return QuadMatrix<4>(arr);
-}
-
-QuadMatrix<4> rotmatZ(float angle)
-{
-    double a = M_PI * angle / 180.0;
-    float arr[4][4] = {
-        {(float)cos(a), (float)-sin(a), 0.0f, 0.0f},
-        {(float)sin(a), (float)cos(a), 0.0f, 0.0f},
-        {0.0f, 0.0f, 1.0f, 0.0f},
-        {0.0f, 0.0f, 0.0f, 1.0f}};
-    return QuadMatrix<4>(arr);
-}
-
 int main(int argc, char *argv[])
 {
     spnav_event sev;
@@ -63,7 +31,7 @@ int main(int argc, char *argv[])
 
     if (spnav_open() == -1)
     {
-        fprintf(stderr, "failed to connect to spacenavd\n");
+        fprintf(stderr, "Failed to connect to spacenavd\n");
         return 1;
     }
 

src/utils.cpp

@@ -0,0 +1,13 @@
+#include <cmath>
+
+#include "utils.hpp"
+
+float rad2deg(float rad)
+{
+    return 180.0f * rad / M_PI;
+}
+
+float deg2rad(float deg)
+{
+    return M_PI * deg / 180.0f;
+}

src/utils.hpp

@@ -0,0 +1,4 @@
+#pragma once
+
+float rad2deg(float rad);
+float deg2rad(float deg);