1 files changed, 34 insertions, 31 deletions
diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp
index 6c48b17a..b65ccde8 100644
--- a/facetracknoir/tracker.cpp
+++ b/facetracknoir/tracker.cpp
@@ -50,6 +50,39 @@ static void get_curve(double pos, double& out, THeadPoseDOF& axis) {
     out += axis.zero;
 }
 
+static void t_compensate(double* input, double* output)
+{
+    const auto H = input[Yaw] * M_PI / 180;
+    const auto P = input[Pitch] * M_PI / 180;
+    const auto B = input[Roll] * M_PI / 180;
+
+    const auto cosH = cos(H);
+    const auto sinH = sin(H);
+    const auto cosP = cos(P);
+    const auto sinP = sin(P);
+    const auto cosB = cos(B);
+    const auto sinB = sin(B);
+
+    double foo[] = {
+        cosH * cosB - sinH * sinP * sinB,
+        - sinB * cosP,
+        sinH * cosB + cosH * sinP * sinB,
+        cosH * sinB + sinH * sinP * cosB,
+        cosB * cosP,
+        sinB * sinH - cosH * sinP * cosB,
+        - sinH * cosP,
+        - sinP,
+        cosH * cosP,
+    };
+
+    cv::Mat rmat(3, 3, CV_64F, foo);
+    const cv::Mat tvec(3, 1, CV_64F, input);
+    cv::Mat ret = rmat * tvec;
+
+    for (int i = 0; i < 3; i++)
+        output[i] = ret.at<double>(i);
+}
+
 /** QThread run method @override **/
 void Tracker::run() {
     T6DOF offset_camera, gameoutput_camera, target_camera;
@@ -129,37 +162,7 @@ void Tracker::run() {
             }
 
             if (compensate)
-            {
-                const auto H = output_camera.axes[Yaw] * M_PI / 180;
-                const auto P = output_camera.axes[Pitch] * M_PI / 180;
-                const auto B = output_camera.axes[Roll] * M_PI / 180;
-
-                const auto cosH = cos(H);
-                const auto sinH = sin(H);
-                const auto cosP = cos(P);
-                const auto sinP = sin(P);
-                const auto cosB = cos(B);
-                const auto sinB = sin(B);
-
-                double foo[] = {
-                    cosH * cosB - sinH * sinP * sinB,
-                    - sinB * cosP,
-                    sinH * cosB + cosH * sinP * sinB,
-                    cosH * sinB + sinH * sinP * cosB,
-                    cosB * cosP,
-                    sinB * sinH - cosH * sinP * cosB,
-                    - sinH * cosP,
-                    - sinP,
-                    cosH * cosP,
-                };
-
-                cv::Mat rmat(3, 3, CV_64F, foo);
-                cv::Mat tvec(3, 1, CV_64F, output_camera.axes);
-                cv::Mat ret = rmat * tvec;
-
-                for (int i = 0; i < 3; i++)
-                    output_camera.axes[i] = ret.at<double>(i);
-            }
+                t_compensate(output_camera.axes, output_camera.axes);
 
             if (Libraries->pProtocol) {
                 gameoutput_camera = output_camera;