Implement normal modifier compensation for storage and preview

trackpoint-app/include/EMFTrackPoint.hpp

@@ -13,6 +13,7 @@ public:
   double getDepth();
   EMFTrackSettings getEMFTrackSettings();
   void updateEMFTrackSettings(EMFTrackSettings settings);
+  float getNormalCompensation();
 
 private:
   double _width;

trackpoint-app/include/MeshTools.hpp

@@ -7,4 +7,6 @@
 class MeshTools {
 public:
   static void calculateNormals(const std::vector<Lib3MF::sPosition> verticesBuffer, const std::vector<Lib3MF::sTriangle> triangleBuffer, osg::ref_ptr<osg::Vec3Array> vertices, osg::ref_ptr<osg::Vec3Array> normals);
+  static float angleBetween(osg::Vec3 a, osg::Vec3 b);
+  static float compensationLength(osg::Vec3 a, osg::Vec3 b, float length);
 };

trackpoint-app/include/OptiTrackPoint.hpp

@@ -12,6 +12,7 @@ public:
   double getRadius();
   OptiTrackSettings getOptiTrackSettings();
   void updateOptiTrackSettings(OptiTrackSettings settings);
+  float getNormalCompensation();
 
 private:
   void updateShift();

trackpoint-app/include/PointShape.hpp

@@ -27,6 +27,7 @@ public:
   void setupEMFTrack(EMFTrackSettings emfTrackSettings);
   void setupSteamVRTrack(SteamVRTrackSettings steamVrTrackSettings);
   void setupActionPoints();
+  void setCompensation(bool compensation, float compensationLength);
   osg::ref_ptr<osg::Geode> getMesh();
 
 private:
@@ -44,4 +45,6 @@ private:
   osg::Vec3f _normalModifier;
   double _optiTrackSteamVRLength;
   ActiveTrackingSystem _activeTrackingSystem;
+  bool _compensation = true;
+  float _compensationLength = 0.0f;
 };

trackpoint-app/include/SteamVRTrackPoint.hpp

@@ -11,6 +11,7 @@ public:
   double getLength();
   SteamVRTrackSettings getSteamVRTrackSettings();
   void updateSteamVRTrackSettings(SteamVRTrackSettings settings);
+  float getNormalCompensation();
 
 private:
   void updateShift();

trackpoint-app/src/EMFTrackPoint.cpp

@@ -1,6 +1,9 @@
 // Include own headers
 #include "EMFTrackPoint.hpp"
 
