From 3fc21e23083bce507fc9832073297c9a861c3505 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 14 Dec 2013 17:45:05 +0100 Subject: allow for Z swapping for tcomp Signed-off-by: Stanislaw Halik --- facetracknoir/tracker.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) (limited to 'facetracknoir/tracker.cpp') diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp index 8f8ad356..0c5501d4 100644 --- a/facetracknoir/tracker.cpp +++ b/facetracknoir/tracker.cpp @@ -26,7 +26,8 @@ Tracker::Tracker( FaceTrackNoIR *parent ) : should_quit(false), do_center(false), enabled(true), - compensate(true) + compensate(true), + tcomp_rz(false) { mainApp = parent; } @@ -50,8 +51,9 @@ static void get_curve(double pos, double& out, THeadPoseDOF& axis) { out += axis.zero; } -static void t_compensate(double* input, double* output) +static void t_compensate(double* input, double* output, bool rz) { + double z = rz ? -1 : 1; const auto H = input[Yaw] * M_PI / 180; const auto P = input[Pitch] * M_PI / 180; const auto B = input[Roll] * M_PI / 180; @@ -60,8 +62,8 @@ static void t_compensate(double* input, double* output) 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); + const auto cosB = cos(B * z); + const auto sinB = sin(B * z); double foo[] = { cosH * cosB - sinH * sinP * sinB, @@ -152,7 +154,7 @@ void Tracker::run() { } if (compensate) - t_compensate(output_camera.axes, output_camera.axes); + t_compensate(output_camera.axes, output_camera.axes, tcomp_rz); if (Libraries->pProtocol) { Libraries->pProtocol->sendHeadposeToGame( output_camera.axes ); // degrees & centimeters -- cgit v1.2.3