summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--opentrack-logic/tracker.cpp30
-rw-r--r--opentrack-logic/tracker.h3
-rw-r--r--opentrack/simple-mat.cpp6
-rw-r--r--opentrack/simple-mat.hpp6
4 files changed, 32 insertions, 13 deletions
diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index 3fb5c6fc..6557b061 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -110,6 +110,7 @@ void Tracker::logic()
static constexpr double pi = 3.141592653;
static constexpr double r2d = 180. / pi;
+ static constexpr double d2r = pi / 180.;
using namespace euler;
@@ -131,12 +132,17 @@ void Tracker::logic()
const double off[] =
{
- (double)-s.camera_yaw,
- (double)-s.camera_pitch,
- (double)-s.camera_roll
+ d2r * (double)-s.camera_yaw,
+ d2r * (double)-s.camera_pitch,
+ d2r * (double)-s.camera_roll
};
const rmat cam = euler_to_rmat(off);
- rmat r = euler_to_rmat(&value[Yaw]);
+ rmat r;
+ {
+ euler_t tmp(&value[Yaw]);
+ tmp = d2r * tmp;
+ r = euler_to_rmat(&tmp[0]);
+ }
euler_t t(value(0), value(1), value(2));
r = cam * r;
@@ -179,12 +185,12 @@ void Tracker::logic()
m_ = r_b.t() * r;
}
- const euler_t euler = rmat_to_euler(m_);
+ const euler_t euler = r2d * rmat_to_euler(m_);
for (int i = 0; i < 3; i++)
{
value(i) = tmp[i];
- value(i+3) = euler(i) * r2d;
+ value(i+3) = euler(i);
}
}
@@ -207,10 +213,15 @@ void Tracker::logic()
value(i) = map(value(i), m(i));
if (s.tcomp_p)
- t_compensate(euler_to_rmat(&value[Yaw]),
+ {
+ euler_t value_(value(Yaw) * d2r,
+ value(Pitch) * d2r,
+ value(Roll) * d2r);
+ t_compensate(euler_to_rmat(&value_[0]),
value,
value,
s.tcomp_tz);
+ }
for (int i = 0; i < 6; i++)
value(i) += m(i).opts.zero;
@@ -266,9 +277,10 @@ void Tracker::run()
logic();
- long q = sleep_ms * 1000L - t.elapsed()/1000L;
+ long q = long(sleep_ms * 1000L - t.elapsed()/1000L);
using std::max;
- usleep(max(1L, q));
+ using ulong = unsigned long;
+ usleep(ulong(max(1L, q)));
}
{
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index dad955c8..1c29fb8d 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -61,7 +61,8 @@ private:
volatile bool should_quit;
SelectedLibraries const& libs;
- using rmat = dmat<3, 3>;
+ using rmat = euler::rmat;
+ using euler_t = euler::euler_t;
rmat r_b;
double t_b[3];
diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp
index 4ab68e0f..367cf365 100644
--- a/opentrack/simple-mat.cpp
+++ b/opentrack/simple-mat.cpp
@@ -36,9 +36,9 @@ euler_t rmat_to_euler(const dmat<3, 3>& R)
// tait-bryan angles, not euler
rmat euler_to_rmat(const double* input)
{
- auto H = input[0] * pi / 180;
- auto P = input[1] * pi / 180;
- auto B = input[2] * pi / 180;
+ const double H = input[0];
+ const double P = input[1];
+ const double B = input[2];
const auto c1 = cos(H);
const auto s1 = sin(H);
diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index f5680ec4..8dc03014 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -253,3 +253,9 @@ euler_t OPENTRACK_API_EXPORT rmat_to_euler(const dmat<3, 3>& R);
} // end ns euler
template<int h_, int w_> using dmat = Mat<double, h_, w_>;
+
+template<typename num, int h, int w>
+inline Mat<num, h, w> operator*(num scalar, const Mat<num, h, w>& mat)
+{
+ return mat * scalar;
+}