Merged in fixes to hydra and rift code, along with a bug in the pitch inversion of FTNoIR itself.

Also added an include directory the Rift needs which was absent before.

10 files changed, 162 insertions, 137 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e7a3b9de..79d7903c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -453,7 +453,8 @@ endif()
     endif()
 	
 	if(SDK_RIFT)
-		include_directories("${SDK_RIFT}/include")
+        include_directories("${SDK_RIFT}/include")
+        include_directories("${SDK_RIFT}/Src")
         add_library(opentrack-tracker-rift SHARED ${opentrack-tracker-rift-c} ${opentrack-tracker-rift-moc} ${opentrack-tracker-rift-uih} ${opentrack-tracker-rift-rcc})
         target_link_libraries(opentrack-tracker-rift ${MY_QT_LIBS})
 		if(WIN32)
diff --git a/facetracknoir/facetracknoir.cpp b/facetracknoir/facetracknoir.cpp
index fafef908..623b16b2 100644
--- a/facetracknoir/facetracknoir.cpp
+++ b/facetracknoir/facetracknoir.cpp
@@ -682,9 +682,8 @@ void FaceTrackNoIR::startTracker( ) {
 	// This is necessary, because the events are only triggered 'on change'

 	//

     tracker->setInvertAxis(Yaw, ui.chkInvertYaw->isChecked() );

-    tracker->setInvertAxis(TY, ui.chkInvertPitch->isChecked() );

-    tracker->setInvertAxis(Roll, ui.chkInvertRoll->isChecked() );

-    tracker->setInvertAxis(TX, ui.chkInvertX->isChecked() );

+    tracker->setInvertAxis(Pitch, ui.chkInvertPitch->isChecked() );

+    tracker->setInvertAxis(Roll, ui.chkInvertRoll->isChecked() );    tracker->setInvertAxis(TX, ui.chkInvertX->isChecked() );

     tracker->setInvertAxis(TY, ui.chkInvertY->isChecked() );

     tracker->setInvertAxis(TZ, ui.chkInvertZ->isChecked() );

 

@@ -849,6 +848,7 @@ void FaceTrackNoIR::showHeadPose() {
     ui.lcdNumY->display(QString("%1").arg(newdata[TY], 0, 'f', 1));

     ui.lcdNumZ->display(QString("%1").arg(newdata[TZ], 0, 'f', 1));

 

+

     ui.lcdNumRotX->display(QString("%1").arg(newdata[Yaw], 0, 'f', 1));

     ui.lcdNumRotY->display(QString("%1").arg(newdata[Pitch], 0, 'f', 1));

     ui.lcdNumRotZ->display(QString("%1").arg(newdata[Roll], 0, 'f', 1));

@@ -859,12 +859,14 @@ void FaceTrackNoIR::showHeadPose() {
 	// Get the output-pose and also display it.

 	//

     tracker->getOutputHeadPose(newdata);

+

     ui.pose_display->rotateBy(newdata[Yaw], newdata[Roll], newdata[Pitch]);

 

     ui.lcdNumOutputPosX->display(QString("%1").arg(newdata[TX], 0, 'f', 1));

     ui.lcdNumOutputPosY->display(QString("%1").arg(newdata[TY], 0, 'f', 1));

     ui.lcdNumOutputPosZ->display(QString("%1").arg(newdata[TZ], 0, 'f', 1));

 

+

     ui.lcdNumOutputRotX->display(QString("%1").arg(newdata[Yaw], 0, 'f', 1));

     ui.lcdNumOutputRotY->display(QString("%1").arg(newdata[Pitch], 0, 'f', 1));

     ui.lcdNumOutputRotZ->display(QString("%1").arg(newdata[Roll], 0, 'f', 1));

diff --git a/facetracknoir/facetracknoir.h b/facetracknoir/facetracknoir.h
index fd9c06a7..a6d99165 100644
--- a/facetracknoir/facetracknoir.h
+++ b/facetracknoir/facetracknoir.h
@@ -186,8 +186,7 @@ private:
 

         void setInvertAxis( Axis axis, int invert );

         void setInvertYaw(int invert) {

-            setInvertAxis(Yaw, invert);

-        }

+            setInvertAxis(Yaw, invert);        }

         void setInvertPitch(int invert) {

             setInvertAxis(Pitch, invert);

         }        

diff --git a/facetracknoir/rotation.h b/facetracknoir/rotation.h
index a1500969..1e472cf2 100644
--- a/facetracknoir/rotation.h
+++ b/facetracknoir/rotation.h
@@ -12,12 +12,12 @@
 class Rotation {

 

 public:

-	Rotation() : a(1.0),b(0.0),c(0.0),d(0.0) {}

+	Rotation() : w(1.0),x(0.0),y(0.0),z(0.0) {}

 	Rotation(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); }

-	Rotation(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}

+	Rotation(double x, double y, double z, double w) : x(x),y(y),z(z),w(w) {}

 

 	Rotation inv(){	// inverse

-		return Rotation(a,-b,-c,-d);

+		return Rotation(-x,-y,-z, w);

 	}

 

 

@@ -25,46 +25,48 @@ public:
 	// see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

 	void fromEuler(double yaw, double pitch, double roll)

 	{

-		double sin_phi = sin(roll/2.0);

-		double cos_phi = cos(roll/2.0);

-		double sin_the = sin(pitch/2.0);

-		double cos_the = cos(pitch/2.0);

-		double sin_psi = sin(yaw/2.0);

-		double cos_psi = cos(yaw/2.0);

-

-		a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;

-		b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;

-		c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;

-		d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;

+		// Assuming the angles are in radians.

+		double c1 = cos(yaw);

+		double s1 = sin(yaw);

+		double c2 = cos(roll);

+		double s2 = sin(roll);

+		double c3 = cos(pitch);

+		double s3 = sin(pitch);

+		w = sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;

+		double w4 = (4.0 * w);

+		x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;

+		y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;

+		z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;

 	}

 

 	void toEuler(double& yaw, double& pitch, double& roll)

 	{

-		roll = atan2(2.0*(a*b + c*d), 1.0 - 2.0*(b*b + c*c));

-		pitch = asin(2.0*(a*c - b*d));

-		yaw =  atan2(2.0*(a*d + b*c), 1.0 - 2.0*(c*c + d*d));

+		

+		yaw =  atan2(2.0*(y*w - x*z), 1.0 - 2.0*(y*y + z*z));

+		roll = asin(2.0*(x*y + z*w));

+		pitch = atan2(2.0*(x*w - y*z), 1.0 - 2.0*(x*x + z*z));

 	}

 	

 /*	 const Rotation operator*(const Rotation& A, const Rotation& B)

 	{

-		return Rotation(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d,	// quaternion multiplication

-						A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,

-						A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,

-						A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);

+		return Rotation(A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z,	// quaternion multiplication

+						A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y,

+						A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x,

+						A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w);

 	}*/

 

 

 		 const Rotation operator*(const Rotation& B)

 	{

 		const Rotation& A = *this;

-		return Rotation(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d,	// quaternion multiplication

-						A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,

-						A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,

-						A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);

+		return Rotation(A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z,	// quaternion multiplication

+						A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y,

+						A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x,

+						A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w);

 	}

 

 protected:

-	double a,b,c,d; // quaternion coefficients

+	double w,x,y,z; // quaternion coefficients

 };

 

 

diff --git a/facetracknoir/tracker_types.cpp b/facetracknoir/tracker_types.cpp
index 11adc985..89a06d7e 100644
--- a/facetracknoir/tracker_types.cpp
+++ b/facetracknoir/tracker_types.cpp
@@ -13,10 +13,10 @@ T6DOF operator-(const T6DOF& A, const T6DOF& B)
 

 	T6DOF C;

     R_C.toEuler(C.axes[Yaw], C.axes[Pitch], C.axes[Roll]);

+    R_C.toEuler(C.axes[Yaw], C.axes[Pitch], C.axes[Roll]);

     C.axes[Yaw] *= R2D;

     C.axes[Pitch] *= R2D;

     C.axes[Roll] *= R2D;

-

     C.axes[TX] = A.axes[TX] - B.axes[TX];

     C.axes[TY] = A.axes[TY] - B.axes[TY];

     C.axes[TZ] = A.axes[TZ] - B.axes[TZ];

diff --git a/ftnoir_tracker_base/ftnoir_tracker_types.h b/ftnoir_tracker_base/ftnoir_tracker_types.h
index 583d508b..8bf12990 100644
--- a/ftnoir_tracker_base/ftnoir_tracker_types.h
+++ b/ftnoir_tracker_base/ftnoir_tracker_types.h
@@ -30,8 +30,8 @@
 #ifndef FTNOIR_TRACKER_TYPES_H

 #define FTNOIR_TRACKER_TYPES_H

 

+

 enum Axis {

-    TX = 0, TY, TZ, Yaw, Pitch, Roll

-};

+    TX = 0, TY, TZ, Yaw, Pitch, Roll};

 

 #endif // FTNOIR_TRACKER_TYPES_H

