Cleanup, fix compensation for opti and steamvr

trackpoint-app/include/PointShape.hpp

@@ -35,7 +35,6 @@ private:
   osg::ref_ptr<osg::Switch> _selectionSwitch;
   osg::ref_ptr<osg::MatrixTransform> _selectionTranslateGroup;
   osg::ref_ptr<osg::MatrixTransform> _selectionRotateGroup;
-  osg::ref_ptr<osg::MatrixTransform> _selectionMoveToEndGroup;
   osg::ref_ptr<osg::MatrixTransform> _screwMove;
   osg::ref_ptr<osg::Group> _renderRoot;
   osg::ref_ptr<osg::ShapeDrawable> _shape;
@@ -43,7 +42,9 @@ private:
   osg::ref_ptr<osg::Geode> _thread;
   osg::ref_ptr<osg::Geode> _geode;
   osg::Vec3f _normalModifier;
-  double _optiTrackSteamVRLength;
+  OptiTrackSettings _optiTrackSettings;
+  EMFTrackSettings _emfTrackSettings;
+  SteamVRTrackSettings _steamVrTrackSettings;
   ActiveTrackingSystem _activeTrackingSystem;
   bool _compensation = true;
   float _compensationLength = 0.0f;

trackpoint-app/src/PointShape.cpp

@@ -19,11 +19,8 @@ PointShape::PointShape(const osg::ref_ptr<osg::Group> renderRoot, const ActiveTr
   _selectionRotateGroup = new osg::MatrixTransform;
   _selectionRotateGroup->addChild(_screwMove.get());
 
-  _selectionMoveToEndGroup = new osg::MatrixTransform;
-  _selectionMoveToEndGroup->addChild(_selectionRotateGroup.get());
-
   _selectionTranslateGroup = new osg::MatrixTransform;
-  _selectionTranslateGroup->addChild(_selectionMoveToEndGroup.get());
+  _selectionTranslateGroup->addChild(_selectionRotateGroup.get());
 
   _selectionSwitch = new osg::Switch;
   _selectionSwitch->addChild(_selectionTranslateGroup.get());
@@ -54,22 +51,6 @@ void PointShape::rotateToNormalVector(osg::Vec3f normal, float normalRotation) {
   osg::Matrix matrix = osg::Matrix::rotate(osg::Vec3f(0.0f, 0.0f, 1.0f), normal);
   matrix = matrix.operator*(osg::Matrix::rotate(normalRotation * M_PI / 180, normal));
   _selectionRotateGroup->setMatrix(matrix);
-  if (_activeTrackingSystem == OptiTrack || _activeTrackingSystem == SteamVRTrack) {
-    float movementFixLength = _optiTrackSteamVRLength / 2;
-    if (_compensation) {
-      movementFixLength -= _compensationLength / 2;
-    }
-    osg::Vec3f movementVector = normal.operator*(movementFixLength);
-    _selectionMoveToEndGroup->setMatrix(osg::Matrix::translate(movementVector));
-  }
-  if (_activeTrackingSystem == SteamVRTrack) {
-    float threadFixLength = _optiTrackSteamVRLength / 2;
-    if (_compensation) {
-      threadFixLength += _compensationLength / 2;
-    }
-    osg::Vec3f movementVector = osg::Vec3f(0.0f, 0.0f, 1.0f).operator*(threadFixLength);
-    _screwMove->setMatrix(osg::Matrix::translate(movementVector));
-  }
 }
 
 void PointShape::setVisibility(bool mode) {
@@ -88,20 +69,14 @@ void PointShape::setColor(osg::Vec4 color) {
 
 void PointShape::setupOptiTrack(OptiTrackSettings optiTrackSettings) {
   if (_activeTrackingSystem == OptiTrack) {
-    _optiTrackSteamVRLength = optiTrackSettings.length;
+    _optiTrackSettings = optiTrackSettings;
     _geode = new osg::Geode;
     _shape = new osg::ShapeDrawable;
-    float baseLength = optiTrackSettings.length - 5.0f;
-    float smallShapePosition = optiTrackSettings.length - 7.5f;
-    if (_compensation) {
-      baseLength += _compensationLength;
-      smallShapePosition += _compensationLength / 2;
-    }
-    _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, -2.5f), optiTrackSettings.radius, baseLength));
+    _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, (optiTrackSettings.length - 5.0f) / 2), optiTrackSettings.radius, optiTrackSettings.length - 5.0f));
     _shape->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_shape.get());
     _optiConnector = new osg::ShapeDrawable;
-    _optiConnector->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, smallShapePosition), 0.74f, 5.0f));
+    _optiConnector->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, optiTrackSettings.length - 2.5f), 0.74f, 5.0f));
     _optiConnector->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_optiConnector.get());
     OSGWidget::fixMaterialState(_geode);
