Implement Steam VR Track collision check, allow check disabling

trackpoint-app/gui/EditWidget.ui

@@ -40,7 +40,7 @@
         <x>0</x>
         <y>0</y>
         <width>506</width>
-        <height>1035</height>
+        <height>1062</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_7">
@@ -573,6 +573,16 @@
                 </property>
                </widget>
               </item>
+              <item>
+               <widget class="QCheckBox" name="optiTrackEnableSanityCheck">
+                <property name="text">
+                 <string>Perform automatic sanity check</string>
+                </property>
+                <property name="checked">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
              </layout>
             </widget>
            </item>
@@ -749,12 +759,19 @@
                </widget>
               </item>
               <item>
-               <widget class="QPushButton" name="pushButton_2">
+               <widget class="QPushButton" name="steamVrTrackCollisionCheck">
                 <property name="text">
                  <string>Perform collision check</string>
                 </property>
                </widget>
               </item>
+              <item>
+               <widget class="QCheckBox" name="steamVrTrackEnableCollisionCheck">
+                <property name="text">
+                 <string>Perform automatic collision check</string>
+                </property>
+               </widget>
+              </item>
              </layout>
             </widget>
            </item>

trackpoint-app/include/EditWidget.hpp

@@ -28,6 +28,8 @@ public:
   void resetAllSettings();
   void setExportAvailable(bool available);
   void setExportStatus(int jobs, int done);
+  bool getOptiTrackSanityCheckStatus();
+  bool getSteamVRTrackCollisionCheckStatus();
 
 protected:
   virtual void showEvent(QShowEvent* event);
@@ -55,8 +57,13 @@ private slots:
   void changePositions();
   void setPositionEditing(bool mode);
   void manualOptiTrackSanityCheck();
+  void setOptiTrackSanityCheckStatus();
+  void manualSteamVRTrackCollisionCheck();
+  void setSteamVRTrackCollisionCheckStatus();
 
 private:
   Ui::EditWidget* ui;
   int selectedPoint = -1;
+  bool _optiTrackSanityCheck = true;
+  bool _steamVrTrackCollisionCheck = false;
 };

trackpoint-app/include/MainWindow.hpp

@@ -30,6 +30,8 @@ public:
   void showOptiTrackSanityLineError();
   void showOptiTrackSanityPlaneError();
   void showOptiTrackSanitySuccess();
+  void showSteamVRTrackCollisionCheckError();
+  void showSteamVRTrackCollisionCheckSuccess();
 
 protected:
   virtual void closeEvent(QCloseEvent *event);

trackpoint-app/include/MeshTools.hpp

@@ -2,6 +2,7 @@
 
 // Include modules
 #include "OptiTrackPoint.hpp"
+#include "SteamVRTrackPoint.hpp"
 
 // Include dependencies
 #include "lib3mf_implicit.hpp"
@@ -13,4 +14,5 @@ public:
   static float angleBetween(osg::Vec3 a, osg::Vec3 b);
   static float compensationLength(osg::Vec3 a, osg::Vec3 b, float length);
   static bool optiTrackSanityCheck(const std::vector<OptiTrackPoint*> points, const bool showSuccessMessage);
+  static bool steamVrTrackCollisionCheck(std::vector<SteamVRTrackPoint*> points, const bool showSuccessMessage, osg::ref_ptr<osg::Group> verifyGroup);
 };

trackpoint-app/include/OSGWidget.hpp

@@ -34,11 +34,14 @@ public:
   virtual ~OSGWidget();
   void renderBaseMesh(const osg::ref_ptr<osg::Vec3Array> vertices, const osg::ref_ptr<osg::Vec3Array> normals);
   osg::ref_ptr<osg::Geode> getMesh();
+  osg::ref_ptr<osg::Group> getVerifyGroup();
   PickHandler* getPicker();
   TrackPointRenderer* getPointRenderer();
   void loadSteamvrThread();
