summaryrefslogtreecommitdiffhomepage
path: root/opentrack/simple-mat.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2016-06-14 11:44:30 +0200
committerStanislaw Halik <sthalik@misaki.pl>2016-06-14 18:14:46 +0200
commit2f0d5047d3fbe3958f2663c1ac7e7a2ecccc9336 (patch)
treecd0932f9d6d4ca0f6e82a6db4c104ea2eb4d2c78 /opentrack/simple-mat.cpp
parent8c882ca9311e70fc0a1577a5c38baba16e4398b2 (diff)
api/tracker: add euler filter
Gimbal lock is a problem too often.
Diffstat (limited to 'opentrack/simple-mat.cpp')
-rw-r--r--opentrack/simple-mat.cpp17
1 files changed, 13 insertions, 4 deletions
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;
}