Fix head rotation

Author: Chris Hold

src/gui/qopenglplotter.cpp

@@ -382,8 +382,8 @@ void ssr::QOpenGLPlotter::_draw_reference()
 
     glTranslatef(0.03f, -0.03f, 0.0f);
 
-    // rotate according to reference position
-    glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f);
+    // rotate according to reference offset position
+    glRotatef(90.0f + _scene.get_reference_offset().orientation.azimuth, 0.0f, 0.0f, 1.0f);
 
     glBindTexture(GL_TEXTURE_2D, _listener_shadow_texture);
 
@@ -396,8 +396,8 @@ void ssr::QOpenGLPlotter::_draw_reference()
 
     glPopMatrix();
 
-    // rotate according to reference position
-    glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f);
+    // rotate according to reference offset position
+    glRotatef(90.0f + _scene.get_reference_offset().orientation.azimuth, 0.0f, 0.0f, 1.0f);
 
     glBindTexture(GL_TEXTURE_2D, _listener_texture);
 

src/gui/quserinterface.cpp

@@ -980,8 +980,8 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event)
     // absolut mouse position in OpenGL coordinates
     Position mouse_pos(static_cast<float>(pos_x), static_cast<float>(pos_y));
 
-    // position relative to reference position
-    mouse_pos -= _scene.get_reference().position;
+    // position relative to reference position offset
+    mouse_pos -= _scene.get_reference_offset().position;
 
     _get_openGL_pos(_previous_mouse_event.x(),
                     _previous_mouse_event.y(),
@@ -990,10 +990,10 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event)
     // previous absolut position in OpenGL coordinates
     Position prev_mouse_pos(static_cast<float>(pos_x), static_cast<float>(pos_y));
 
-    // previous position relative to relative position
-    prev_mouse_pos -= _scene.get_reference().position;
+    // previous position relative to relative position offset
+    prev_mouse_pos -= _scene.get_reference_offset().position;
 
-    _controller.take_control()->reference_rotation(_scene.get_reference().orientation +
+    _controller.take_control()->reference_offset_rotation(_scene.get_reference_offset().rotate(90.0f).orientation +
                                           (mouse_pos.orientation() - prev_mouse_pos.orientation()));
 
   } // else if

src/trackerintersense.cpp

@@ -193,12 +193,12 @@ void* ssr::TrackerInterSense::thread(void *arg)
     ISD_GetTrackingData(_tracker_h, &tracker_data);
     _controller.take_control()->reference_offset_rotation(
         Orientation(-tracker_data.Station[0].Euler[0]
-           + 90.0f));
+           + 90.0f + 90.0f));
 #else
     ISD_GetData(_tracker_h, &tracker_data);
     _controller.take_control()->reference_offset_rotation(
         Orientation(-static_cast<float>(tracker_data.Station[0].Orientation[0])
-           + 90.0f));
+           + 90.0f + 90.0f));
 #endif
 
     // wait a bit

src/trackerpolhemus.cpp

@@ -301,7 +301,7 @@ ssr::TrackerPolhemus::thread(void *arg)
               >> _current_data.roll;
 
     _controller.take_control()->reference_offset_rotation(
-        Orientation(-_current_data.azimuth + _az_corr));
+        Orientation(-_current_data.azimuth + _az_corr + 90.0f));
   };
   return arg;
 }

src/trackerrazor.h

@@ -74,7 +74,7 @@ class TrackerRazor : public Tracker
         _init_az_corr = false;
       }
       _controller.take_control()->reference_offset_rotation(
-          Orientation(-_current_azimuth + _az_corr));
+          Orientation(-_current_azimuth + _az_corr + 90.0f));
     }
     void on_error(const std::string &msg) { ERROR("Razor AHRS: " << msg); }
 

src/trackervrpn.cpp

@@ -142,5 +142,5 @@ ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t)
 
   _current_azimuth = azi;
   _controller.take_control()->reference_offset_rotation(
-      Orientation(-azi + _az_corr));
+      Orientation(-azi + _az_corr + 90.0f));
 }