summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--opentrack/simple-mat.cpp39
-rw-r--r--opentrack/simple-mat.hpp2
-rw-r--r--opentrack/tracker.cpp7
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));