Various changes and refactoring. Rotation correct for the Hydra, mostly

correct for the Rift, Rift looks better in OpenTrack, Hydra looks better
in Dirt3. Also rewrote the Rotation code for Quaternions, although it
seems broken with and without the changes. Will investigate.
-Chris

1 files changed, 80 insertions, 17 deletions

diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
index 9e985444..13336313 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
@@ -3,9 +3,20 @@
 #include "facetracknoir/global-settings.h"
 	#include "OVR.h"
 #include <cstdio>
+#define SIXENSE_STATIC_LIB
+#define SIXENSE_UTILS_STATIC_LIB
+#include <sixense.h>
+#include <sixense_math.hpp>
+#ifdef WIN32
+#include <sixense_utils/mouse_pointer.hpp>
+#endif
+#include <sixense_utils/derivatives.hpp>
+#include <sixense_utils/button_states.hpp>
+#include <sixense_utils/event_triggers.hpp>
+#include <sixense_utils/controller_manager/controller_manager.hpp>
+
 using namespace OVR;
 
-bool Rift_Tracker::isInitialised = false;
 
 Rift_Tracker::Rift_Tracker()
 {
@@ -27,11 +38,26 @@ Rift_Tracker::Rift_Tracker()
 
 Rift_Tracker::~Rift_Tracker()
 {
+	
+	sixenseExit();
     pSensor.Clear();
 	pHMD.Clear();
 	pManager.Clear();
+	System::Destroy();
 }
 
+/*
+void controller_manager_setup_callback( sixenseUtils::ControllerManager::setup_step step ) {
+	
+		QMessageBox::warning(0,"OpenTrack Info", "controller manager callback",QMessageBox::Ok,QMessageBox::NoButton);
+	if( sixenseUtils::getTheControllerManager()->isMenuVisible() ) {
+		// Ask the controller manager what the next instruction string should be.
+		std::string controller_manager_text_string = sixenseUtils::getTheControllerManager()->getStepString();
+		QMessageBox::warning(0,"OpenTrack Info", controller_manager_text_string.c_str(),QMessageBox::Ok,QMessageBox::NoButton);
+		// We could also load the supplied controllermanager textures using the filename: sixenseUtils::getTheControllerManager()->getTextureFileName();
+
+	}
+}*/
 
 void Rift_Tracker::StartTracker(QFrame* videoFrame)
 {
@@ -40,11 +66,7 @@ void Rift_Tracker::StartTracker(QFrame* videoFrame)
     //
     // Startup the Oculus SDK device handling, use the first Rift sensor we find.
     //
-	if(!isInitialised){
-	    System::Init(Log::ConfigureDefaultLog(LogMask_All));
-		isInitialised = true;
-	}
-
+	System::Init(Log::ConfigureDefaultLog(LogMask_All));
 	pManager = *DeviceManager::Create();
     DeviceEnumerator<HMDDevice>& enumerator = pManager->EnumerateDevices<HMDDevice>();
     if (enumerator.IsAvailable())
@@ -58,34 +80,75 @@ void Rift_Tracker::StartTracker(QFrame* videoFrame)
         }else{
             QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to find Rift tracker",QMessageBox::Ok,QMessageBox::NoButton);
         }
-        SFusion.SetYawCorrectionEnabled(true);
-        SFusion.SetMagReference();
-    }
+		isCalibrated = false;
+		MagCal.BeginAutoCalibration(SFusion);
+		SFusion.SetMagReference(SFusion.GetOrientation());
 
+    }
+		// Init sixense
+	//QMessageBox::warning(0,"OpenTrack Info", "sixense init",QMessageBox::Ok,QMessageBox::NoButton);
+	sixenseInit();
+	//QMessageBox::warning(0,"OpenTrack Info", "sixense init complete, setting controller manager",QMessageBox::Ok,QMessageBox::NoButton);
+	// Init the controller manager. This makes sure the controllers are present, assigned to left and right hands, and that
+	// the hemisphere calibration is complete.
+	//sixenseUtils::getTheControllerManager()->setGameType( sixenseUtils::ControllerManager::ONE_PLAYER_TWO_CONTROLLER );
+	//sixenseUtils::getTheControllerManager()->registerSetupCallback( controller_manager_setup_callback );
+	//QMessageBox::warning(0,"OpenTrack Info", "controller manager callback registered",QMessageBox::Ok,QMessageBox::NoButton);
 	return;
 }
 
+
 bool Rift_Tracker::GiveHeadPoseData(double *data)
 {
     if (pHMD.GetPtr() != NULL) {
+
+		 if (SFusion.IsMagReady() && !isCalibrated ){
+            SFusion.SetYawCorrectionEnabled(true);
+			QMessageBox::warning(0,"OpenTrack Info", "Calibrated magnetic sensor",QMessageBox::Ok,QMessageBox::NoButton);
+		}else{
+			if(isCalibrated){
+				isCalibrated = false;
+				QMessageBox::warning(0,"OpenTrack Info", "Lost magnetic calibration",QMessageBox::Ok,QMessageBox::NoButton);
+			}
+        }
+
+		// Magnetometer calibration procedure
+		MagCal.UpdateAutoCalibration(SFusion);
         Quatf hmdOrient = SFusion.GetOrientation();
         float yaw = 0.0f;
         float pitch = 0.0f;
         float roll = 0.0f;
-        hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch , &roll);
+        //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[RY] =pitch;
         newHeadPose[RZ] = roll;
         newHeadPose[RX] = yaw;
-#if 0
-        if (bEnableX) {
+
+
+		sixenseSetActiveBase(0);
+		sixenseAllControllerData acd;
+		sixenseGetAllNewestData( &acd );
+		//sixenseUtils::getTheControllerManager()->update( &acd );
+#if 1
+		sixenseControllerData cd;
+	
+		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) {
             data[TX] = newHeadPose[TX];
-        }
-        if (bEnableY) {
+        //}
+        //if (bEnableY) {
             data[TY] = newHeadPose[TY];
-        }
-        if (bEnableY) {
+        //}
+        //if (bEnableY) {
             data[TZ] = newHeadPose[TZ];
-        }
+        //}
 #endif
         if (bEnableYaw) {
             data[RX] = newHeadPose[RX] * 57.295781f;