diff --git a/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp b/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp
index 24ffedcd..3789c7bf 100644
--- a/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp
+++ b/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp
@@ -74,41 +74,42 @@ bool Hydra_Tracker::GiveHeadPoseData(double *data)
 	//sixenseUtils::getTheControllerManager()->update( &acd );
 
 	//sixenseControllerData cd;
-	Rotation quat = Rotation(acd.controllers[0].rot_quat[0],acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3]);
+	//Rotation quat = Rotation(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
+	sixenseMath::Matrix4 mat = sixenseMath::Matrix4(acd.controllers[0].rot_mat);// sixenseMath::Quat(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
+
     double yaw =  0.0f;
     double pitch = 0.0f;
     double roll = 0.0f;
-
-	quat.toEuler(yaw, pitch, roll);
-
-    newHeadPose[RY] = pitch;
-    newHeadPose[RZ] = roll;
-    newHeadPose[RX] = yaw;
-
+	float ypr[3];
+	
+	mat.getEulerAngles().fill(ypr);
+    newHeadPose[Yaw] = ypr[0];
+    newHeadPose[Pitch] = ypr[1];
+	newHeadPose[Roll] = ypr[2];
 
 	
 	newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
 	newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
 	newHeadPose[TZ] = acd.controllers[0].pos[2]/50.0f;
 		
