fix matrix multiplication, tune sensitivity parameter

henrykrumb · Aug 3, 2023 · 2e31b54 · 2e31b54
1 parent 4552b84
commit 2e31b54
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 26 deletions.
diff --git a/src/matrix.hpp b/src/matrix.hpp
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <string>
+#include <sstream>
+
 template <int N>
 class QuadMatrix
 {
@@ -31,9 +34,10 @@ class QuadMatrix
     bool equals(const QuadMatrix& other) const;
     bool operator==(const QuadMatrix& other) { return equals(other); }
     bool operator!=(const QuadMatrix& other) { return !equals(other); }
-
     // TODO: overload operators
 
+    std::string toString() const;
+
 protected:
     float m_data[N][N];
 };

diff --git a/src/matrix.tpp b/src/matrix.tpp
@@ -90,7 +90,7 @@ QuadMatrix<N> QuadMatrix<N>::multiply(const QuadMatrix<N> &other)
             result.m_data[i][j] = 0.0f;
             for (int k = 0; k < N; k++)
             {
-                result.m_data[i][j] += m_data[i][j] * other.m_data[j][k];
+                result.m_data[i][j] += m_data[i][k] * other.m_data[k][j];
             }
         }
     }
@@ -146,16 +146,6 @@ float QuadMatrix<2>::determinant()
     return det;
 }
 
-template <>
-float QuadMatrix<3>::determinant()
-{
-    float det = 0.0f;
-    det += m_data[0][0] * m_data[1][1] * m_data[2][2];
-    det += m_data[0][1] * m_data[1][2] * m_data[2][0];
-    det += m_data[0][2] * m_data[1][0] * m_data[2][1];
-    return det;
-}
-
 template <int N>
 QuadMatrix<N - 1> QuadMatrix<N>::minor(const int strike_row, const int strike_column)
 {
@@ -193,12 +183,13 @@ float QuadMatrix<N>::determinant()
     {
         // choose third row, as we're likely to have many zeros in there
         // if we're dealing with homogeneous 4x4 transforms
-        int i = 3;
+        int i = N - 1;
         if (m_data[i][j] == 0)
         {
             continue;
         }
-        int minor_determinant = minor(i, j).determinant();
+        auto m = minor(i, j);
+        float minor_determinant = m.determinant();
         if ((i + j) % 2 != 0)
         {
             minor_determinant = -minor_determinant;
@@ -255,4 +246,20 @@ bool QuadMatrix<N>::equals(const QuadMatrix<N> &other) const
         }
     }
     return true;
+}
+
+template <int N>
+std::string QuadMatrix<N>::toString() const
+{
+    std::stringstream sstr;
+
+    for (int i = 0; i < N; i++)
+    {
+        for (int j = 0; j < N; j++)
+        {
+            sstr << m_data[i][j] << " ";
+        }
+        sstr << "\n";
+    }
+    return sstr.str();
 }
diff --git a/src/spnavigt.cpp b/src/spnavigt.cpp
@@ -88,7 +88,9 @@ int main(int argc, char *argv[])
         exit(EXIT_FAILURE);
     }
 
+    float ax = 0.0f, ay = 0.0f, az = 0.0f;
     bool dirty = true;
+    float sensitivity = 0.005f;
     while (running)
     {
         client_socket = server_socket->WaitForConnection(timeout);
@@ -100,34 +102,45 @@ int main(int argc, char *argv[])
             {
                 if (sev.type == SPNAV_EVENT_MOTION)
                 {
-                    T.set(0, 3, T.get(0, 3) + (float)-sev.motion.x * 0.01);
-                    T.set(2, 3, T.get(2, 3) + (float)sev.motion.y * 0.01);
-                    T.set(1, 3, T.get(1, 3) + (float)-sev.motion.z * 0.01);
-                    //Rx = rotmatX(sev.motion.rx);
-                    //Ry = rotmatY(sev.motion.rz);
-                    //Rz = rotmatZ(sev.motion.ry);
+                    T.set(0, 3, T.get(0, 3) + (float)-sev.motion.x * sensitivity);
+                    T.set(2, 3, T.get(2, 3) + (float)sev.motion.y * sensitivity);
+                    T.set(1, 3, T.get(1, 3) + (float)-sev.motion.z * sensitivity);
+
+                    ax += sev.motion.rx * sensitivity;
+                    ay += sev.motion.ry * sensitivity;
+                    az += sev.motion.rz * sensitivity;
 
                     printf("got motion event: t(%d, %d, %d) ", sev.motion.x, sev.motion.y, sev.motion.z);
                     printf("r(%d, %d, %d)\n", sev.motion.rx, sev.motion.ry, sev.motion.rz);
                     dirty = true;
                 }
                 else if (sev.type == SPNAV_EVENT_BUTTON)
                 {
-                    T.identity();
+                    if (sev.button.bnum == 0)
+                    {
+                        T.identity();
+                    }
+                    else
+                    {
+                        ax = 0;
+                        ay = 0;
+                        az = 0;
+                    }
+                    dirty = true;
                     printf("got button %s event b(%d)\n", sev.button.press ? "press" : "release", sev.button.bnum);
                 }
             }
 
             if (dirty)
             {
+                Rx = rotmatX(ax);
+                Ry = rotmatY(az);
+                Rz = rotmatZ(ay);
+
                 auto transform_message = igtl::TransformMessage::New();
                 transform_message->SetDeviceName("SpaceMouse");
                 float float_matrix[4][4];
-                QuadMatrix<4> transform_matrix = Rx.multiply(Ry).multiply(Rz);
-                transform_matrix.set(0, 3, T.get(0, 3));
-                transform_matrix.set(2, 3, T.get(2, 3));
-                transform_matrix.set(1, 3, T.get(1, 3));
-
+                QuadMatrix<4> transform_matrix = Rz.multiply(Ry).multiply(Rx).multiply(T);
                 transform_matrix.toArray(float_matrix);
                 transform_message->SetMatrix(float_matrix);
                 transform_message->Pack();