fix axis swappage in PT

Issue: #63
Reported-by: @FlyingCircus-

1 files changed, 25 insertions, 32 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 91f94c71..4a344fb1 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -129,8 +129,8 @@ void Tracker::apply_inner()
 
 void Tracker::reset()
 {
-	QMutexLocker lock(&mutex);
-	point_tracker.reset();
+    QMutexLocker lock(&mutex);
+    point_tracker.reset();
 }
 
 void Tracker::start_tracker(QFrame *parent_window)
@@ -153,7 +153,7 @@ void Tracker::start_tracker(QFrame *parent_window)
 #ifndef OPENTRACK_API
 void Tracker::StopTracker(bool exit)
 {
-	set_command(PAUSE);
+    set_command(PAUSE);
 }
 #endif
 
@@ -164,35 +164,28 @@ void Tracker::StopTracker(bool exit)
 void Tracker::data(THeadPoseData *data)
 {
 
-    	FrameTrafo X_CM = point_tracker.pose();
-		FrameTrafo X_MH(Matx33f::eye(), t_MH);
-		FrameTrafo X_GH = X_CM * X_MH;
-        Matx33f R = X_GH.R;
-		Vec3f   t = X_GH.t;
-
-        // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
-		// -z -> x, y -> z, x -> -y
-		Matx33f R_EG( 0, 0,-1,
-		             -1, 0, 0,
-		              0, 1, 0);
-		R = R_EG * R * R_EG.t();
-
-		// extract rotation angles
-		float alpha, beta, gamma;
-		beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
-		alpha = atan2( R(1,0), R(0,0));
-		gamma = atan2( R(2,1), R(2,2));		
-
-        QMutexLocker lock(&mutex);
-
-        data[Yaw]   =   rad2deg * alpha;
-        data[Pitch] = - rad2deg * beta;	// FTNoIR expects a minus here
-        data[Roll]  =   rad2deg * gamma;
-
-        // get translation(s)
-        data[TX] = t[0] / 10.0;	// convert to cm
-        data[TY] = t[1] / 10.0;
-        data[TZ] = t[2] / 10.0;
+    FrameTrafo X_CM = point_tracker.pose();
+    FrameTrafo X_MH(Matx33f::eye(), t_MH);
+    FrameTrafo X_GH = X_CM * X_MH;
+    Matx33f R = X_GH.R;
+    Vec3f   t = X_GH.t;
+
+    // extract rotation angles
+    float alpha, beta, gamma;
+    beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+    alpha = atan2( R(1,0), R(0,0));
+    gamma = atan2( R(2,1), R(2,2));
+
+    QMutexLocker lock(&mutex);
+
+    data[Yaw] =   rad2deg * alpha;
+    data[Roll] = - rad2deg * beta;	// FTNoIR expects a minus here
+    data[Pitch]  =   rad2deg * gamma;
+
+    // get translation(s)
+    data[TX] = t[0] / 10.0;	// convert to cm
+    data[TY] = t[1] / 10.0;
+    data[TZ] = t[2] / 10.0;
 }
 
 //-----------------------------------------------------------------------------