summaryrefslogtreecommitdiffhomepage
path: root/opentrack
diff options
context:
space:
mode:
Diffstat (limited to 'opentrack')
-rw-r--r--opentrack/simple-mat.cpp98
-rw-r--r--opentrack/simple-mat.hpp72
2 files changed, 111 insertions, 59 deletions
diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp
new file mode 100644
index 00000000..45a6ef5b
--- /dev/null
+++ b/opentrack/simple-mat.cpp
@@ -0,0 +1,98 @@
+#include "simple-mat.hpp"
+
+namespace euler {
+
+static constexpr double pi = 3.141592653;
+
+enum Axis
+{
+ TX, TY, TZ, Yaw, Pitch, Roll
+};
+
+static constexpr double eps_ = 1e-1;
+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
+ };
+
+ 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));
+ rot(i) -= eps__;
+ rmat tmp = euler_to_rmat(rot);
+ rot = rmat_to_euler(tmp);
+ rot(i) += eps__;
+ }
+ 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);
+ const double roll_1 = atan2(R(1, 2) / cos_p1, R(2, 2) / cos_p1);
+ const double roll_2 = atan2(R(1, 2) / cos_p2, R(2, 2) / cos_p2);
+ const double yaw_1 = atan2(R(0, 1) / cos_p1, R(0, 0) / cos_p1);
+ const double yaw_2 = atan2(R(0, 1) / cos_p2, R(0, 0) / cos_p2);
+
+ using std::fabs;
+
+ const double err1 = fabs(pitch_1) + fabs(roll_1) + fabs(yaw_1);
+ const double err2 = fabs(pitch_2) + fabs(roll_2) + fabs(yaw_2);
+
+ if (err1 > err2)
+ {
+ bool fix_neg_pitch = pitch_1 < 0;
+ return euler_t(yaw_2, fix_neg_pitch ? -pi - pitch_1 : pitch_2, roll_2);
+ }
+ else
+ return euler_t(yaw_1, pitch_1, roll_1);
+}
+
+// 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;
+
+ const auto c1 = cos(H);
+ const auto s1 = sin(H);
+ const auto c2 = cos(P);
+ const auto s2 = sin(P);
+ const auto c3 = cos(B);
+ const auto s3 = sin(B);
+
+ double foo[] = {
+ // z
+ c1 * c2,
+ c1 * s2 * s3 - c3 * s1,
+ s1 * s3 + c1 * c3 * s2,
+ // y
+ c2 * s1,
+ c1 * c3 + s1 * s2 * s3,
+ c3 * s1 * s2 - c1 * s3,
+ // x
+ -s2,
+ c2 * s3,
+ c2 * c3
+ };
+
+ return dmat<3, 3>(foo);
+}
+
+} // end ns euler
diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index d56d536a..864f0e49 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -232,67 +232,21 @@ public:
return ret;
}
+};
- template<int h__, int w__> using dmat = Mat<double, h__, w__>;
+namespace euler {
- // 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 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);
- const double roll_1 = atan2(R(1, 2) / cos_p1, R(2, 2) / cos_p1);
- const double roll_2 = atan2(R(1, 2) / cos_p2, R(2, 2) / cos_p2);
- const double yaw_1 = atan2(R(0, 1) / cos_p1, R(0, 0) / cos_p1);
- const double yaw_2 = atan2(R(0, 1) / cos_p2, R(0, 0) / cos_p2);
-
- using std::fabs;
-
- const double err1 = fabs(pitch_1) + fabs(roll_1) + fabs(yaw_1);
- const double err2 = fabs(pitch_2) + fabs(roll_2) + fabs(yaw_2);
-
- if (err1 > err2)
- {
- bool fix_neg_pitch = pitch_1 < 0;
- return dmat<3, 1>(yaw_2, fix_neg_pitch ? -pi - pitch_1 : pitch_2, roll_2);
- }
- else
- return dmat<3, 1>(yaw_1, pitch_1, roll_1);
- }
+template<int y, int x> using dmat = Mat<double, y, x>;
+using rmat = dmat<3, 3>;
+using euler_t = dmat<3, 1>;
- // tait-bryan angles, not euler
- static dmat<3, 3> 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;
-
- const auto c1 = cos(H);
- const auto s1 = sin(H);
- const auto c2 = cos(P);
- const auto s2 = sin(P);
- const auto c3 = cos(B);
- const auto s3 = sin(B);
-
- double foo[] = {
- // z
- c1 * c2,
- c1 * s2 * s3 - c3 * s1,
- s1 * s3 + c1 * c3 * s2,
- // y
- c2 * s1,
- c1 * c3 + s1 * s2 * s3,
- c3 * s1 * s2 - c1 * s3,
- // x
- -s2,
- c2 * s3,
- c2 * c3
- };
-
- return dmat<3, 3>(foo);
- }
-};
+euler_t euler_filter(const euler_t& rot_);
+
+rmat euler_to_rmat(const double* input);
+
+// http://stackoverflow.com/a/18436193
+euler_t rmat_to_euler(const dmat<3, 3>& R);
+
+} // end ns euler
template<int h_, int w_> using dmat = Mat<double, h_, w_>;