From 2f0d5047d3fbe3958f2663c1ac7e7a2ecccc9336 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 14 Jun 2016 11:44:30 +0200 Subject: api/tracker: add euler filter Gimbal lock is a problem too often. --- opentrack/simple-mat.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) (limited to 'opentrack/simple-mat.cpp') diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp index 45a6ef5b..645bcbbe 100644 --- a/opentrack/simple-mat.cpp +++ b/opentrack/simple-mat.cpp @@ -9,7 +9,7 @@ enum Axis TX, TY, TZ, Yaw, Pitch, Roll }; -static constexpr double eps_ = 1e-1; +static constexpr double eps_ = 2e1; static constexpr double d2r = pi / 180; static constexpr double eps = eps_ * d2r; @@ -25,15 +25,24 @@ euler_t euler_filter(const euler_t& rot_) pi, pi/2, pi }; + double changed_eps[] = { 0, 0, 0 }; + for (int i = 0; i < 3; i++) if (fabs(rot(i)) > thres[i] - eps && fabs(rot(i)) < thres[i] + eps) { const double eps__ = copysign(eps, rot(i)); + changed_eps[i] = eps__; + rot(i) -= eps__; - rmat tmp = euler_to_rmat(rot); - rot = rmat_to_euler(tmp); - rot(i) += eps__; + rot = rmat_to_euler(euler_to_rmat(rot)); } + + for (int i = 0; i < 3; i++) + { + rot(i) += changed_eps[i]; + if (fabs(rot(i)) > 2*pi) + rot(i) = copysign(2*pi, rot(i)); + } return rot; } -- cgit v1.2.3