+  void loadSteamvrCollision();
   void clear();
   osg::ref_ptr<osg::Geometry> _steamvrThreadMesh;
+  osg::ref_ptr<osg::Geometry> _steamvrTrackerMesh;
 
 protected:
   virtual void paintEvent(QPaintEvent* paintEvent);
@@ -70,6 +73,7 @@ private:
   osgViewer::View* _view;
   osg::ref_ptr<osg::Group> _root;
   osg::ref_ptr<osg::Group> _pointRoot;
+  osg::ref_ptr<osg::Group> _verifyGroup;
   osg::ref_ptr<osg::Group> _coordinateAxes;
   osg::ref_ptr<osg::Geode> _mesh;
 
@@ -80,4 +84,5 @@ private:
   bool selectionFinished_;
 
   bool _steamvrLoaded = false;
+  bool _steamvrCollisionLoaded = false;
 };

trackpoint-app/resources/generate.py

@@ -2,7 +2,7 @@
 
 from stl import mesh
 
-files = ["steamvrthread.stl"]
+files = ["steamvrthread.stl", "steamvrtracker.stl"]
 scadFiles = ["threads.scad"]
 
 fileData = {}

trackpoint-app/resources/legal-info.txt

@@ -0,0 +1 @@
+HTC Vive Tracker model by Sketchfab user "Marco_Romero" (https://sketchfab.com/3d-models/htc-vive-tracker-4bcb460ac22248f7abf4beeacae954e3), licensed under CC Attribution (https://creativecommons.org/licenses/by/4.0/)

trackpoint-app/src/EditWidget.cpp

@@ -37,6 +37,7 @@ EditWidget::EditWidget(QWidget* parent): QWidget(parent), ui(new Ui::EditWidget)
   QObject::connect(ui->optiTrackRadius, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateOptiTrackSettings(false); });
   QObject::connect(ui->optiTrackLoadDefaults, &QPushButton::clicked, this, [=](){ this->updateOptiTrackSettings(true); });
   QObject::connect(ui->optiTrackSanityCheck, &QPushButton::clicked, this, &EditWidget::manualOptiTrackSanityCheck);
+  QObject::connect(ui->optiTrackEnableSanityCheck, &QCheckBox::stateChanged, this, &EditWidget::setOptiTrackSanityCheckStatus);
   // EMF Track settings
   QObject::connect(ui->emfTrackWidth, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateEMFTrackSettings(false); });
   QObject::connect(ui->emfTrackHeight, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateEMFTrackSettings(false); });
@@ -45,6 +46,8 @@ EditWidget::EditWidget(QWidget* parent): QWidget(parent), ui(new Ui::EditWidget)
   // StramVRTrack settings
   QObject::connect(ui->steamVrTrackLength, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateSteamVRTrackSettings(false); });
   QObject::connect(ui->steamVrTrackLoadDefaults, &QPushButton::clicked, this, [=](){ this->updateSteamVRTrackSettings(true); });
+  QObject::connect(ui->steamVrTrackCollisionCheck, &QPushButton::clicked, this, &EditWidget::manualSteamVRTrackCollisionCheck);
+  QObject::connect(ui->steamVrTrackEnableCollisionCheck, &QCheckBox::stateChanged, this, &EditWidget::setSteamVRTrackCollisionCheckStatus);
   // Action point settings
   QObject::connect(ui->actionPointIdentifier, &QLineEdit::textChanged, this, &EditWidget::updateActionPointSettings);
   QObject::connect(ui->actionPointLoadDefaults, &QPushButton::clicked, this, &EditWidget::resetActionPointSettings);
@@ -199,6 +202,14 @@ void EditWidget::setExportStatus(int jobs, int done) {
   ui->exportLabel->setText(QString::fromUtf8(text.str().c_str()));
 }
 
