From 1e6f070677ebcf2649924f682aa66b939fa874f8 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 9 Aug 2015 04:03:40 +0200 Subject: main: remap axis before mapping Issue: #213 --- opentrack/tracker.cpp | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) (limited to 'opentrack/tracker.cpp') diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index e03342e4..acbfded5 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -81,7 +81,12 @@ void Tracker::logic() if (!zero_) for (int i = 0; i < 6; i++) { - value(i) = newpose[i]; + auto& axis = m(i); + int k = axis.opts.src; + if (k < 0 || k >= 6) + value(i) = 0; + else + value(i) = newpose[k]; raw(i) = newpose[i]; } else @@ -157,23 +162,10 @@ void Tracker::logic() for (int i = 0; i < 6; i++) value[i] *= inverts[i] ? -1. : 1.; - Pose output_pose_; - - for (int i = 0; i < 6; i++) - { - auto& axis = m(i); - int k = axis.opts.src; - if (k < 0 || k >= 6) - output_pose_(i) = 0; - else - output_pose_(i) = value(k); - } - - - libs.pProtocol->pose(output_pose_); + libs.pProtocol->pose(value); QMutexLocker foo(&mtx); - output_pose = output_pose_; + output_pose = value; raw_6dof = raw; } -- cgit v1.2.3 From 573a78eeda6423ff4ce72dda9f00c366b20672d5 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 9 Aug 2015 04:04:32 +0200 Subject: main: apply center pose after compensation Issue: #212 --- opentrack/tracker.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) mode change 100644 => 100755 opentrack/tracker.cpp (limited to 'opentrack/tracker.cpp') diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp old mode 100644 new mode 100755 index acbfded5..f31e1185 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -46,7 +46,7 @@ double Tracker::map(double pos, Mapping& axis) axis.curve.setTrackingActive( !altp ); axis.curveAlt.setTrackingActive( altp ); auto& fc = altp ? axis.curveAlt : axis.curve; - return fc.getValue(pos) + axis.opts.zero; + return fc.getValue(pos); } void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output, bool rz) @@ -149,7 +149,7 @@ void Tracker::logic() if (libs.pFilter) libs.pFilter->filter(tmp, value); } - + for (int i = 0; i < 6; i++) value(i) = map(value(i), m(i)); @@ -158,7 +158,10 @@ void Tracker::logic() value, value, s.tcomp_tz); - + + for (int i = 0; i < 6; i++) + value(i) += m(i).opts.zero; + for (int i = 0; i < 6; i++) value[i] *= inverts[i] ? -1. : 1.; -- cgit v1.2.3 From 0f89e2439ac7a89baf371ce3dd522e96bb933c83 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Thu, 13 Aug 2015 09:42:56 +0200 Subject: tracker: use concise syntax for vector access --- opentrack/tracker.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'opentrack/tracker.cpp') diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index f31e1185..e79d32c1 100755 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -55,11 +55,11 @@ void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output, dmat<3, 1> tvec({ xyz[2], -xyz[0], -xyz[1] }); const dmat<3, 1> ret = rmat * tvec; if (!rz) - output[2] = ret(0, 0); + output[2] = ret(0); else output[2] = xyz[2]; - output[1] = -ret(2, 0); - output[0] = -ret(1, 0); + output[1] = -ret(2); + output[0] = -ret(1); } void Tracker::logic() @@ -95,7 +95,7 @@ void Tracker::logic() for (int i = 0; i < 3; i++) { - raw(i+3) = value(i+3) = mat(i, 0) * r2d; + raw(i+3) = value(i+3) = mat(i) * r2d; raw(i) = value(i) = t_b[i]; } } @@ -127,19 +127,19 @@ void Tracker::logic() { centerp = false; for (int i = 0; i < 3; i++) - t_b[i] = t(i, 0); + t_b[i] = t(i); r_b = r; } { - double tmp[3] = { t(0, 0) - t_b[0], t(1, 0) - t_b[1], t(2, 0) - t_b[2] }; + double tmp[3] = { t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2] }; t_compensate(cam, tmp, tmp, false); const rmat m_ = r_b.t() * r; const dmat<3, 1> euler = rmat::rmat_to_euler(m_); for (int i = 0; i < 3; i++) { value(i) = tmp[i]; - value(i+3) = euler(i, 0) * r2d; + value(i+3) = euler(i) * r2d; } } -- cgit v1.2.3 From 1ea849d0f6f934beb69ed72790f02d079a7ba435 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Thu, 13 Aug 2015 09:43:13 +0200 Subject: tracker: no need to run logic() at all --- opentrack/tracker.cpp | 4 ---- 1 file changed, 4 deletions(-) (limited to 'opentrack/tracker.cpp') diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index e79d32c1..22a8933c 100755 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -197,10 +197,6 @@ void Tracker::run() { } { - // do one last pass with origin pose - for (int i = 0; i < 6; i++) - newpose[i] = 0; - logic(); // filter may inhibit exact origin Pose p; libs.pProtocol->pose(p); -- cgit v1.2.3 From cfed344344b996aea1e6388031dc20b1d15a25f3 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 18 Aug 2015 08:51:28 +0200 Subject: simple-mat: replace initializer_list with variadic ctor Gives us type safety rather than argument count mismatch. Also there's no more narrowing conversion issue. Replace usages. Explicitly delete initializer_list ctor. --- opentrack/simple-mat.hpp | 70 ++++++++++++++++++++++++++++++++++++++---------- opentrack/tracker.cpp | 4 +-- pose-widget/glwidget.cpp | 40 +++++++++++++-------------- 3 files changed, 76 insertions(+), 38 deletions(-) (limited to 'opentrack/tracker.cpp') diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp index 9f0911cb..2b4cb72e 100755 --- a/opentrack/simple-mat.hpp +++ b/opentrack/simple-mat.hpp @@ -41,13 +41,43 @@ namespace { enum { P = a == 1 ? 1 : 3 }; enum { Q = a == 1 ? 3 : 1 }; }; + + template struct assignable; + + template + struct assignable { + enum { value = true }; + }; + + template + struct assignable { + enum { value = std::is_assignable::value && assignable::value }; + }; + + template + struct is_arglist_correct + { + enum { value = h * w == sizeof...(ts) && assignable::value }; + }; } template -struct Mat +class Mat { num data[h_][w_]; + static_assert(h_ > 0 && w_ > 0, "must have positive mat dimensions"); + + struct cast + { + template + static num _(u x) { return static_cast(x); } + }; + + Mat(std::initializer_list&& xs) = delete; + +public: + // parameters w_ and h_ are rebound so that SFINAE occurs // removing them causes a compile-time error -sh 20150811 @@ -101,9 +131,9 @@ struct Mat typename std::enable_if::value, Mat::P, is_dim3::Q>>::type cross(const Mat& p2) const { - return Mat({y() * p2.z() - p2.y() * z(), - p2.x() * z() - x() * p2.z(), - x() * p2.y() - y() * p2.x()}); + return Mat(y() * p2.z() - p2.y() * z(), + p2.x() * z() - x() * p2.z(), + x() * p2.y() - y() * p2.x()); } Mat operator+(const Mat& other) const @@ -161,9 +191,9 @@ struct Mat num sum = num(0); for (int k = 0; k < h_; k++) - sum += data[j][k]*other.data[k][i]; + sum += data[j][k]*other(k, i); - ret.data[j][i] = sum; + ret(j, i) = sum; } return ret; @@ -172,19 +202,29 @@ struct Mat inline num operator()(int j, int i) const { return data[j][i]; } inline num& operator()(int j, int i) { return data[j][i]; } - Mat(std::initializer_list&& list) + template::value>> + Mat(ts const&... xs) { - auto iter = list.begin(); - for (int i = 0; i < h_; i++) - for (int j = 0; j < w_; j++) - data[i][j] = *iter++; + const std::initializer_list init = { cast::_(xs)... }; + auto iter = init.begin(); + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + data[j][i] = *iter++; + } + + template + Mat(const t* xs) + { + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + data[j][i] = num(*xs++); } Mat() { for (int j = 0; j < h_; j++) for (int i = 0; i < w_; i++) - data[j][i] = 0; + data[j][i] = num(0); } Mat(const num* mem) @@ -194,6 +234,8 @@ struct Mat data[j][i] = mem[i*h_+j]; } + Mat(num* mem) : Mat(const_cast(mem)) {} + // XXX add more operators as needed, third-party dependencies mostly // not needed merely for matrix algebra -sh 20141030 @@ -237,10 +279,10 @@ struct Mat if (std::abs(pitch_1) + std::abs(roll_1) + std::abs(yaw_1) > std::abs(pitch_2) + std::abs(roll_2) + std::abs(yaw_2)) { bool fix_neg_pitch = pitch_1 < 0; - return dmat<3, 1>({yaw_2, std::fmod(fix_neg_pitch ? -pi - pitch_1 : pitch_2, pi), roll_2}); + return dmat<3, 1>(yaw_2, std::fmod(fix_neg_pitch ? -pi - pitch_1 : pitch_2, pi), roll_2); } else - return dmat<3, 1>({yaw_1, pitch_1, roll_1}); + return dmat<3, 1>(yaw_1, pitch_1, roll_1); } // tait-bryan angles, not euler diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 22a8933c..403e87c5 100755 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -52,7 +52,7 @@ double Tracker::map(double pos, Mapping& axis) 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] }); + dmat<3, 1> tvec( xyz[2], -xyz[0], -xyz[1] ); const dmat<3, 1> ret = rmat * tvec; if (!rz) output[2] = ret(0); @@ -107,7 +107,7 @@ void Tracker::logic() }; const rmat cam = rmat::euler_to_rmat(off); rmat r = rmat::euler_to_rmat(&value[Yaw]); - dmat<3, 1> t { value(0), value(1), value(2) }; + dmat<3, 1> t(value(0), value(1), value(2)); r = cam * r; diff --git a/pose-widget/glwidget.cpp b/pose-widget/glwidget.cpp index 6c04c4e6..334c845b 100755 --- a/pose-widget/glwidget.cpp +++ b/pose-widget/glwidget.cpp @@ -45,14 +45,10 @@ void GLWidget::rotateBy(float xAngle, float yAngle, float zAngle, float x, float float c3 = cos(zAngle / 57.295781); float s3 = sin(zAngle / 57.295781); - float foo[] = { - c2*c3, -c2*s3, s2, - c1*s3+c3*s1*s2, c1*c3-s1*s2*s3, -c2*s1, - s1*s3-c1*c3*s2, c3*s1+c1*s2*s3, c1*c2, - }; - - rotation = rmat(foo); - translation = vec3({x, y, z}); + rotation = rmat(c2*c3, -c2*s3, s2, + c1*s3+c3*s1*s2, c1*c3-s1*s2*s3, -c2*s1, + s1*s3-c1*c3*s2, c3*s1+c1*s2*s3, c1*c2); + translation = vec3(x, y, z); update(); } @@ -68,8 +64,8 @@ public: const vec2& p3) { origin = p1; - v0 = vec2({ p3.x() - p1.x(), p3.y() - p1.y() }); - v1 = vec2({ p2.x() - p1.x(), p2.y() - p1.y() }); + v0 = vec2(p3.x() - p1.x(), p3.y() - p1.y()); + v1 = vec2(p2.x() - p1.x(), p2.y() - p1.y()); dot00 = v0.dot(v0); dot01 = v0.dot(v1); dot11 = v1.dot(v1); @@ -82,7 +78,7 @@ public: const num dot02 = v0.dot(v2); const num u = (dot11 * dot02 - dot01 * dot12) * invDenom; const num v = (dot00 * dot12 - dot01 * dot02) * invDenom; - uv = vec2({u, v}); + uv = vec2(u, v); return (u >= 0) && (v >= 0) && (u + v <= 1); } @@ -107,19 +103,19 @@ void GLWidget::project_quad_texture() { const int sx = width(), sy = height(); vec2 pt[4]; const vec3 corners[] = { - vec3({0., 0, 0}), - vec3({sx-1, 0, 0}), - vec3({0, sy-1, 0}), - vec3({sx-1, sy-1, 0.}) + vec3(0., 0, 0), + vec3(sx-1, 0, 0), + vec3(0, sy-1, 0), + vec3(sx-1, sy-1, 0.) }; for (int i = 0; i < 4; i++) { - pt[i] = project(vec3({corners[i].x() - sx/2, corners[i].y() - sy/2, 0})); + pt[i] = project(vec3(corners[i].x() - sx/2, corners[i].y() - sy/2, 0)); pt[i].x() += sx/2.; pt[i].y() += sy/2.; } - vec3 normal1({0, 0, 1}); + vec3 normal1(0, 0, 1); vec3 normal2; { vec3 foo[3]; @@ -137,15 +133,15 @@ void GLWidget::project_quad_texture() { vec2 p2[4]; for (int i = 0; i < 4; i++) - p2[i] = vec2({pt[i].x(), pt[i].y()}); + p2[i] = vec2(pt[i].x(), pt[i].y()); QImage texture(QSize(sx, sy), QImage::Format_RGB888); QColor bgColor = palette().color(QPalette::Current, QPalette::Window); texture.fill(bgColor); const vec2 projected[2][3] = { { p2[0], p2[1], p2[2] }, { p2[3], p2[1], p2[2] } }; const vec2 origs[2][3] = { - { vec2({0, 0}), vec2({ow-1, 0}), vec2({0, oh-1}) }, - { vec2({ow-1, oh-1}), vec2({ow-1, 0}), vec2({0, oh-1}) } + { vec2(0, 0), vec2(ow-1, 0), vec2(0, oh-1) }, + { vec2(ow-1, oh-1), vec2(ow-1, 0), vec2(0, oh-1) } }; const Triangle triangles[2] = { Triangle(projected[0][0], projected[0][1], projected[0][2]), @@ -167,7 +163,7 @@ void GLWidget::project_quad_texture() { for (int y = 0; y < sy; y++) for (int x = 0; x < sx; x++) { - vec2 pos({x, y}); + vec2 pos(x, y); for (int i = 0; i < 2; i++) { vec2 uv; if (triangles[i].barycentric_coords(pos, uv)) @@ -205,7 +201,7 @@ GLWidget::vec2 GLWidget::project(const vec3 &point) num y = h * translation.y() / 2 / -40; if (std::abs(y) > h/2) y = y > 0 ? h/2 : h/-2; - return vec2 { z * (ret.x() + x), z * (ret.y() + y) }; + return vec2(z * (ret.x() + x), z * (ret.y() + y)); } GLWidget::vec3 GLWidget::project2(const vec3 &point) -- cgit v1.2.3 From 21cd2c012ef23ec3f4b374d82056f1fa1694d904 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 22 Aug 2015 19:02:57 +0200 Subject: props changed only Some source files had executable bit on. Remove it. --- CMakeLists.txt | 0 bin/NPClient.dll | Bin bin/NPClient64.dll | Bin bin/freetrackclient.dll | Bin clientfiles/make-csv.pl | 0 csv/csv.cpp | 0 facetracknoir/keyboard.h | 0 facetracknoir/process_detector.cpp | 0 facetracknoir/process_detector.h | 0 ftnoir_filter_kalman/ftnoir_filter_kalman.h | 0 ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp | 0 ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 0 ftnoir_tracker_aruco/include/markerdetector.h | 0 ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 0 ftnoir_tracker_udp/ftnoir_tracker_udp.cpp | 0 opentrack-compat/process-list.hpp | 0 opentrack/simple-mat.hpp | 0 opentrack/tracker.cpp | 0 opentrack/win32-shortcuts.cpp | 0 opentrack/win32-shortcuts.h | 0 pose-widget/glwidget.cpp | 0 pose-widget/glwidget.h | 0 qfunctionconfigurator/functionconfig.cpp | 0 23 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 CMakeLists.txt mode change 100755 => 100644 bin/NPClient.dll mode change 100755 => 100644 bin/NPClient64.dll mode change 100755 => 100644 bin/freetrackclient.dll mode change 100755 => 100644 clientfiles/make-csv.pl mode change 100755 => 100644 csv/csv.cpp mode change 100755 => 100644 facetracknoir/keyboard.h mode change 100755 => 100644 facetracknoir/process_detector.cpp mode change 100755 => 100644 facetracknoir/process_detector.h mode change 100755 => 100644 ftnoir_filter_kalman/ftnoir_filter_kalman.h mode change 100755 => 100644 ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp mode change 100755 => 100644 ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp mode change 100755 => 100644 ftnoir_tracker_aruco/include/markerdetector.h mode change 100755 => 100644 ftnoir_tracker_pt/ftnoir_tracker_pt.cpp mode change 100755 => 100644 ftnoir_tracker_udp/ftnoir_tracker_udp.cpp mode change 100755 => 100644 opentrack-compat/process-list.hpp mode change 100755 => 100644 opentrack/simple-mat.hpp mode change 100755 => 100644 opentrack/tracker.cpp mode change 100755 => 100644 opentrack/win32-shortcuts.cpp mode change 100755 => 100644 opentrack/win32-shortcuts.h mode change 100755 => 100644 pose-widget/glwidget.cpp mode change 100755 => 100644 pose-widget/glwidget.h mode change 100755 => 100644 qfunctionconfigurator/functionconfig.cpp (limited to 'opentrack/tracker.cpp') diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/bin/NPClient.dll b/bin/NPClient.dll old mode 100755 new mode 100644 diff --git a/bin/NPClient64.dll b/bin/NPClient64.dll old mode 100755 new mode 100644 diff --git a/bin/freetrackclient.dll b/bin/freetrackclient.dll old mode 100755 new mode 100644 diff --git a/clientfiles/make-csv.pl b/clientfiles/make-csv.pl old mode 100755 new mode 100644 diff --git a/csv/csv.cpp b/csv/csv.cpp old mode 100755 new mode 100644 diff --git a/facetracknoir/keyboard.h b/facetracknoir/keyboard.h old mode 100755 new mode 100644 diff --git a/facetracknoir/process_detector.cpp b/facetracknoir/process_detector.cpp old mode 100755 new mode 100644 diff --git a/facetracknoir/process_detector.h b/facetracknoir/process_detector.h old mode 100755 new mode 100644 diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h old mode 100755 new mode 100644 diff --git a/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp b/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp old mode 100755 new mode 100644 diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp old mode 100755 new mode 100644 diff --git a/ftnoir_tracker_aruco/include/markerdetector.h b/ftnoir_tracker_aruco/include/markerdetector.h old mode 100755 new mode 100644 diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp old mode 100755 new mode 100644 diff --git a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp old mode 100755 new mode 100644 diff --git a/opentrack-compat/process-list.hpp b/opentrack-compat/process-list.hpp old mode 100755 new mode 100644 diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp old mode 100755 new mode 100644 diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp old mode 100755 new mode 100644 diff --git a/opentrack/win32-shortcuts.cpp b/opentrack/win32-shortcuts.cpp old mode 100755 new mode 100644 diff --git a/opentrack/win32-shortcuts.h b/opentrack/win32-shortcuts.h old mode 100755 new mode 100644 diff --git a/pose-widget/glwidget.cpp b/pose-widget/glwidget.cpp old mode 100755 new mode 100644 diff --git a/pose-widget/glwidget.h b/pose-widget/glwidget.h old mode 100755 new mode 100644 diff --git a/qfunctionconfigurator/functionconfig.cpp b/qfunctionconfigurator/functionconfig.cpp old mode 100755 new mode 100644 -- cgit v1.2.3 From e07a72493f6145b55efb305c955032ffd73e94c3 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 28 Aug 2015 12:28:29 +0200 Subject: tracker: change centering order again Needs more testing with PT and other non-inertial trackers. Issue: #227 --- opentrack/tracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'opentrack/tracker.cpp') diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 403e87c5..c9e34c2d 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -134,7 +134,7 @@ void Tracker::logic() { double tmp[3] = { t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2] }; t_compensate(cam, tmp, tmp, false); - const rmat m_ = r_b.t() * r; + const rmat m_ = r * r_b.t(); const dmat<3, 1> euler = rmat::rmat_to_euler(m_); for (int i = 0; i < 3; i++) { -- cgit v1.2.3