@@ -111,13 +86,10 @@ void PointShape::setupOptiTrack(OptiTrackSettings optiTrackSettings) {
 
 void PointShape::setupEMFTrack(EMFTrackSettings emfTrackSettings) {
   if (_activeTrackingSystem == EMFTrack) {
+    _emfTrackSettings = emfTrackSettings;
     _geode = new osg::Geode;
     _shape = new osg::ShapeDrawable;
-    float depth = static_cast<float>(emfTrackSettings.depth);
-    if (_compensation) {
-      depth += _compensationLength;
-    }
-    _shape->setShape(new osg::Box(osg::Vec3(0.0f, 0.0f, static_cast<float>(emfTrackSettings.depth) / 2), static_cast<float>(emfTrackSettings.width), static_cast<float>(emfTrackSettings.height), depth));
+    _shape->setShape(new osg::Box(osg::Vec3(0.0f, 0.0f, static_cast<float>(emfTrackSettings.depth) / 2), static_cast<float>(emfTrackSettings.width), static_cast<float>(emfTrackSettings.height), static_cast<float>(emfTrackSettings.depth)));
     _shape->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_shape.get());
     OSGWidget::fixMaterialState(_geode);
@@ -127,14 +99,10 @@ void PointShape::setupEMFTrack(EMFTrackSettings emfTrackSettings) {
 
 void PointShape::setupSteamVRTrack(SteamVRTrackSettings steamVrTrackSettings) {
   if (_activeTrackingSystem == SteamVRTrack) {
-    _optiTrackSteamVRLength = steamVrTrackSettings.length;
+    _steamVrTrackSettings = steamVrTrackSettings;
     _geode = new osg::Geode;
     _shape = new osg::ShapeDrawable;
-    float baseLength = steamVrTrackSettings.length;
-    if (_compensation) {
-      baseLength += _compensationLength;
-    }
-    _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, 0.0f), STEAMVR_CONSTANT_RADIUS, baseLength));
+    _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, steamVrTrackSettings.length / 2), STEAMVR_CONSTANT_RADIUS, steamVrTrackSettings.length));
     _shape->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_shape.get());
 
@@ -143,6 +111,8 @@ void PointShape::setupSteamVRTrack(SteamVRTrackSettings steamVrTrackSettings) {
     _thread->addDrawable(MainWindow::getInstance()->getOsgWidget()->_steamvrThreadMesh.get());
     OSGWidget::fixMaterialState(_thread);
     _screwMove->addChild(_thread.get());
+    osg::Vec3f movementVector = osg::Vec3f(0.0f, 0.0f, 1.0f).operator*(steamVrTrackSettings.length);
+    _screwMove->setMatrix(osg::Matrix::translate(movementVector));
 
     OSGWidget::fixMaterialState(_geode);
     _selectionRotateGroup->addChild(_geode.get());
@@ -164,6 +134,40 @@ void PointShape::setupActionPoints() {
 void PointShape::setCompensation(bool compensation, float compensationLength) {
   _compensation = compensation;
   _compensationLength = compensationLength;
+  switch(_activeTrackingSystem) {
+    case OptiTrack: {
+      float baseLength = _optiTrackSettings.length - 5.0f;
+      float basePosition = _optiTrackSettings.length - 5.0f;
+      if (_compensation) {
+        baseLength += _compensationLength;
+        basePosition -= _compensationLength;
+      }
+      _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, basePosition / 2), _optiTrackSettings.radius, baseLength));
+      break;
+    }
+    case EMFTrack: {
+      float depth = static_cast<float>(_emfTrackSettings.depth);
+      if (_compensation) {
+        depth += _compensationLength;
+      }
+      osg::Box* box = static_cast<osg::Box*>(_shape->getShape());
+      box->set(box->getCenter(), osg::Vec3(box->getHalfLengths().x(), box->getHalfLengths().y(), depth));
+      break;
+    }
+    case SteamVRTrack: {
+      float baseLength = _steamVrTrackSettings.length;
+      float basePosition = _steamVrTrackSettings.length;
+      if (_compensation) {
+        baseLength += _compensationLength;
+        basePosition -= _compensationLength;
+      }
+      _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, basePosition / 2), STEAMVR_CONSTANT_RADIUS, baseLength));
+      break;
+    }
+    case ActionPoints: {
+      break;
+    }
+  }
 }
 
 osg::ref_ptr<osg::Geode> PointShape::getMesh() {

trackpoint-app/src/TrackPointRenderer.cpp

@@ -21,8 +21,8 @@ void TrackPointRenderer::render(ActiveTrackingSystem activeTrackingSystem) {
       int id = 0;
       for (OptiTrackPoint* point: points) {
         PointShape* newShape = addPointShape(static_cast<TrackPoint*>(point), activeTrackingSystem);
-        newShape->setCompensation(point->getCompensation(), point->getNormalCompensation());
         newShape->setupOptiTrack(point->getOptiTrackSettings());
+        newShape->setCompensation(point->getCompensation(), point->getNormalCompensation());
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
       }
@@ -33,8 +33,8 @@ void TrackPointRenderer::render(ActiveTrackingSystem activeTrackingSystem) {
       int id = 0;
       for (EMFTrackPoint* point: points) {
         PointShape* newShape = addPointShape(static_cast<TrackPoint*>(point), activeTrackingSystem);
-        newShape->setCompensation(point->getCompensation(), point->getNormalCompensation());
         newShape->setupEMFTrack(point->getEMFTrackSettings());
+        newShape->setCompensation(point->getCompensation(), point->getNormalCompensation());
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
       }
@@ -45,8 +45,8 @@ void TrackPointRenderer::render(ActiveTrackingSystem activeTrackingSystem) {
       int id = 0;
       for (SteamVRTrackPoint* point: points) {
         PointShape* newShape = addPointShape(static_cast<TrackPoint*>(point), activeTrackingSystem);
-        newShape->setCompensation(point->getCompensation(), point->getNormalCompensation());
         newShape->setupSteamVRTrack(point->getSteamVRTrackSettings());
+        newShape->setCompensation(point->getCompensation(), point->getNormalCompensation());
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
       }
@@ -57,8 +57,8 @@ void TrackPointRenderer::render(ActiveTrackingSystem activeTrackingSystem) {
       int id = 0;
       for (ActionPoint* point: points) {
         PointShape* newShape = addPointShape(static_cast<TrackPoint*>(point), activeTrackingSystem);
-        newShape->setCompensation(point->getCompensation(), 0.0f);
         newShape->setupActionPoints();
+        newShape->setCompensation(point->getCompensation(), 0.0f);
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
       }