From 92484c38b9109217634641ffc586d8eae384a8ac Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Thu, 4 Apr 2019 00:52:50 +0200 Subject: [PATCH 1/5] Fix head rotation --- src/gui/qopenglplotter.cpp | 8 ++++---- src/gui/quserinterface.cpp | 10 +++++----- src/trackerintersense.cpp | 4 ++-- src/trackerpolhemus.cpp | 4 ++-- src/trackerrazor.h | 4 ++-- src/trackervrpn.cpp | 28 ++++++++++++++++++++++++++++ 6 files changed, 43 insertions(+), 15 deletions(-) diff --git a/src/gui/qopenglplotter.cpp b/src/gui/qopenglplotter.cpp index 0e65e8f1..ee5d3e84 100644 --- a/src/gui/qopenglplotter.cpp +++ b/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); diff --git a/src/gui/quserinterface.cpp b/src/gui/quserinterface.cpp index 782ff5b8..265a214a 100644 --- a/src/gui/quserinterface.cpp +++ b/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(pos_x), static_cast(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(pos_x), static_cast(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 diff --git a/src/trackerintersense.cpp b/src/trackerintersense.cpp index 880e473d..673c9b01 100644 --- a/src/trackerintersense.cpp +++ b/src/trackerintersense.cpp @@ -191,12 +191,12 @@ ssr::TrackerInterSense::_thread() ISD_GetTrackingData(_tracker_h, &tracker_data); _controller.take_control()->reference_rotation_offset( Orientation(-tracker_data.Station[0].Euler[0] - + 90.0f)); + + 90.0f + 90.0f)); #else ISD_GetData(_tracker_h, &tracker_data); _controller.take_control()->reference_rotation_offset( Orientation(-static_cast(tracker_data.Station[0].Orientation[0]) - + 90.0f)); + + 90.0f + 90.0f)); #endif // wait a bit diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index f2651ae8..e7a144ba 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -291,7 +291,7 @@ ssr::TrackerPolhemus::_thread() >> _current_data.elevation >> _current_data.roll; - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_data.azimuth + _az_corr)); + _controller.take_control()->reference_offset_rotation( + Orientation(-_current_data.azimuth + _az_corr + 90.0f)); }; } diff --git a/src/trackerrazor.h b/src/trackerrazor.h index f0ab35e4..1ed47527 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -73,8 +73,8 @@ class TrackerRazor : public Tracker calibrate(); _init_az_corr = false; } - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_azimuth + _az_corr)); + _controller.take_control()->reference_offset_rotation( + Orientation(-_current_azimuth + _az_corr + 90.0f)); } void on_error(const std::string &msg) { ERROR("Razor AHRS: " << msg); } diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 0f171602..3973587f 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -141,4 +141,32 @@ ssr::TrackerVrpn::_thread() // TODO: make this configurable: vrpn_SleepMsecs(10); }; + return arg; +} + +void VRPN_CALLBACK +ssr::TrackerVrpn::_vrpn_change_handler(void* arg, const vrpn_TRACKERCB t) +{ + return static_cast(arg)->vrpn_change_handler(t); +} + +void +ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t) +{ + // TODO: check t.sensor for sensor number! + + // get quaternions information + double w = t.quat[0]; + double x = t.quat[1]; + double y = t.quat[2]; + double z = t.quat[3]; + + // TODO: store _az_corr as quaternion and directly set 3D rotation + + // calculate yaw (azimuth) (in radians) from quaternions + double azi = std::atan2(2*(w*x+y*z),1-2*(x*x+y*y)); + + _current_azimuth = azi; + _controller.take_control()->reference_offset_rotation( + Orientation(-azi + _az_corr + 90.0f)); } From 41f726cb42ed1e891ee4dcb287f7ddfb86510deb Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Fri, 5 Apr 2019 11:14:59 +0200 Subject: [PATCH 2/5] Fix 90 deg offset --- src/gui/qopenglplotter.cpp | 8 ++++++-- src/gui/quserinterface.cpp | 4 ++-- src/legacy_scene.h | 2 +- src/rendersubscriber.h | 2 +- src/trackerintersense.cpp | 4 ++-- src/trackerpolhemus.cpp | 2 +- src/trackerrazor.cpp | 2 +- src/trackerrazor.h | 2 +- src/trackervrpn.cpp | 2 +- 9 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/gui/qopenglplotter.cpp b/src/gui/qopenglplotter.cpp index ee5d3e84..2c358b20 100644 --- a/src/gui/qopenglplotter.cpp +++ b/src/gui/qopenglplotter.cpp @@ -383,7 +383,9 @@ void ssr::QOpenGLPlotter::_draw_reference() glTranslatef(0.03f, -0.03f, 0.0f); // rotate according to reference offset position - glRotatef(90.0f + _scene.get_reference_offset().orientation.azimuth, 0.0f, 0.0f, 1.0f); + glRotatef(_scene.get_reference().orientation.azimuth + + _scene.get_reference_offset().orientation.azimuth, + 0.0f, 0.0f, 1.0f); glBindTexture(GL_TEXTURE_2D, _listener_shadow_texture); @@ -397,7 +399,9 @@ void ssr::QOpenGLPlotter::_draw_reference() glPopMatrix(); // rotate according to reference offset position - glRotatef(90.0f + _scene.get_reference_offset().orientation.azimuth, 0.0f, 0.0f, 1.0f); + glRotatef(_scene.get_reference().orientation.azimuth + + _scene.get_reference_offset().orientation.azimuth, + 0.0f, 0.0f, 1.0f); glBindTexture(GL_TEXTURE_2D, _listener_texture); diff --git a/src/gui/quserinterface.cpp b/src/gui/quserinterface.cpp index 265a214a..1dc86a69 100644 --- a/src/gui/quserinterface.cpp +++ b/src/gui/quserinterface.cpp @@ -993,8 +993,8 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event) // previous position relative to relative position offset prev_mouse_pos -= _scene.get_reference_offset().position; - _controller.take_control()->reference_offset_rotation(_scene.get_reference_offset().rotate(90.0f).orientation + - (mouse_pos.orientation() - prev_mouse_pos.orientation())); + _controller.take_control()->reference_offset_rotation(_scene.get_reference_offset().orientation + + (mouse_pos.orientation() - prev_mouse_pos.orientation())); } // else if diff --git a/src/legacy_scene.h b/src/legacy_scene.h index 309058d6..ec7b049d 100644 --- a/src/legacy_scene.h +++ b/src/legacy_scene.h @@ -398,7 +398,7 @@ class LegacyScene : public api::SceneControlEvents void reference_rotation_offset(const Rot& rotation) override { Orientation orientation(rotation); - orientation.azimuth -= 90.0f; // Undo angle conversion offset + // orientation.azimuth -= 90.0f; // Undo angle conversion offset _reference_offset.orientation = orientation; } diff --git a/src/rendersubscriber.h b/src/rendersubscriber.h index a9ea5092..119f1afe 100644 --- a/src/rendersubscriber.h +++ b/src/rendersubscriber.h @@ -185,7 +185,7 @@ class RenderSubscriber : public api::BundleEvents Orientation orientation{rot}; // For backwards compatibility, 90 degrees are added when converting to // Orientation. This, however, should not be done for the reference offset. - orientation.azimuth -= 90.0f; + // orientation.azimuth -= 90.0f; _renderer.state.reference_offset_orientation = orientation; _reference_rotation_offset = rot; } diff --git a/src/trackerintersense.cpp b/src/trackerintersense.cpp index 673c9b01..880e473d 100644 --- a/src/trackerintersense.cpp +++ b/src/trackerintersense.cpp @@ -191,12 +191,12 @@ ssr::TrackerInterSense::_thread() ISD_GetTrackingData(_tracker_h, &tracker_data); _controller.take_control()->reference_rotation_offset( Orientation(-tracker_data.Station[0].Euler[0] - + 90.0f + 90.0f)); + + 90.0f)); #else ISD_GetData(_tracker_h, &tracker_data); _controller.take_control()->reference_rotation_offset( Orientation(-static_cast(tracker_data.Station[0].Orientation[0]) - + 90.0f + 90.0f)); + + 90.0f)); #endif // wait a bit diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index e7a144ba..868a4632 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -292,6 +292,6 @@ ssr::TrackerPolhemus::_thread() >> _current_data.roll; _controller.take_control()->reference_offset_rotation( - Orientation(-_current_data.azimuth + _az_corr + 90.0f)); + Orientation(-_current_data.azimuth + _az_corr)); }; } diff --git a/src/trackerrazor.cpp b/src/trackerrazor.cpp index 7bce6a24..4dea79d9 100644 --- a/src/trackerrazor.cpp +++ b/src/trackerrazor.cpp @@ -35,7 +35,7 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller : Tracker() , _controller(controller) , _current_azimuth(0.0f) - , _az_corr(90.0f) + , _az_corr(0.0f) , _init_az_corr(true) , _tracker(nullptr) { diff --git a/src/trackerrazor.h b/src/trackerrazor.h index 1ed47527..e24a4180 100644 --- a/src/trackerrazor.h +++ b/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 + 90.0f)); + Orientation(-_current_azimuth + _az_corr)); } void on_error(const std::string &msg) { ERROR("Razor AHRS: " << msg); } diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 3973587f..3fc495c1 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -168,5 +168,5 @@ ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t) _current_azimuth = azi; _controller.take_control()->reference_offset_rotation( - Orientation(-azi + _az_corr + 90.0f)); + Orientation(-azi + _az_corr)); } From 736f232cbf1b9770f2b22007752d6739bd8cf222 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Thu, 11 Apr 2019 23:50:13 +0200 Subject: [PATCH 3/5] Fix rebase onto master and revert additional rotations --- src/gui/quserinterface.cpp | 2 +- src/legacy_scene.h | 2 +- src/rendersubscriber.h | 2 +- src/trackerpolhemus.cpp | 2 +- src/trackerrazor.cpp | 2 +- src/trackerrazor.h | 2 +- src/trackervrpn.cpp | 28 ---------------------------- 7 files changed, 6 insertions(+), 34 deletions(-) diff --git a/src/gui/quserinterface.cpp b/src/gui/quserinterface.cpp index 1dc86a69..d44465a5 100644 --- a/src/gui/quserinterface.cpp +++ b/src/gui/quserinterface.cpp @@ -993,7 +993,7 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event) // previous position relative to relative position offset prev_mouse_pos -= _scene.get_reference_offset().position; - _controller.take_control()->reference_offset_rotation(_scene.get_reference_offset().orientation + + _controller.take_control()->reference_rotation_offset(_scene.get_reference_offset().orientation + (mouse_pos.orientation() - prev_mouse_pos.orientation())); } // else if diff --git a/src/legacy_scene.h b/src/legacy_scene.h index ec7b049d..309058d6 100644 --- a/src/legacy_scene.h +++ b/src/legacy_scene.h @@ -398,7 +398,7 @@ class LegacyScene : public api::SceneControlEvents void reference_rotation_offset(const Rot& rotation) override { Orientation orientation(rotation); - // orientation.azimuth -= 90.0f; // Undo angle conversion offset + orientation.azimuth -= 90.0f; // Undo angle conversion offset _reference_offset.orientation = orientation; } diff --git a/src/rendersubscriber.h b/src/rendersubscriber.h index 119f1afe..a9ea5092 100644 --- a/src/rendersubscriber.h +++ b/src/rendersubscriber.h @@ -185,7 +185,7 @@ class RenderSubscriber : public api::BundleEvents Orientation orientation{rot}; // For backwards compatibility, 90 degrees are added when converting to // Orientation. This, however, should not be done for the reference offset. - // orientation.azimuth -= 90.0f; + orientation.azimuth -= 90.0f; _renderer.state.reference_offset_orientation = orientation; _reference_rotation_offset = rot; } diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index 868a4632..f2651ae8 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -291,7 +291,7 @@ ssr::TrackerPolhemus::_thread() >> _current_data.elevation >> _current_data.roll; - _controller.take_control()->reference_offset_rotation( + _controller.take_control()->reference_rotation_offset( Orientation(-_current_data.azimuth + _az_corr)); }; } diff --git a/src/trackerrazor.cpp b/src/trackerrazor.cpp index 4dea79d9..7bce6a24 100644 --- a/src/trackerrazor.cpp +++ b/src/trackerrazor.cpp @@ -35,7 +35,7 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller : Tracker() , _controller(controller) , _current_azimuth(0.0f) - , _az_corr(0.0f) + , _az_corr(90.0f) , _init_az_corr(true) , _tracker(nullptr) { diff --git a/src/trackerrazor.h b/src/trackerrazor.h index e24a4180..f0ab35e4 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -73,7 +73,7 @@ class TrackerRazor : public Tracker calibrate(); _init_az_corr = false; } - _controller.take_control()->reference_offset_rotation( + _controller.take_control()->reference_rotation_offset( Orientation(-_current_azimuth + _az_corr)); } void on_error(const std::string &msg) { ERROR("Razor AHRS: " << msg); } diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 3fc495c1..0f171602 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -141,32 +141,4 @@ ssr::TrackerVrpn::_thread() // TODO: make this configurable: vrpn_SleepMsecs(10); }; - return arg; -} - -void VRPN_CALLBACK -ssr::TrackerVrpn::_vrpn_change_handler(void* arg, const vrpn_TRACKERCB t) -{ - return static_cast(arg)->vrpn_change_handler(t); -} - -void -ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t) -{ - // TODO: check t.sensor for sensor number! - - // get quaternions information - double w = t.quat[0]; - double x = t.quat[1]; - double y = t.quat[2]; - double z = t.quat[3]; - - // TODO: store _az_corr as quaternion and directly set 3D rotation - - // calculate yaw (azimuth) (in radians) from quaternions - double azi = std::atan2(2*(w*x+y*z),1-2*(x*x+y*y)); - - _current_azimuth = azi; - _controller.take_control()->reference_offset_rotation( - Orientation(-azi + _az_corr)); } From ab60693cc00f7cb498738cd071c3b76b988c2163 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Fri, 12 Apr 2019 00:02:58 +0200 Subject: [PATCH 4/5] Fix mouse handling listener --- src/gui/quserinterface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gui/quserinterface.cpp b/src/gui/quserinterface.cpp index d44465a5..a701564c 100644 --- a/src/gui/quserinterface.cpp +++ b/src/gui/quserinterface.cpp @@ -993,7 +993,8 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event) // previous position relative to relative position offset prev_mouse_pos -= _scene.get_reference_offset().position; - _controller.take_control()->reference_rotation_offset(_scene.get_reference_offset().orientation + + _controller.take_control()->reference_rotation_offset(_scene.get_reference().orientation + + _scene.get_reference_offset().orientation + (mouse_pos.orientation() - prev_mouse_pos.orientation())); } // else if From 4ed9d5ead559daec61b7ec0c1095b358ca5302fe Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Fri, 12 Apr 2019 00:12:25 +0200 Subject: [PATCH 5/5] Fix Head tracking rotation --- src/trackerpolhemus.cpp | 2 +- src/trackerrazor.h | 2 +- src/trackervrpn.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index f2651ae8..fcfad114 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -192,7 +192,7 @@ ssr::TrackerPolhemus::_open_serial_port(const char *portname) void ssr::TrackerPolhemus::calibrate() { - _az_corr = _current_data.azimuth; + _az_corr = _current_data.azimuth + 90; } void diff --git a/src/trackerrazor.h b/src/trackerrazor.h index f0ab35e4..34fc9cdf 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -57,7 +57,7 @@ class TrackerRazor : public Tracker if (_tracker != nullptr) delete _tracker; } - virtual void calibrate() { _az_corr = _current_azimuth; } + virtual void calibrate() { _az_corr = _current_azimuth + 90; } private: /// constructor diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 0f171602..934a1568 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -109,7 +109,7 @@ ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t) void ssr::TrackerVrpn::calibrate() { - _az_corr = _current_azimuth; + _az_corr = _current_azimuth + 90; } void