Implement OptiTrack sanity check (Closes #21)

trackpoint-app/gui/EditWidget.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>508</width>
-    <height>1040</height>
+    <height>912</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -40,7 +40,7 @@
         <x>0</x>
         <y>0</y>
         <width>506</width>
-        <height>1038</height>
+        <height>1035</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_7">
@@ -566,6 +566,13 @@
                 </property>
                </widget>
               </item>
+              <item>
+               <widget class="QPushButton" name="optiTrackSanityCheck">
+                <property name="text">
+                 <string>Perform sanity check</string>
+                </property>
+               </widget>
+              </item>
              </layout>
             </widget>
            </item>
@@ -741,6 +748,13 @@
                 </property>
                </widget>
               </item>
+              <item>
+               <widget class="QPushButton" name="pushButton_2">
+                <property name="text">
+                 <string>Perform collision check</string>
+                </property>
+               </widget>
+              </item>
              </layout>
             </widget>
            </item>

trackpoint-app/include/EditWidget.hpp

@@ -54,6 +54,7 @@ private slots:
   void exportProject();
   void changePositions();
   void setPositionEditing(bool mode);
+  void manualOptiTrackSanityCheck();
 
 private:
   Ui::EditWidget* ui;

trackpoint-app/include/MainWindow.hpp

@@ -27,6 +27,9 @@ public:
   ProjectStore* getStore();
   EditWidget* getEditWiget();
   void showErrorMessage(std::string message, std::string title = "Error");
+  void showOptiTrackSanityLineError();
+  void showOptiTrackSanityPlaneError();
+  void showOptiTrackSanitySuccess();
 
 protected:
   virtual void closeEvent(QCloseEvent *event);

trackpoint-app/include/MeshTools.hpp

@@ -1,5 +1,8 @@
 #pragma once
 
+// Include modules
+#include "OptiTrackPoint.hpp"
+
 // Include dependencies
 #include "lib3mf_implicit.hpp"
 #include <osg/Array>
@@ -9,4 +12,5 @@ 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);
+  static bool optiTrackSanityCheck(const std::vector<OptiTrackPoint*> points, const bool showSuccessMessage);
 };

trackpoint-app/include/defaults.hpp

@@ -4,6 +4,7 @@
 
 #define OPTITRACK_DEFAULT_LENGTH 10.0
 #define OPTITRACK_DEFAULT_RADIUS 1.5
+#define OPTITRACK_SANITY_DISTANCE_THRESHOLD 5.0
 
 #define EMFTRACK_DEFAULT_WIDTH 50.0
 #define EMFTRACK_DEFAULT_HEIGHT 10.0

trackpoint-app/src/EditWidget.cpp

@@ -7,6 +7,7 @@
 #include "PickHandler.hpp"
 #include "TrackPointRenderer.hpp"
 #include "OpenScadRenderer.hpp"
+#include "MeshTools.hpp"
 
 // Include dependencies
 #include <sstream>
@@ -35,6 +36,7 @@ EditWidget::EditWidget(QWidget* parent): QWidget(parent), ui(new Ui::EditWidget)
   QObject::connect(ui->optiTrackLength, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateOptiTrackSettings(false); });
   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);
   // EMF Track settings
   QObject::connect(ui->emfTrackWidth, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateEMFTrackSettings(false); });
   QObject::connect(ui->emfTrackHeight, &QDoubleSpinBox::valueChanged, this, [=](){ this->updateEMFTrackSettings(false); });
@@ -237,6 +239,9 @@ void EditWidget::updateNormalModifier() {
     MainWindow::getInstance()->getStore()->getTrackPointById(selectedPoint, activeTrackingSystem)->updateNormalModifier(modifier);
     MainWindow::getInstance()->getOsgWidget()->getPointRenderer()->render(activeTrackingSystem);
     MainWindow::getInstance()->getStore()->projectModified();
+    if (activeTrackingSystem == OptiTrack) {
+      MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
+    }
   }
 }
 
@@ -253,6 +258,9 @@ void EditWidget::resetNormalModifier() {
     MainWindow::getInstance()->getStore()->getTrackPointById(selectedPoint, activeTrackingSystem)->updateNormalModifier(modifier);
     MainWindow::getInstance()->getOsgWidget()->getPointRenderer()->render(activeTrackingSystem);
     MainWindow::getInstance()->getStore()->projectModified();
+    if (activeTrackingSystem == OptiTrack) {
+      MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
+    }
   }
 }
 
@@ -277,6 +285,9 @@ void EditWidget::updateNormalRotation(bool reset) {
     MainWindow::getInstance()->getStore()->getTrackPointById(selectedPoint, activeTrackingSystem)->updateNormalRotation(normalRotation);
     MainWindow::getInstance()->getOsgWidget()->getPointRenderer()->render(activeTrackingSystem);
     MainWindow::getInstance()->getStore()->projectModified();
+    if (activeTrackingSystem == OptiTrack) {
+      MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
+    }
   }
 }
 
@@ -325,6 +336,7 @@ void EditWidget::updateOptiTrackSettings(bool reset) {
     MainWindow::getInstance()->getStore()->getOptiTrackPoints()[selectedPoint]->updateOptiTrackSettings(settings);
     MainWindow::getInstance()->getOsgWidget()->getPointRenderer()->render(OptiTrack);
     MainWindow::getInstance()->getStore()->projectModified();
+    MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), false);
   }
 }
 
