diff options
Diffstat (limited to 'opentrack/tracker.cpp')
| -rw-r--r-- | opentrack/tracker.cpp | 24 |
1 files changed, 18 insertions, 6 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index be0ecd84..a6433da5 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -26,6 +26,7 @@ Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) : m(m), centerp(false), enabledp(true), + zero_(false), should_quit(false), libs(libs), r_b(dmat<3,3>::eye()), @@ -127,12 +128,26 @@ void Tracker::logic() m(5).opts.invert, }; + static constexpr double pi = 3.141592653; + static constexpr double r2d = 180. / pi; + Pose value, raw; - for (int i = 0; i < 6; i++) + if (!zero_) + for (int i = 0; i < 6; i++) + { + value(i) = newpose[i]; + raw(i) = newpose[i]; + } + else { - value(i) = newpose[i]; - raw(i) = newpose[i]; + auto mat = rmat_to_euler(r_b); + + for (int i = 0; i < 3; i++) + { + raw(i+3) = value(i+3) = mat(i, 0) * r2d; + raw(i) = value(i) = t_b[i]; + } } if (centerp) @@ -149,9 +164,6 @@ void Tracker::logic() const dmat<3, 1> euler = rmat_to_euler(m_); for (int i = 0; i < 3; i++) { - static constexpr double pi = 3.141592653; - static constexpr double r2d = 180. / pi; - value(i) -= t_b[i]; value(i+3) = euler(i, 0) * r2d; } |