-	//if (bEnableX) {
+	if (bEnableX) {
         data[TX] = newHeadPose[TX];
-    //}
-    //if (bEnableY) {
+    }
+    if (bEnableY) {
         data[TY] = newHeadPose[TY];
-    //}
-    //if (bEnableY) {
+    }
+    if (bEnableY) {
         data[TZ] = newHeadPose[TZ];
-    //}
+    }
 
     if (bEnableYaw) {
-        data[RX] = newHeadPose[RX] * 57.295781f;
+        data[Yaw] = newHeadPose[Yaw] * 57.295781f;
     }
     if (bEnablePitch) {
-        data[RY] = newHeadPose[RY] * 57.295781f;
+        data[Pitch] = newHeadPose[Pitch] * 57.295781f;
     }
     if (bEnableRoll) {
-        data[RZ] = newHeadPose[RZ] * 57.295781f;
+        data[Roll] = newHeadPose[Roll] * 57.295781f;
     }
     
 	return true;
diff --git a/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui b/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
index 789d2d25..eaf5b9d6 100644
--- a/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
+++ b/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <ui version="4.0">
  <class>UIRiftControls</class>
- <widget class="QWidget" name="UIRiftControls">
+ <widget class="QWidget" name="UIHydraControls">
   <property name="geometry">
    <rect>
     <x>0</x>
@@ -45,78 +45,97 @@
        </rect>
       </property>
       <layout class="QGridLayout" name="gridLayout_2">
-       <item row="0" column="0">
-        <widget class="QLabel" name="label_6">
-         <property name="text">
-          <string>Roll:</string>
-         </property>
-        </widget>
-       </item>
-       <item row="2" column="0">
-        <widget class="QLabel" name="label_11">
-         <property name="text">
-          <string>Yaw:</string>
-         </property>
-        </widget>
-       </item>
-       <item row="1" column="1">
-        <widget class="QCheckBox" name="chkEnablePitch">
-         <property name="maximumSize">
-          <size>
-           <width>20</width>
-           <height>16777215</height>
-          </size>
-         </property>
-         <property name="layoutDirection">
-          <enum>Qt::LeftToRight</enum>
-         </property>
-         <property name="text">
-          <string/>
-         </property>
-        </widget>
-       </item>
-       <item row="1" column="0">
-        <widget class="QLabel" name="label_9">
-         <property name="text">
-          <string>Pitch:</string>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="1">
-        <widget class="QCheckBox" name="chkEnableRoll">
-         <property name="maximumSize">
-          <size>
-           <width>20</width>
-           <height>16777215</height>
-          </size>
-         </property>
-         <property name="layoutDirection">
-          <enum>Qt::LeftToRight</enum>
-         </property>
-         <property name="styleSheet">
-          <string notr="true"/>
-         </property>
-         <property name="text">
-          <string/>
-         </property>
-        </widget>
-       </item>
-       <item row="2" column="1">
-        <widget class="QCheckBox" name="chkEnableYaw">
-         <property name="maximumSize">
-          <size>
-           <width>20</width>
-           <height>16777215</height>
-          </size>
-         </property>
-         <property name="layoutDirection">
-          <enum>Qt::LeftToRight</enum>
-         </property>
-         <property name="text">
-          <string/>
-         </property>
-        </widget>
-       </item>
+
+
+
+        <item row="0" column="0">
+          <widget class="QLabel" name="label_6">
+            <property name="text">
+              <string>Pitch:</string>
+            </property>
+          </widget>
+        </item>
+
+        <item row="0" column="1">
+          <widget class="QCheckBox" name="chkEnablePitch">
+            <property name="maximumSize">
+              <size>
+                <width>20</width>
+                <height>16777215</height>
+              </size>
+            </property>
+            <property name="layoutDirection">
+              <enum>Qt::LeftToRight</enum>
+            </property>
+            <property name="styleSheet">
+              <string notr="true"/>
+            </property>
+            <property name="text">
+              <string/>
+            </property>
+          </widget>
+        </item>
+
+        <item row="1" column="0">
+          <widget class="QLabel" name="label_9">
+            <property name="text">
+              <string>Yaw:</string>
+            </property>
+          </widget>
+        </item>
+
+
+        <item row="1" column="1">
+          <widget class="QCheckBox" name="chkEnableYaw">
+            <property name="maximumSize">
+              <size>
+                <width>20</width>
+                <height>16777215</height>
+              </size>
+            </property>
+            <property name="layoutDirection">
+              <enum>Qt::LeftToRight</enum>
+            </property>
+            <property name="text">
+              <string/>
+            </property>
+          </widget>
+        </item>
+
+
+
+        <item row="2" column="0">
+          <widget class="QLabel" name="label_11">
+            <property name="text">
+              <string>Roll:</string>
+            </property>
+          </widget>
+        </item>
+
+        <item row="2" column="1">
+          <widget class="QCheckBox" name="chkEnableRoll">
+            <property name="maximumSize">
+              <size>
+                <width>20</width>
+                <height>16777215</height>
+              </size>
+            </property>
+            <property name="layoutDirection">
+              <enum>Qt::LeftToRight</enum>
+            </property>
+            <property name="text">
+              <string/>
+            </property>
+          </widget>
+        </item>
+
+
+
+
+
+
+
+
       </layout>
      </widget>
     </widget>
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
index 367ab389..4e9dd7a3 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
@@ -66,7 +66,8 @@ void Rift_Tracker::StartTracker(QFrame* videoFrame)
         }else{
             QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to find Rift tracker",QMessageBox::Ok,QMessageBox::NoButton);
         }