@@ -480,3 +492,7 @@ void EditWidget::setPositionEditing(bool mode) {
   ui->anchorY->setReadOnly(mode);
   ui->anchorZ->setReadOnly(mode);
 }
+
+void EditWidget::manualOptiTrackSanityCheck() {
+  MeshTools::optiTrackSanityCheck(MainWindow::getInstance()->getStore()->getOptiTrackPoints(), true);
+}

trackpoint-app/src/MainWindow.cpp

@@ -214,3 +214,24 @@ bool MainWindow::isRenderingPopup() {
       return false;
   }
 }
+
+void MainWindow::showOptiTrackSanityLineError() {
+  QMessageBox msg(this);
+  msg.setText(QString("There is at least one track point in a line with two others, which should be avoided."));
+  msg.setWindowTitle(QString("OptiTrack Sanity Check found a problem"));
+  msg.exec();
+}
+
+void MainWindow::showOptiTrackSanityPlaneError() {
+  QMessageBox msg(this);
+  msg.setText(QString("There is at least one track point in a plane with three others, which should be avoided."));
+  msg.setWindowTitle(QString("OptiTrack Sanity Check found a problem"));
+  msg.exec();
+}
+
+void MainWindow::showOptiTrackSanitySuccess() {
+  QMessageBox msg(this);
+  msg.setText(QString("All tracking points are valid!"));
+  msg.setWindowTitle(QString("OptiTrack Sanity Check Passed"));
+  msg.exec();
+}

trackpoint-app/src/MeshTools.cpp

@@ -1,6 +1,9 @@
 // Include own header
 #include "MeshTools.hpp"
 
+// Include modules
+#include "MainWindow.hpp"
+
 // Include dependencies
 #include <math.h>
 
@@ -38,3 +41,61 @@ float MeshTools::compensationLength(osg::Vec3 a, osg::Vec3 b, float length) {
   float angle = MeshTools::angleBetween(a, newNormal);
   return isnan(tan(angle) * length) ? 0.0f : tan(angle) * length;
 }
+
+bool MeshTools::optiTrackSanityCheck(const std::vector<OptiTrackPoint*> points, const bool showSuccessMessage) {
+  // Check for three on a line
+  bool foundLineProblem = false;
+  if (points.size() >= 3) {
+    for (int i = 0; i < (points.size() - 1); i++) {
+      for (int j = i + 1; j < points.size(); j++) {
+        for (int k = 0; k < points.size(); k++) {
+          if (k != i && k != j) {
+            osg::Vec3 a = points[i]->getTrackPoint();
+            osg::Vec3 b = points[j]->getTrackPoint().operator-(a);
+            osg::Vec3 point = points[k]->getTrackPoint();
+            osg::Vec3 pMinusA = point.operator-(a);
+            osg::Vec3 crossProduct = pMinusA.operator^(b);
+            float distance = crossProduct.length() / b.length();
+            if (distance < OPTITRACK_SANITY_DISTANCE_THRESHOLD) {
+              foundLineProblem = true;
+            }
+          }
+        }
+      }
+    }
+  }
+  // Check for four on a plane
+  bool foundPlaneProblem = false;
+  if (points.size() >= 4) {
+    for (int i = 0; i < (points.size() - 2); i++) {
+      for (int j = i + 1; j < (points.size() - 1); j++) {
+        for (int k = j + 1; k < points.size(); k++) {
+          for (int l = 0; l < points.size(); l++) {
+            if (l != i && l != j && l != k) {
+              osg::Vec3 a = points[i]->getTrackPoint();
+              osg::Vec3 b = points[j]->getTrackPoint();
+              osg::Vec3 c = points[k]->getTrackPoint();
+              osg::Vec3 point = points[l]->getTrackPoint();
+              osg::Plane* plane = new osg::Plane(a, b, c);
+              float distance = std::abs(plane->distance(point));
+              delete plane;
+              if (distance < OPTITRACK_SANITY_DISTANCE_THRESHOLD) {
+                foundPlaneProblem = true;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+  if (foundLineProblem) {
+    MainWindow::getInstance()->showOptiTrackSanityLineError();
+  }
+  if (foundPlaneProblem) {
+    MainWindow::getInstance()->showOptiTrackSanityPlaneError();
+  }
+  if (!foundLineProblem && !foundPlaneProblem && showSuccessMessage) {
+    MainWindow::getInstance()->showOptiTrackSanitySuccess();
+  }
+  return foundLineProblem || foundPlaneProblem;
+}

trackpoint-app/src/ProjectStore.cpp

@@ -171,6 +171,7 @@ void ProjectStore::addTrackPoint(osg::Vec3 point, osg::Vec3 normal, ActiveTracki
     case OptiTrack: {
       OptiTrackPoint* optiTrackPoint = new OptiTrackPoint(point, normal, _normalModifier, _normalRotation, _compensation, _optiTrackSettings.length, _optiTrackSettings.radius);
       _optiTrackPoints.push_back(optiTrackPoint);
+      MeshTools::optiTrackSanityCheck(_optiTrackPoints, false);
       break;
     }
     case EMFTrack: {