+bool EditWidget::getOptiTrackSanityCheckStatus() {
+  return _optiTrackSanityCheck;
+}
+
+bool EditWidget::getSteamVRTrackCollisionCheckStatus() {
+  return _steamVrTrackCollisionCheck;
+}
+
 void EditWidget::selectTool(Tool tool) {
   switch(tool) {
     case InsertionTool: {
@@ -241,6 +252,8 @@ void EditWidget::updateNormalModifier() {
     MainWindow::getInstance()->getStore()->projectModified();
     if (activeTrackingSystem == OptiTrack) {
       MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
+    } else if (activeTrackingSystem == SteamVRTrack) {
+      MeshTools::steamVrTrackCollisionCheck(MainWindow::getInstance()->getStore()->getSteamVRTrackPoints(), false, MainWindow::getInstance()->getOsgWidget()->getVerifyGroup());
     }
   }
 }
@@ -260,6 +273,8 @@ void EditWidget::resetNormalModifier() {
     MainWindow::getInstance()->getStore()->projectModified();
     if (activeTrackingSystem == OptiTrack) {
       MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
+    } else if (activeTrackingSystem == SteamVRTrack) {
+      MeshTools::steamVrTrackCollisionCheck(MainWindow::getInstance()->getStore()->getSteamVRTrackPoints(), false, MainWindow::getInstance()->getOsgWidget()->getVerifyGroup());
     }
   }
 }
@@ -287,6 +302,8 @@ void EditWidget::updateNormalRotation(bool reset) {
     MainWindow::getInstance()->getStore()->projectModified();
     if (activeTrackingSystem == OptiTrack) {
       MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
+    } else if (activeTrackingSystem == SteamVRTrack) {
+      MeshTools::steamVrTrackCollisionCheck(MainWindow::getInstance()->getStore()->getSteamVRTrackPoints(), false, MainWindow::getInstance()->getOsgWidget()->getVerifyGroup());
     }
   }
 }
@@ -384,6 +401,7 @@ void EditWidget::updateSteamVRTrackSettings(bool reset) {
     MainWindow::getInstance()->getStore()->getSteamVRTrackPoints()[selectedPoint]->updateSteamVRTrackSettings(settings);
     MainWindow::getInstance()->getOsgWidget()->getPointRenderer()->render(SteamVRTrack);
     MainWindow::getInstance()->getStore()->projectModified();
+    MeshTools::steamVrTrackCollisionCheck(MainWindow::getInstance()->getStore()->getSteamVRTrackPoints(), false, MainWindow::getInstance()->getOsgWidget()->getVerifyGroup());
   }
 }
 
@@ -496,3 +514,15 @@ void EditWidget::setPositionEditing(bool mode) {
 void EditWidget::manualOptiTrackSanityCheck() {
   MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), true);
 }
+
+void EditWidget::setOptiTrackSanityCheckStatus() {
+  _optiTrackSanityCheck = ui->compensation->checkState() == Qt::Checked ? true : false;
+}
+
+void EditWidget::manualSteamVRTrackCollisionCheck() {
+  MeshTools::steamVrTrackCollisionCheck(MainWindow::getInstance()->getStore()->getSteamVRTrackPoints(), true, MainWindow::getInstance()->getOsgWidget()->getVerifyGroup());
+}
+
+void EditWidget::setSteamVRTrackCollisionCheckStatus() {
+  _steamVrTrackCollisionCheck = ui->compensation->checkState() == Qt::Checked ? true : false;
+}

trackpoint-app/src/MainWindow.cpp

@@ -235,3 +235,17 @@ void MainWindow::showOptiTrackSanitySuccess() {
   msg.setWindowTitle(QString("OptiTrack Sanity Check Passed"));
   msg.exec();
 }
