summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-12-15 21:47:35 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-12-15 21:47:35 +0100
commit769d1340d3a76547a2252f52eb0a8615015f20e0 (patch)
tree0d676fa2f3028bca75796fd983c78bc062452ed3
parent8e01d0feed376f5e93698fb820e224014d8d9532 (diff)
move rmat->euler and euler->rmat conversions to header
-rw-r--r--opentrack/simple-mat.hpp58
-rw-r--r--opentrack/tracker.cpp69
-rw-r--r--opentrack/tracker.h3
3 files changed, 67 insertions, 63 deletions
diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index b36b21ba..447327a8 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -75,7 +75,63 @@ struct Mat
return ret;
}
+
+ template<int h_, int w_> using dmat = Mat<double, h_, w_>;
+
+ // 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 up = 90 * pi / 180.;
+ static constexpr double bound = 1. - 2e-4;
+ if (R(0, 2) > bound)
+ {
+ double roll = atan(R(1, 0) / R(2, 0));
+ return dmat<3, 1>({0., up, roll});
+ }
+ if (R(0, 2) < -bound)
+ {
+ double roll = atan(R(1, 0) / R(2, 0));
+ return dmat<3, 1>({0., -up, roll});
+ }
+ double pitch = asin(-R(0, 2));
+ double roll = atan2(R(1, 2), R(2, 2));
+ double yaw = atan2(R(0, 1), R(0, 0));
+ return dmat<3, 1>({yaw, pitch, roll});
+ }
+
+ // 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);
+ }
};
template<int h, int w> using dmat = Mat<double, h, w>;
-
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index a6433da5..f621083b 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -49,62 +49,7 @@ double Tracker::map(double pos, bool invertp, Mapping& axis)
return fc.getValue(pos) + axis.opts.zero;
}
-// 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 up = 90 * pi / 180.;
- static constexpr double bound = 1. - 2e-4;
- if (R(0, 2) > bound)
- {
- double roll = atan(R(1, 0) / R(2, 0));
- return dmat<3, 1>({0., up, roll});
- }
- if (R(0, 2) < -bound)
- {
- double roll = atan(R(1, 0) / R(2, 0));
- return dmat<3, 1>({0., -up, roll});
- }
- double pitch = asin(-R(0, 2));
- double roll = atan2(R(1, 2), R(2, 2));
- double yaw = atan2(R(0, 1), R(0, 0));
- return dmat<3, 1>({yaw, pitch, roll});
-}
-
-// 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);
-}
-
-void Tracker::t_compensate(const dmat<3, 3>& rmat, const double* xyz, double* output, bool rz)
+void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output, bool rz)
{
// TY is really yaw axis. need swapping accordingly.
dmat<3, 1> tvec({ xyz[2], -xyz[0], -xyz[1] });
@@ -141,7 +86,7 @@ void Tracker::logic()
}
else
{
- auto mat = rmat_to_euler(r_b);
+ auto mat = rmat::rmat_to_euler(r_b);
for (int i = 0; i < 3; i++)
{
@@ -155,13 +100,13 @@ void Tracker::logic()
centerp = false;
for (int i = 0; i < 3; i++)
t_b[i] = value(i);
- r_b = euler_to_rmat(&value[Yaw]);
+ r_b = rmat::euler_to_rmat(&value[Yaw]);
}
{
- const dmat<3, 3> rmat = euler_to_rmat(&value[Yaw]);
- const dmat<3, 3> m_ = rmat * r_b.t();
- const dmat<3, 1> euler = rmat_to_euler(m_);
+ const rmat r = rmat::euler_to_rmat(&value[Yaw]);
+ const rmat m_ = r * r_b.t();
+ const dmat<3, 1> euler = rmat::rmat_to_euler(m_);
for (int i = 0; i < 3; i++)
{
value(i) -= t_b[i];
@@ -184,7 +129,7 @@ void Tracker::logic()
value[i] *= inverts[i] ? -1. : 1.;
if (s.tcomp_p)
- t_compensate(euler_to_rmat(&value[Yaw]),
+ t_compensate(rmat::euler_to_rmat(&value[Yaw]),
value,
value,
s.tcomp_tz);
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index f7f69a7b..58eb69dd 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -32,6 +32,8 @@ private:
volatile bool should_quit;
SelectedLibraries const& libs;
+ using rmat = dmat<3, 3>;
+
dmat<3, 3> r_b;
double t_b[3];
@@ -40,6 +42,7 @@ private:
void t_compensate(const dmat<3, 3>& rmat, const double* ypr, double* output, bool rz);
void run() override;
+
public:
Tracker(main_settings& s, Mappings& m, SelectedLibraries& libs);
~Tracker();