From 3a2b6b9d822b2cde6d8d55a11abdc8979f5e198d Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 11 Nov 2014 12:38:37 +0100 Subject: add again gimbal lock workaround code @KyokushinPL reports +-90 pitch boundary broken otherwise --- opentrack/tracker.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 8892c85a..5b36971e 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -51,6 +51,18 @@ double Tracker::map(double pos, bool invertp, Mapping& axis) // http://stackoverflow.com/a/18436193 static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R) { + static constexpr double pi = 3.141592653; + const double up = 90 * pi / 180.; + if (R(0, 2) > 0.998) + { + double roll = atan(R(1, 0) / R(2, 0)); + return dmat<3, 1>({0., up, roll}); + } + if (R(0, 2) < -0.998) + { + double roll = atan(R(1, 0) / R(2, 0)); + return dmat<3, 1>({0., -up, roll}); + } double pitch = asin(-R(0, 2)); double roll = atan2(R(1, 2), R(2, 2)); double yaw = atan2(R(0, 1), R(0, 0)); -- cgit v1.2.3