diff options
| -rw-r--r-- | opentrack/simple-mat.cpp | 39 | ||||
| -rw-r--r-- | opentrack/simple-mat.hpp | 2 | ||||
| -rw-r--r-- | opentrack/tracker.cpp | 7 |
3 files changed, 0 insertions, 48 deletions
diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp index 645bcbbe..4ab68e0f 100644 --- a/opentrack/simple-mat.cpp +++ b/opentrack/simple-mat.cpp @@ -9,46 +9,8 @@ enum Axis TX, TY, TZ, Yaw, Pitch, Roll }; -static constexpr double eps_ = 2e1; -static constexpr double d2r = pi / 180; -static constexpr double eps = eps_ * d2r; - -euler_t euler_filter(const euler_t& rot_) -{ - using std::fabs; - using std::copysign; - - euler_t rot(rot_); - - static constexpr double thres[] = - { - 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__; - 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; -} - euler_t rmat_to_euler(const dmat<3, 3>& R) { - static constexpr double pi = 3.141592653; const double pitch_1 = asin(-R(0, 2)); const double pitch_2 = pi - pitch_1; const double cos_p1 = cos(pitch_1), cos_p2 = cos(pitch_2); @@ -74,7 +36,6 @@ euler_t rmat_to_euler(const dmat<3, 3>& R) // tait-bryan angles, not euler rmat euler_to_rmat(const double* input) { - static constexpr double pi = 3.141592653; auto H = input[0] * pi / 180; auto P = input[1] * pi / 180; auto B = input[2] * pi / 180; diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp index 2855ad0e..54ebb920 100644 --- a/opentrack/simple-mat.hpp +++ b/opentrack/simple-mat.hpp @@ -248,8 +248,6 @@ template<int y, int x> using dmat = Mat<double, y, x>; using rmat = dmat<3, 3>; using euler_t = dmat<3, 1>; -euler_t euler_filter(const euler_t& rot_); - rmat euler_to_rmat(const double* input); // http://stackoverflow.com/a/18436193 diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 819c3592..9d754324 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -209,13 +209,6 @@ void Tracker::logic() libs.pFilter->filter(tmp, value); } - { - euler_t e(value(Yaw), value(Pitch), value(Roll)); - e = euler_filter(e); - for (int i = 0; i < 3; i++) - value(i + Yaw) = e(i); - } - for (int i = 0; i < 6; i++) value(i) = map(value(i), m(i)); |