+// Include modules
+#include "MeshTools.hpp"
+
 EMFTrackPoint::EMFTrackPoint(const osg::Vec3 point, const osg::Vec3 normal, const osg::Vec3 normalModifier, const float normalRotation, const bool compensation, const double width, const double height, const double depth): TrackPoint(point, normal, normalModifier, normalRotation, compensation) {
   _width = width;
   _height = height;
@@ -28,3 +31,8 @@ void EMFTrackPoint::updateEMFTrackSettings(EMFTrackSettings settings) {
   _height = settings.height;
   _depth = settings.depth;
 }
+
+float EMFTrackPoint::getNormalCompensation() {
+  float compensationLength = MeshTools::compensationLength(_normal, _normalModifier, std::sqrt(std::pow(_width / 2, 2) + std::pow(_height / 2, 2)));
+  return compensationLength > 0.0f ? compensationLength : 0.0f;
+}

trackpoint-app/src/MeshTools.cpp

@@ -1,6 +1,9 @@
 // Include own header
 #include "MeshTools.hpp"
 
+// Include dependencies
+#include <math.h>
+
 void MeshTools::calculateNormals(const std::vector<Lib3MF::sPosition> verticesBuffer, const std::vector<Lib3MF::sTriangle> triangleBuffer, osg::ref_ptr<osg::Vec3Array> vertices, osg::ref_ptr<osg::Vec3Array> normals) {
   for (const Lib3MF::sTriangle triangle: triangleBuffer) {
     // Create osg vectors
@@ -20,3 +23,18 @@ void MeshTools::calculateNormals(const std::vector<Lib3MF::sPosition> verticesBu
     }
   }
 }
+
+float MeshTools::angleBetween(osg::Vec3 a, osg::Vec3 b) {
+  float normA = a.length();
+  float normB = b.length();
+  float dotProduct = a.operator*(b);
+  return acos(dotProduct / (normA * normB));
+}
+
+float MeshTools::compensationLength(osg::Vec3 a, osg::Vec3 b, float length) {
+  osg::Matrix modifierRotation = osg::Matrix::rotate(b.x() * M_PI / 180, osg::Vec3(1.0f, 0.0f, 0.0f), b.y() * M_PI / 180, osg::Vec3(0.0f, 1.0f, 0.0f), b.z() * M_PI / 180, osg::Vec3(0.0f, 0.0f, 1.0f));
+  osg::Vec3 newNormal = modifierRotation.preMult(a);
+  newNormal.normalize();
+  float angle = MeshTools::angleBetween(a, newNormal);
+  return tan(angle) * length;
+}

trackpoint-app/src/OptiTrackPoint.cpp

@@ -1,6 +1,9 @@
 // Include own headers
 #include "OptiTrackPoint.hpp"
 
+// Include modules
+#include "MeshTools.hpp"
+
 OptiTrackPoint::OptiTrackPoint(const osg::Vec3 point, const osg::Vec3 normal, const osg::Vec3 normalModifier, const float normalRotation, const bool compensation, const double length, const double radius): TrackPoint(point, normal, normalModifier, normalRotation, compensation) {
   _length = length;
   _radius = radius;
@@ -8,7 +11,7 @@ OptiTrackPoint::OptiTrackPoint(const osg::Vec3 point, const osg::Vec3 normal, co
 }
 
 double OptiTrackPoint::getLength() {
-  return _length;// + MeshTools::compensationLength(_normal, _normalModifier, _radius);
+  return _length;
 }
 
 double OptiTrackPoint::getRadius() {
@@ -29,3 +32,8 @@ void OptiTrackPoint::updateShift() {
   osg::Vec3 shift = _normal.operator*(_length);
   _trackOrigin = shift.operator+(_origin);
 }
+
+float OptiTrackPoint::getNormalCompensation() {
+  float compensationLength = MeshTools::compensationLength(_normal, _normalModifier, _radius);
+  return compensationLength > 0.0f ? compensationLength : 0.0f;
+}

trackpoint-app/src/PickHandler.cpp

@@ -3,6 +3,7 @@
 
 // Include modules
 #include "TrackPointRenderer.hpp"
+#include "MeshTools.hpp"
 
 // Include dependencies
 #include <osg/io_utils>
@@ -203,6 +204,29 @@ void PickHandler::rotateToNormalVector(osg::Vec3f normal) {
     }
     _shape->rotateToNormalVector(normal, normalRotation);
     _shape->setNormalModifier(modifier);
+    float objectWidth;
+    switch(activeTrackingSystem) {
+      case OptiTrack: {
+        OptiTrackSettings settings = MainWindow::getInstance()->getStore()->getOptiTrackSettings();
+        objectWidth = settings.radius;
+        break;
+      }
+      case EMFTrack: {
+        EMFTrackSettings settings = MainWindow::getInstance()->getStore()->getEMFTrackSettings();
+        objectWidth = std::sqrt(std::pow(settings.width / 2, 2) + std::pow(settings.height / 2, 2));
+        break;
+      }
+      case SteamVRTrack: {
+        objectWidth = STEAMVR_CONSTANT_RADIUS;
+        break;
+      }
+      case ActionPoints: {
+        objectWidth = 0.0f;
+        break;
+      }
+    }
+    float compensationLength = MeshTools::compensationLength(normal, modifier, objectWidth);
+    _shape->setCompensation(MainWindow::getInstance()->getStore()->getCompensation(), compensationLength);
   }
 }
 

trackpoint-app/src/PointShape.cpp

@@ -55,11 +55,19 @@ void PointShape::rotateToNormalVector(osg::Vec3f normal, float normalRotation) {
   matrix = matrix.operator*(osg::Matrix::rotate(normalRotation * M_PI / 180, normal));
   _selectionRotateGroup->setMatrix(matrix);
   if (_activeTrackingSystem == OptiTrack || _activeTrackingSystem == SteamVRTrack) {
-    osg::Vec3f movementVector = normal.operator*(_optiTrackSteamVRLength / 2);
+    float movementFixLength = _optiTrackSteamVRLength / 2;
+    if (_compensation) {
+      movementFixLength -= _compensationLength / 2;
+    }
+    osg::Vec3f movementVector = normal.operator*(movementFixLength);
     _selectionMoveToEndGroup->setMatrix(osg::Matrix::translate(movementVector));
   }
   if (_activeTrackingSystem == SteamVRTrack) {
-    osg::Vec3f movementVector = osg::Vec3f(0.0f, 0.0f, 1.0f).operator*(_optiTrackSteamVRLength / 2);
+    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));
   }
 }