+
+void MainWindow::showSteamVRTrackCollisionCheckError() {
+  QMessageBox msg(this);
+  msg.setText(QString("There is at least one track point where the SteamVR tracking device might collide with the printed object."));
+  msg.setWindowTitle(QString("SteamVR Track Collition Check found a problem"));
+  msg.exec();
+}
+
+void MainWindow::showSteamVRTrackCollisionCheckSuccess() {
+  QMessageBox msg(this);
+  msg.setText(QString("All tracking points are free of collisions!"));
+  msg.setWindowTitle(QString("SteamVR Track Collision Check Passed"));
+  msg.exec();
+}

trackpoint-app/src/MeshTools.cpp

@@ -6,6 +6,7 @@
 
 // Include dependencies
 #include <math.h>
+#include <osg/ComputeBoundsVisitor>
 
 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) {
@@ -43,6 +44,9 @@ float MeshTools::compensationLength(osg::Vec3 a, osg::Vec3 b, float length) {
 }
 
 bool MeshTools::optiTrackSanityCheck(const std::vector<OptiTrackPoint*> points, const bool showSuccessMessage) {
+  if (!MainWindow::getInstance()->getEditWiget()->getOptiTrackSanityCheckStatus()) {
+    return false;
+  }
   // Check for three on a line
   bool foundLineProblem = false;
   if (points.size() >= 3) {
@@ -99,3 +103,54 @@ bool MeshTools::optiTrackSanityCheck(const std::vector<OptiTrackPoint*> points,
   }
   return foundLineProblem || foundPlaneProblem;
 }
+
+bool MeshTools::steamVrTrackCollisionCheck(std::vector<SteamVRTrackPoint*> points, const bool showSuccessMessage, osg::ref_ptr<osg::Group> verifyGroup) {
+  if (!MainWindow::getInstance()->getEditWiget()->getSteamVRTrackCollisionCheckStatus()) {
+    return false;
+  }
+  MainWindow::getInstance()->getOsgWidget()->loadSteamvrCollision();
+  bool foundProblem = false;
+  for (SteamVRTrackPoint* point: points) {
+    osg::ref_ptr<osg::MatrixTransform> move = new osg::MatrixTransform;
+    osg::Vec3f movementVector = osg::Vec3f(0.0f, 0.0f, 1.0f).operator*(point->getLength());
+    move->setMatrix(osg::Matrix::translate(movementVector));
+
+    osg::ref_ptr<osg::MatrixTransform> rotate = new osg::MatrixTransform;
+    rotate->addChild(move.get());
+    osg::Vec3f normalModifier = point->getNormalModifier();
+    osg::Vec3f normal = point->getNormal();
+    float normalRotation = point->getNormalRotation();
+    osg::Matrix modifierRotation = osg::Matrix::rotate(normalModifier.x() * M_PI / 180, osg::Vec3(1.0f, 0.0f, 0.0f), normalModifier.y() * M_PI / 180, osg::Vec3(0.0f, 1.0f, 0.0f), normalModifier.z() * M_PI / 180, osg::Vec3(0.0f, 0.0f, 1.0f));
+    normal = modifierRotation.preMult(normal);
+    normal.normalize();
+    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));
+    rotate->setMatrix(matrix);
+
+    osg::ref_ptr<osg::MatrixTransform> translate = new osg::MatrixTransform;
+    translate->addChild(rotate.get());
+    translate->setMatrix(osg::Matrix::translate(point->getTranslation()));
+    verifyGroup->addChild(translate.get());
+
+    osg::ref_ptr<osg::Geode> tracker = new osg::Geode;
+    tracker->addDrawable(MainWindow::getInstance()->getOsgWidget()->_steamvrTrackerMesh.get());
+    move->addChild(tracker.get());
+
+    osg::ComputeBoundsVisitor computeBoundsVisitor;
+    verifyGroup->accept(computeBoundsVisitor);
+    osg::BoundingBox trackerBox = computeBoundsVisitor.getBoundingBox();
+
+    if (trackerBox.intersects(MainWindow::getInstance()->getOsgWidget()->getMesh()->getBoundingBox())) {
+      foundProblem = true;
+    }
+
+    verifyGroup->removeChild(translate.get());
+  }
+  if (foundProblem) {
+    MainWindow::getInstance()->showSteamVRTrackCollisionCheckError();
+  }
+  if (!foundProblem && showSuccessMessage) {
+    MainWindow::getInstance()->showSteamVRTrackCollisionCheckSuccess();
+  }
+  return foundProblem;
+}

