summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-11-11 12:38:37 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-11-11 12:39:01 +0100
commit3a2b6b9d822b2cde6d8d55a11abdc8979f5e198d (patch)
treeb07200da5c40502f6ecf38dac5e56b74bd14f079
parent3514acd1b038e6f906ae2af8b46e626b85724783 (diff)
add again gimbal lock workaround code
@KyokushinPL reports +-90 pitch boundary broken otherwise
-rw-r--r--opentrack/tracker.cpp12
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));