pt: restore clip model pitch. remove workaround

Issue: #63, #93

2 files changed, 7 insertions, 7 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 2c64d5cb..f182cacc 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -172,15 +172,15 @@ void Tracker::data(THeadPoseData *data)
     R = R_EG * R * R_EG.t();
 
     static constexpr float pi = 3.141592653;
-    float cy = cos(pi*.5);
-    float sy = sin(pi*.5);
+    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);
     // 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));
+    data[Yaw] = -rad2deg * atan2( R(1,0), R(0,0));
+    data[Pitch] = -rad2deg * atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+    data[Roll] = rad2deg * atan2( R(2,1), R(2,2));
 
     // 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 d4cf68ec..ee65c81e 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -83,10 +83,10 @@ 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);
+    s.m02_y = -static_cast<double>(s.clip_by);
     s.m02_z = -static_cast<double>(s.clip_bz);
 
     settings_changed();