trackpoint-app/src/OSGWidget.cpp

@@ -180,6 +180,9 @@ OSGWidget::OSGWidget(QWidget* parent): QOpenGLWidget(parent),
   // graphics window switch viewports properly.
   this->setMouseTracking(true);
 
+  _verifyGroup = new osg::Group;
+  _root->addChild(_verifyGroup.get());
+
   _pointRoot = new osg::Group;
   _root->addChild(_pointRoot.get());
   _pointRenderer = new TrackPointRenderer(this, _pointRoot);
@@ -210,6 +213,10 @@ osg::ref_ptr<osg::Geode> OSGWidget::getMesh() {
   return _mesh;
 }
 
+osg::ref_ptr<osg::Group> OSGWidget::getVerifyGroup() {
+  return _verifyGroup;
+}
+
 PickHandler* OSGWidget::getPicker() {
   return _picker;
 }
@@ -244,6 +251,32 @@ void OSGWidget::loadSteamvrThread() {
   }
 }
 
+void OSGWidget::loadSteamvrCollision() {
+  if (!_steamvrCollisionLoaded) {
+    std::vector<Lib3MF::sPosition> verticesBuffer;
+    std::vector<Lib3MF::sTriangle> triangleBuffer;
+    for (unsigned int i = 0; i < sizeof(steamvrtracker_VERTICES) / sizeof(float); i += 3) {
+      Lib3MF::sPosition vertex = {steamvrtracker_VERTICES[i], steamvrtracker_VERTICES[i + 1], steamvrtracker_VERTICES[i + 2]};
+      verticesBuffer.push_back(vertex);
+    }
+    for (unsigned int i = 0; i < sizeof(steamvrtracker_TRIANGLES) / sizeof(unsigned int); i += 3) {
+      Lib3MF::sTriangle triangle = {steamvrtracker_TRIANGLES[i], steamvrtracker_TRIANGLES[i + 1], steamvrtracker_TRIANGLES[i + 2]};
+      triangleBuffer.push_back(triangle);
+    }
+    osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
+    osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array;
+    MeshTools::calculateNormals(verticesBuffer, triangleBuffer, vertices, normals);
+
+    _steamvrTrackerMesh = new osg::Geometry;
+    _steamvrTrackerMesh->setVertexArray(vertices.get());
+    _steamvrTrackerMesh->setNormalArray(normals.get(), osg::Array::BIND_PER_VERTEX);
+    _steamvrTrackerMesh->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLES, 0, vertices->getNumElements()));
+    osgUtil::optimizeMesh(_steamvrTrackerMesh.get());
+
+    _steamvrCollisionLoaded = true;
+  }
+}
+
 void OSGWidget::clear() {
   _root->removeChild(_mesh);
 }

trackpoint-app/src/ProjectStore.cpp

@@ -183,6 +183,7 @@ void ProjectStore::addTrackPoint(osg::Vec3 point, osg::Vec3 normal, ActiveTracki
     case SteamVRTrack: {
       SteamVRTrackPoint* steamVrTrackPoint = new SteamVRTrackPoint(point, normal, _normalModifier, _normalRotation, _compensation, _steamVrTrackSettings.length);
       _steamVrTrackPoints.push_back(steamVrTrackPoint);
+      MeshTools::steamVrTrackCollisionCheck(_steamVrTrackPoints, false, MainWindow::getInstance()->getOsgWidget()->getVerifyGroup());
       break;
     }
     case ActionPoints: {