diff options
| -rw-r--r-- | opentrack/tracker.cpp | 14 |
1 files changed, 13 insertions, 1 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index a0cca2d6..0ba818bc 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -51,8 +51,20 @@ 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.9991) + { + double roll = atan(R(1, 0) / R(2, 0)); + return dmat<3, 1>({0., up, roll}); + } + if (R(0, 2) < -0.9991) + { + double roll = atan(R(1, 0) / R(2, 0)); + return dmat<3, 1>({0., -up, roll}); + } // don't use atan2 here, confuses quadrants. see issue #63 -sh - double pitch = atan( -R(0, 2) / sqrt(R(1,2)*R(1,2) + R(2,2)*R(2,2)) ); + double pitch = asin(R(0, 2)); double roll = atan(R(1, 2) / R(2, 2)); double yaw = atan(R(0, 1) / R(0, 0)); return dmat<3, 1>({yaw, pitch, roll}); |