restore PT axis order swap

Reported-by: @FlyingCircus-
Issue: #63

2 files changed, 15 insertions, 11 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 3afb7fa7..1545f6b1 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -166,17 +166,21 @@ void Tracker::data(THeadPoseData *data)
     Matx33f R = X_GH.R;
     Vec3f   t = X_GH.t;
 
-    // extract rotation angles
-    float alpha, beta, gamma;
-    beta  = atan( -R(2,0) / sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
-    alpha = atan( R(1,0) / R(0,0));
-    gamma = atan( R(2,1) / R(2,2));
-
+    Matx33f R_EG( 0, 0,-1,
+                 -1, 0, 0,
+                  0, 1, 0);
+    R = R_EG * R * R_EG.t();
+
+    static constexpr float pi = 3.141592653;
+    float cy = cos(pi*.5);
+    float sy = sin(pi*.5);
+    Matx33f r_y(cy, 0, sy, 0, 1, 0, -sy, 0, cy);
+    R = r_y.t() * R;
     QMutexLocker lock(&mutex);
-
-    data[Yaw] =   rad2deg * alpha;
-    data[Roll] = - rad2deg * beta;	// FTNoIR expects a minus here
-    data[Pitch]  =   rad2deg * gamma;
+    // extract rotation angles
+    data[Pitch]   = rad2deg * atan( -R(0,2) / R(0,0));
+    data[Roll]    = rad2deg * asin( R(0,1));
+    data[Yaw]  = rad2deg * atan( R(2,1) / R(1,1));
 
     // get translation(s)
     data[TX] = t[0] / 10.0;	// convert to cm
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index ad9ef7c0..d4cf68ec 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -83,7 +83,7 @@ TrackerDialog::TrackerDialog()
 void TrackerDialog::set_model_clip()
 {
     s.m01_x =  0;
-    s.m01_y = static_cast<double>(s.clip_ty);
+    s.m01_y = -static_cast<double>(s.clip_ty);
     s.m01_z = -static_cast<double>(s.clip_tz);
     s.m02_x =  0;
     s.m02_y = static_cast<double>(s.clip_by);