-        //MagCal.BeginAutoCalibration(SFusion);
+		isCalibrated = false;
+		MagCal.BeginAutoCalibration(SFusion);
         SFusion.SetMagReference(SFusion.GetOrientation());
     }
 }
@@ -79,32 +80,27 @@ bool Rift_Tracker::GiveHeadPoseData(double *data)
 		 if (SFusion.IsMagReady() && !isCalibrated ){
             SFusion.SetYawCorrectionEnabled(true);
 			QMessageBox::warning(0,"OpenTrack Info", "Calibrated magnetic sensor",QMessageBox::Ok,QMessageBox::NoButton);
+			isCalibrated = true;
 		}else{
 			if(isCalibrated){
 				isCalibrated = false;
 				QMessageBox::warning(0,"OpenTrack Info", "Lost magnetic calibration",QMessageBox::Ok,QMessageBox::NoButton);
 			}
         }
-#endif
 
 		// Magnetometer calibration procedure
-		//MagCal.UpdateAutoCalibration(SFusion);
+		MagCal.UpdateAutoCalibration(SFusion);
+#endif
         Quatf hmdOrient = SFusion.GetOrientation();
         float yaw = 0.0f;
         float pitch = 0.0f;
         float roll = 0.0f;
-        //hmdOrient.GetEulerAngles< Axis_X, Axis_Y, Axis_Z>(&pitch, &yaw, &roll);
-		//hmdOrient.GetEulerAngles< Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw);
         hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch , &roll);
-		//hmdOrient.GetEulerAngles< Axis_Y, Axis_Z, Axis_X>(&yaw, &roll, &pitch);
-		//hmdOrient.GetEulerAngles< Axis_Z, Axis_X, Axis_Y>(&roll, &pitch, &yaw);
-		//hmdOrient.GetEulerAngles< Axis_Z, Axis_Y, Axis_X>(&roll, &yaw, &pitch);
-        newHeadPose[Pitch] = pitch;
-        newHeadPose[Roll]  = roll;
-        newHeadPose[Yaw]   = yaw;
+        newHeadPose[Yaw] = yaw;
+        newHeadPose[Pitch] =pitch;
+        newHeadPose[Roll] = roll;
 
 #if 0
-		sixenseControllerData cd;
 	
 		newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
 		newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.h b/ftnoir_tracker_rift/ftnoir_tracker_rift.h
index 0f3bc099..7bacb91c 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.h
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.h
@@ -6,6 +6,7 @@
 #include <math.h>
 #include "facetracknoir/global-settings.h"
 #include "OVR.h"
+#include "Util/Util_MagCalibration.h"
 class Rift_Tracker : public ITracker
 {
 public:
@@ -26,6 +27,10 @@ private:
 	OVR::Ptr<OVR::HMDDevice> pHMD;
 	OVR::Ptr<OVR::SensorDevice> pSensor;
 	OVR::SensorFusion SFusion;
+    // Magnetometer calibration and yaw correction
+    OVR::Util::MagCalibration MagCal;
+	bool isCalibrated;
+
     double newHeadPose[6];								// Structure with new headpose
 	bool bEnableRoll;
 	bool bEnablePitch;