From 75c6790f74739f7a8e20554ea4cf8ddbdc381b90 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 7 Nov 2014 08:04:23 +0100 Subject: fix r/t comp after mat change --- opentrack/tracker.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index acdeb945..89a2ada6 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -93,15 +93,14 @@ static dmat<3, 3> euler_to_rmat(const double* input) void Tracker::t_compensate(const dmat<3, 3>& rmat, const double* xyz, double* output, bool rz) { - static constexpr int p_x = 2, p_y = 0, p_z = 1; - dmat<3, 1> tvec({-xyz[p_x], -xyz[p_y], xyz[p_z]}); + dmat<3, 1> tvec({ xyz[2], -xyz[0], -xyz[1] }); const dmat<3, 1> ret = rmat * tvec; - output[0] = -ret(p_x, 0); - output[1] = -ret(p_y, 0); if (!rz) - output[2] = ret(p_z, 0); + output[2] = ret(0, 0); else output[2] = xyz[2]; + output[1] = -ret(2, 0); + output[0] = -ret(1, 0); } void Tracker::logic() -- cgit v1.2.3