1 files changed, 58 insertions, 10 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 19d1bd7f..7b70d4eb 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -50,15 +50,27 @@ void Tracker_PT::reset_command(Command command)
 
 bool Tracker_PT::get_focal_length(float& ret)
 {
+    static constexpr float pi = 3.1415926;
+    float fov_;
+    switch (s.fov)
+    {
+    default:
+    case 0:
+        fov_ = 56;
+        break;
+    case 1:
+        fov_ = 75;
+        break;
+    }
+
+    const double diag_fov = static_cast<int>(fov_) * pi / 180.f;
     QMutexLocker l(&camera_mtx);
     CamInfo info;
     const bool res = camera.get_info(info);
     if (res)
     {
-        static constexpr double pi = 3.14159265359;
         const int w = info.res_x, h = info.res_y;
         const double diag = sqrt(1. + h/(double)w * h/(double)w);
-        const double diag_fov = static_cast<int>(s.fov) * pi / 180.;
         const double fov = 2.*atan(tan(diag_fov/2.0)/diag);
         ret = .5 / tan(.5 * fov);
         return true;
@@ -114,6 +126,20 @@ void Tracker_PT::run()
             {
                 Affine X_CM = pose();
                 Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below
+                if (X_MH.t[0] == 0 && X_MH.t[1] == 0 && X_MH.t[2] == 0)
+                {
+                    int m = s.model_used;
+                    switch (m)
+                    {
+                    default:
+                    // cap
+                    case 0: X_MH.t[0] = 0; X_MH.t[1] = 0; X_MH.t[2] = 0; break;
+                    // clip
+                    case 1: X_MH.t[0] = 135; X_MH.t[1] = 0; X_MH.t[2] = 0; break;
+                    // left clip
+                    case 2: X_MH.t[0] = -135; X_MH.t[1] = 0; X_MH.t[2] = 0; break;
+                    }
+                }
                 Affine X_GH = X_CM * X_MH;
                 cv::Vec3f p = X_GH.t; // head (center?) position in global space
                 cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx);  // projected to screen
@@ -154,10 +180,36 @@ void Tracker_PT::apply_settings()
 {
     qDebug()<<"Tracker:: Applying settings";
     QMutexLocker l(&camera_mtx);
-    camera.stop();
-    camera.set_device_index(camera_name_to_index(s.camera_name));
-    camera.set_res(s.cam_res_x, s.cam_res_y);
-    camera.set_fps(s.cam_fps);
+    camera.set_device_index(camera_name_to_index("PS3Eye Camera"));
+    int res_x, res_y, cam_fps;
+    switch (s.camera_mode)
+    {
+    default:
+    case 0:
+        res_x = 640;
+        res_y = 480;
+        cam_fps = 75;
+        break;
+    case 1:
+        res_x = 640;
+        res_y = 480;
+        cam_fps = 60;
+        break;
+    case 2:
+        res_x = 320;
+        res_y = 240;
+        cam_fps = 189;
+        break;
+    case 3:
+        res_x = 320;
+        res_y = 240;
+        cam_fps = 120;
+        break;
+    }
+
+    camera.set_res(res_x, res_y);
+    camera.set_fps(cam_fps);
+    qDebug() << "camera start";
     camera.start();
     qDebug()<<"Tracker::apply ends";
 }
@@ -211,7 +263,3 @@ void Tracker_PT::data(double *data)
         data[TZ] = t[2] / 10.0;
     }
 }
-
-#include "ftnoir_tracker_pt_dialog.h"
-OPENTRACK_DECLARE_TRACKER(Tracker_PT, TrackerDialog_PT, TrackerDll)
-