implement camera offset

2 files changed, 19 insertions, 5 deletions

diff --git a/opentrack/main-settings.hpp b/opentrack/main-settings.hpp
index 2c909d27..def21f31 100644
--- a/opentrack/main-settings.hpp
+++ b/opentrack/main-settings.hpp
@@ -28,6 +28,7 @@ struct main_settings {
     axis_opts a_x, a_y, a_z, a_yaw, a_pitch, a_roll;
     value<bool> tcomp_p, tcomp_tz;
     value<bool> tray_enabled;
+    value<int> camera_yaw, camera_pitch;
     main_settings(pbundle b) :
         b(b),
         tracker_dll(b, "tracker-dll", ""),
@@ -42,6 +43,8 @@ struct main_settings {
         a_roll(b, "roll", Roll),
         tcomp_p(b, "compensate-translation", true),
         tcomp_tz(b, "compensate-translation-disable-z-axis", false),
-        tray_enabled(b, "use-system-tray", false)
+        tray_enabled(b, "use-system-tray", false),
+        camera_yaw(b, "camera-yaw", 0),
+        camera_pitch(b, "camera-pitch", 0)
     {}
 };
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index f621083b..60ed4c33 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -94,22 +94,33 @@ void Tracker::logic()
             raw(i) = value(i) = t_b[i];
         }
     }
+
+    const double off[] = {
+        s.camera_yaw,
+        s.camera_pitch,
+        0.
+    };
+    const rmat cam = rmat::euler_to_rmat(off);
+    rmat r = rmat::euler_to_rmat(&value[Yaw]);
+    dmat<3, 1> t { value(0), value(1), value(3) };
+    
+    r = cam * r;
+    t = cam * t;
     
     if (centerp)
     {
         centerp = false;
         for (int i = 0; i < 3; i++)
-            t_b[i] = value(i);
-        r_b = rmat::euler_to_rmat(&value[Yaw]);
+            t_b[i] = t(i, 0);
+        r_b = r;
     }
     
     {
-        const rmat r = rmat::euler_to_rmat(&value[Yaw]);
         const rmat m_ = r * r_b.t();
         const dmat<3, 1> euler = rmat::rmat_to_euler(m_);
         for (int i = 0; i < 3; i++)
         {
-            value(i) -= t_b[i];
+            value(i) = t(i, 0) - t_b[i];
             value(i+3) = euler(i, 0) * r2d;
         }
     }