summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--opentrack/tracker.cpp14
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});