@@ -83,11 +91,17 @@ void PointShape::setupOptiTrack(OptiTrackSettings optiTrackSettings) {
     _optiTrackSteamVRLength = optiTrackSettings.length;
     _geode = new osg::Geode;
     _shape = new osg::ShapeDrawable;
-    _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, -2.5f), optiTrackSettings.radius, optiTrackSettings.length - 5.0f));
+    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->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, optiTrackSettings.length - 7.5f), 0.74f, 5.0f));
+    _optiConnector->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, smallShapePosition), 0.74f, 5.0f));
     _optiConnector->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_optiConnector.get());
     OSGWidget::fixMaterialState(_geode);
@@ -99,7 +113,11 @@ void PointShape::setupEMFTrack(EMFTrackSettings emfTrackSettings) {
   if (_activeTrackingSystem == EMFTrack) {
     _geode = new osg::Geode;
     _shape = new osg::ShapeDrawable;
-    _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)));
+    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->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_shape.get());
     OSGWidget::fixMaterialState(_geode);
@@ -112,7 +130,11 @@ void PointShape::setupSteamVRTrack(SteamVRTrackSettings steamVrTrackSettings) {
     _optiTrackSteamVRLength = steamVrTrackSettings.length;
     _geode = new osg::Geode;
     _shape = new osg::ShapeDrawable;
-    _shape->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, 0.0f), STEAMVR_CONSTANT_RADIUS, steamVrTrackSettings.length));
+    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->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 0.2f));
     _geode->addDrawable(_shape.get());
 
@@ -139,6 +161,11 @@ void PointShape::setupActionPoints() {
   }
 }
 
+void PointShape::setCompensation(bool compensation, float compensationLength) {
+  _compensation = compensation;
+  _compensationLength = compensationLength;
+}
+
 osg::ref_ptr<osg::Geode> PointShape::getMesh() {
   return _geode;
 }

trackpoint-app/src/SteamVRTrackPoint.cpp

@@ -1,6 +1,9 @@
 // Include own headers
 #include "SteamVRTrackPoint.hpp"
 
+// Include modules
+#include "MeshTools.hpp"
+
 SteamVRTrackPoint::SteamVRTrackPoint(const osg::Vec3 point, const osg::Vec3 normal, const osg::Vec3 normalModifier, const float normalRotation, const bool compensation, const double length): TrackPoint(point, normal, normalModifier, normalRotation, compensation) {
   _length = length;
   updateShift();
@@ -23,3 +26,8 @@ void SteamVRTrackPoint::updateShift() {
   osg::Vec3 shift = _normal.operator*(_length + STEAMVR_THREAD_LENGTH + STEAMVR_ORIGIN_OFFSET);
   _trackOrigin = shift.operator+(_origin);
 }
+
+float SteamVRTrackPoint::getNormalCompensation() {
+  float compensationLength = MeshTools::compensationLength(_normal, _normalModifier, STEAMVR_CONSTANT_RADIUS);
+  return compensationLength > 0.0f ? compensationLength : 0.0f;
+}

trackpoint-app/src/TrackPointRenderer.cpp

@@ -21,6 +21,7 @@ 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());
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
@@ -32,6 +33,7 @@ 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());
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
@@ -43,6 +45,7 @@ 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());
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;
@@ -54,6 +57,7 @@ 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();
         commonSetupPointShape(newShape, static_cast<TrackPoint*>(point), id);
         id++;