diff options
| -rw-r--r-- | opentrack/tracker.cpp | 12 |
1 files changed, 12 insertions, 0 deletions
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)); |