1 files changed, 67 insertions, 13 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index c4ab5963..1bf581a7 100644..100755
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -58,15 +58,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;
@@ -116,7 +128,7 @@ void Tracker_PT::run()
             }
             
             Affine X_CM = pose();
-            
+
             std::function<void(const cv::Vec2f&, const cv::Scalar)> fun = [&](const cv::Vec2f& p, const cv::Scalar color)
             {
                 auto p2 = cv::Point(p[0] * frame_.cols + frame_.cols/2, -p[1] * frame_.cols + frame_.rows/2);
@@ -138,7 +150,7 @@ void Tracker_PT::run()
             }
             
             {
-                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
+                Affine X_MH(cv::Matx33f::eye(), get_model_offset()); // just copy pasted these lines from below
                 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
@@ -151,14 +163,60 @@ void Tracker_PT::run()
     qDebug()<<"Tracker:: Thread stopping";
 }
 
+cv::Vec3f Tracker_PT::get_model_offset()
+{
+    cv::Vec3f offset(s.t_MH_x, s.t_MH_y, s.t_MH_z);
+    if (offset[0] == 0 && offset[1] == 0 && offset[2] == 0)
+    {
+        int m = s.model_used;
+        switch (m)
+        {
+        default:
+        // cap
+        case 0: offset[0] = 0; offset[1] = 0; offset[2] = 0; break;
+        // clip
+        case 1: offset[0] = 135; offset[1] = 0; offset[2] = 0; break;
+        // left clip
+        case 2: offset[0] = -135; offset[1] = 0; offset[2] = 0; break;
+        }
+    }
+    return offset;
+}
+
 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();
     frame = cv::Mat();
     qDebug()<<"Tracker::apply ends";
@@ -184,7 +242,7 @@ void Tracker_PT::data(double *data)
     {
         Affine X_CM = pose();
 
-        Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
+        Affine X_MH(cv::Matx33f::eye(), get_model_offset());
         Affine X_GH = X_CM * X_MH;
 
         cv::Matx33f R = X_GH.R;
@@ -213,7 +271,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)
-