From 235e26cc63fe401affafed8a5b4fe52a134fc376 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Mon, 5 Feb 2018 09:33:00 +0100
Subject: [WIP] logic/pipeline: simplification
NEEDS TESTING. DO NOT COMMIT!!!
---
logic/pipeline.cpp | 433 ++++++++++++++++++++++++++++-------------------------
1 file changed, 233 insertions(+), 200 deletions(-)
(limited to 'logic/pipeline.cpp')
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 9059a251..c057b274 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -35,40 +35,39 @@ static constexpr inline double d2r = M_PI / 180.;
reltrans::reltrans() {}
-euler_t reltrans::rotate(const rmat& rmat, const euler_t& xyz,
- bool disable_tx, bool disable_ty, bool disable_tz) const
+euler_t reltrans::rotate(const rmat& R, const euler_t& in, vec3_bool disable) const
{
enum { tb_Z, tb_X, tb_Y };
// TY is really yaw axis. need swapping accordingly.
// sign changes are due to right-vs-left handedness of coordinate system used
- const euler_t ret = rmat * euler_t(xyz(TZ), -xyz(TX), -xyz(TY));
+ const euler_t ret = R * euler_t(in(TZ), -in(TX), -in(TY));
euler_t output;
- if (disable_tz)
- output(TZ) = xyz(TZ);
+ if (disable(TZ))
+ output(TZ) = in(TZ);
else
output(TZ) = ret(tb_Z);
- if (disable_ty)
- output(TY) = xyz(TY);
+ if (disable(TY))
+ output(TY) = in(TY);
else
output(TY) = -ret(tb_Y);
- if (disable_tx)
- output(TX) = xyz(TX);
+ if (disable(TX))
+ output(TX) = in(TX);
else
output(TX) = -ret(tb_X);
return output;
}
-Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat<bool, 6, 1>& disable)
+Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec6_bool& disable)
{
if (state != reltrans_disabled)
{
- euler_t rel { value(TX), value(TY), value(TZ) };
+ euler_t rel { &value[0] };
{
bool tcomp_in_zone_ = progn(
@@ -96,21 +95,11 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat
// only when looking behind or downward
if (in_zone)
{
- const double tcomp_c[] = {
- double(!disable(Yaw)),
- double(!disable(Pitch)),
- double(!disable(Roll)),
- };
-
- const rmat R = euler_to_rmat(euler_t(value(Yaw) * d2r * tcomp_c[0],
- value(Pitch) * d2r * tcomp_c[1],
- value(Roll) * d2r * tcomp_c[2]));
-
- rel = rotate(R,
- rel,
- disable(TX),
- disable(TY),
- disable(TZ));
+ const rmat R = euler_to_rmat(euler_t(value(Yaw) * d2r * !disable(Yaw),
+ value(Pitch) * d2r * !disable(Pitch),
+ value(Roll) * d2r * !disable(Roll)));
+
+ rel = rotate(R, rel, &disable[TX]);
}
if (cur)
@@ -153,6 +142,23 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat
}
}
+euler_t reltrans::apply_neck(const Pose& value, bool enable, int nz) const
+{
+ if (!enable)
+ return {};
+
+ euler_t neck;
+
+ if (nz != 0)
+ {
+ const rmat R = euler_to_rmat(euler_t(&value[Yaw]) * d2r);
+ neck = rotate(R, { 0, 0, nz }, vec3_bool());
+ neck(TZ) = neck(TZ) - nz;
+ }
+
+ return neck;
+}
+
pipeline::pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger) :
m(m),
ev(ev),
@@ -191,78 +197,104 @@ static bool is_nan(const dmat<u,w>& r)
}
template<typename x, typename y, typename... xs>
-static inline bool nan_check_(const x& datum, const y& next, const xs&... rest)
+static force_inline bool nan_check_(const x& datum, const y& next, const xs&... rest)
{
- return !is_nan(datum) && nan_check_(next, rest...);
+ return is_nan(datum) || nan_check_(next, rest...);
}
template<typename x>
-static inline bool nan_check_(const x& datum)
+static force_inline bool nan_check_(const x& datum)
{
- return !is_nan(datum);
+ return is_nan(datum);
}
template<typename>
static bool nan_check_() = delete;
static never_inline
-void emit_nan_check_msg(const char* text, int line)
+void emit_nan_check_msg(const char* text, const char* fun, int line)
{
- once_only(qDebug() << "nan check failed"
- << "line:" << text
- << "for:" << line);
+ once_only(
+ qDebug() << "nan check failed"
+ << "for:" << text
+ << "function:" << fun
+ << "line:" << line;
+ );
}
-#define nan_check(...) \
- do \
- { \
- if (!nan_check_(__VA_ARGS__)) \
- { \
- emit_nan_check_msg(#__VA_ARGS__, __LINE__); \
- goto nan; \
- } \
- } while (false)
-
-void pipeline::logic()
+template<typename... xs>
+static never_inline
+bool maybe_nan(const char* text, const char* fun, int line, const xs&... vals)
{
- using namespace euler;
- using EV = event_handler::event_ordinal;
-
- logger.write_dt();
- logger.reset_dt();
-
- // we must center prior to getting data
- const bool center_ordered = get(f_center) && tracking_started;
- set(f_center, false);
- const bool own_center_logic = center_ordered && libs.pTracker->center();
+ if (nan_check_(vals...))
+ {
+ emit_nan_check_msg(text, fun, line);
+ return true;
+ }
+ return false;
+}
- Pose value, raw;
+#if defined _MSC_VER
+# define OTR_FUNNAME2 (__FUNCSIG__)
+#else
+# define OTR_FUNNAME2 (__PRETTY_FUNCTION__)
+#endif
+// don't expand
+# define OTR_FUNNAME (OTR_FUNNAME2)
+
+#define nan_check(...) \
+ do \
+ { \
+ if (maybe_nan(#__VA_ARGS__, OTR_FUNNAME, __LINE__, __VA_ARGS__)) \
+ goto nan; \
+ } while (false)
+void pipeline::maybe_enable_center_on_tracking_started()
+{
+ if (!tracking_started)
{
- Pose tmp;
- libs.pTracker->data(tmp);
- nan_check(tmp);
- ev.run_events(EV::ev_raw, tmp);
+ for (int i = 0; i < 6; i++)
+ if (std::fabs(newpose(i)) != 0)
+ {
+ tracking_started = true;
+ break;
+ }
- if (get(f_enabled_p) ^ !get(f_enabled_h))
- for (int i = 0; i < 6; i++)
- newpose(i) = tmp(i);
+ if (tracking_started && s.center_at_startup)
+ set(f_center, true);
}
+}
- bool disabled[6];
+void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
+{
+ euler_t tmp = d2r * euler_t(&value[Yaw]);
+ scaled_rotation.rotation = euler_to_rmat(c_div * tmp);
+ real_rotation.rotation = euler_to_rmat(tmp);
- for (int i = 0; i < 6; i++)
+ if (get(f_center))
{
- auto& axis = m(i);
- int k = axis.opts.src;
- disabled[i] = k == 6;
- if (k < 0 || k >= 6)
- value(i) = 0;
+ if (libs.pFilter)
+ libs.pFilter->center();
+
+ if (own_center_logic)
+ {
+ scaled_rotation.rot_center = rmat::eye();
+ real_rotation.rot_center = rmat::eye();
+
+ t_center = euler_t();
+ }
else
- value(i) = newpose(k);
- raw(i) = newpose(i);
+ {
+ real_rotation.rot_center = real_rotation.rotation.t();
+ scaled_rotation.rot_center = scaled_rotation.rotation.t();
+
+ t_center = euler_t(static_cast<const double*>(value));
+ }
}
+}
+Pose pipeline::clamp_value(Pose value) const
+{
// hatire, udp, and freepie trackers can mess up here
for (unsigned i = 3; i < 6; i++)
{
@@ -279,159 +311,162 @@ void pipeline::logic()
value(i) = clamp(x, -180, 180);
}
- logger.write_pose(raw); // raw
+ return value;
+}
- // TODO split this function, it's too big
+Pose pipeline::apply_center(Pose value) const
+{
+ rmat rotation = scaled_rotation.rotation * scaled_rotation.rot_center;
+ euler_t pos = euler_t(value) - t_center;
+ euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
+
+ pos = rel.rotate(real_rotation.rot_center, pos, vec3_bool());
+ for (int i = 0; i < 3; i++)
{
- euler_t tmp = d2r * euler_t(&value[Yaw]);
- scaled_rotation.rotation = euler_to_rmat(c_div * tmp);
- real_rotation.rotation = euler_to_rmat(tmp);
+ // don't invert after t_compensate
+ // inverting here doesn't break centering
+
+ if (m(i+3).opts.invert)
+ rot(i) = -rot(i);
+ if (m(i).opts.invert)
+ pos(i) = -pos(i);
}
- if (!tracking_started)
+ for (int i = 0; i < 3; i++)
{
- for (int i = 0; i < 6; i++)
- if (std::fabs(newpose(i)) != 0)
- {
- tracking_started = true;
- break;
- }
-
- if (tracking_started && s.center_at_startup)
- {
- set(f_center, true);
- }
+ value(i) = pos(i);
+ value(i+3) = rot(i);
}
- if (center_ordered)
+ return value;
+}
+
+std::tuple<Pose, Pose, vec6_bool>
+pipeline::get_selected_axis_value(const Pose& newpose) const
+{
+ Pose value;
+ vec6_bool disabled;
+
+ for (int i = 0; i < 6; i++)
{
- if (libs.pFilter)
- libs.pFilter->center();
+ const Map& axis = m(i);
+ const int k = axis.opts.src;
- if (own_center_logic)
- {
- scaled_rotation.rot_center = rmat::eye();
- real_rotation.rot_center = rmat::eye();
+ disabled(i) = k == 6;
- t_center = euler_t();
- }
+ if (k < 0 || k >= 6)
+ value(i) = 0;
else
- {
- real_rotation.rot_center = real_rotation.rotation.t();
- scaled_rotation.rot_center = scaled_rotation.rotation.t();
-
- t_center = euler_t(&value(TX));
- }
+ value(i) = newpose(k);
}
- {
- rmat rotation = scaled_rotation.rotation;
- euler_t pos = euler_t(&value[TX]) - t_center;
+ return { newpose, value, disabled };
+}
- //switch (s.center_method)
- switch (1)
- {
- case 0:
- // inertial
- rotation = scaled_rotation.rot_center * rotation;
- break;
-
- default:
- case 1:
- // camera
- rotation = rotation * scaled_rotation.rot_center;
- pos = rel.rotate(real_rotation.rot_center, pos, false, false, false);
- break;
- }
+Pose pipeline::maybe_apply_filter(const Pose& value) const
+{
+ Pose tmp(value);
- euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
+ // nan/inf values will corrupt filter internal state
+ if (libs.pFilter)
+ libs.pFilter->filter(value, tmp);
- for (int i = 0; i < 3; i++)
- {
- // don't invert after t_compensate
- // inverting here doesn't break centering
+ return tmp;
+}
- if (m(i+3).opts.invert)
- rot(i) = -rot(i);
- if (m(i).opts.invert)
- pos(i) = -pos(i);
- }
+Pose pipeline::apply_zero_pos(Pose value) const
+{
+ // custom zero position
+ for (int i = 0; i < 6; i++)
+ value(i) += m(i).opts.zero * (m(i).opts.invert ? -1 : 1);
- for (int i = 0; i < 3; i++)
- {
- value(i) = pos(i);
- value(i+3) = rot(i);
- }
- }
+ return value;
+}
- logger.write_pose(value); // "corrected" - after various transformations to account for camera position
+Pose pipeline::apply_reltrans(Pose value, vec6_bool disabled)
+{
+ const euler_t neck = rel.apply_neck(value, s.neck_enable, -s.neck_z);
- ev.run_events(EV::ev_before_filter, value);
+ value = rel.apply_pipeline(s.reltrans_mode, value,
+ { !!s.reltrans_disable_src_yaw,
+ !!s.reltrans_disable_src_pitch,
+ !!s.reltrans_disable_src_roll,
+ !!s.reltrans_disable_tx,
+ !!s.reltrans_disable_ty,
+ !!s.reltrans_disable_tz });
- {
- {
- Pose tmp(value);
+ for (int i = 0; i < 3; i++)
+ value(i) += neck(i);
- // nan/inf values will corrupt filter internal state
- if (libs.pFilter)
- libs.pFilter->filter(tmp, value);
+ // reltrans will move it
+ for (unsigned k = 0; k < 6; k++)
+ if (disabled(k))
+ value(k) = 0;
- nan_check(value);
+ return value;
+}
- logger.write_pose(value); // "filtered"
- }
- }
+void pipeline::logic()
+{
+ using namespace euler;
+ using EV = event_handler::event_ordinal;
- {
- ev.run_events(EV::ev_before_mapping, value);
+ logger.write_dt();
+ logger.reset_dt();
- {
- euler_t neck;
+ // we must center prior to getting data from the tracker
+ const bool center_ordered = get(f_center) && tracking_started;
+ const bool own_center_logic = center_ordered && libs.pTracker->center();
- if (s.neck_enable)
- {
- double nz = -s.neck_z;
+ Pose value, raw;
+ vec6_bool disabled;
- if (nz != 0)
- {
- const rmat R = euler_to_rmat(
- euler_t(value(Yaw) * d2r,
- value(Pitch) * d2r,
- value(Roll) * d2r));
- neck = rel.rotate(R, { 0, 0, nz }, false, false, false);
- neck(TZ) = neck(TZ) - nz;
-
- nan_check(neck);
- }
- }
+ {
+ Pose tmp;
+ libs.pTracker->data(tmp);
+ nan_check(tmp);
+ ev.run_events(EV::ev_raw, tmp);
+
+ if (get(f_enabled_p) ^ !get(f_enabled_h))
+ for (int i = 0; i < 6; i++)
+ newpose(i) = tmp(i);
+ }
- // CAVEAT rotation only, due to tcomp
- for (int i = 3; i < 6; i++)
- value(i) = map(value(i), m(i));
+ std::tie(raw, value, disabled) = get_selected_axis_value(newpose);
+ logger.write_pose(raw); // raw
- nan_check(value);
+ value = clamp_value(value);
- value = rel.apply_pipeline(s.reltrans_mode, value, {
- !!s.reltrans_disable_src_yaw, !!s.reltrans_disable_src_pitch, !!s.reltrans_disable_src_roll,
- !!s.reltrans_disable_tx, !!s.reltrans_disable_ty, !!s.reltrans_disable_tz
- });
+ {
+ maybe_enable_center_on_tracking_started();
+ maybe_set_center_pose(value, own_center_logic);
+ value = apply_center(value);
+ logger.write_pose(value); // "corrected" - after various transformations to account for camera position
+ }
- for (int i = 0; i < 3; i++)
- value(i) += neck(i);
- }
+ {
+ ev.run_events(EV::ev_before_filter, value);
+ value = maybe_apply_filter(value);
+ nan_check(value);
+ logger.write_pose(value); // "filtered"
+ }
- // relative translation can move it
- for (unsigned k = 0; k < 6; k++)
- if (disabled[k])
- value(k) = 0;
+ {
+ ev.run_events(EV::ev_before_mapping, value);
+ // CAVEAT rotation only, due to tcomp
+ for (int i = 3; i < 6; i++)
+ value(i) = map(value(i), m(i));
}
- // CAVEAT translation only, due to tcomp
- for (int i = 0; i < 3; i++)
- value(i) = map(value(i), m(i));
+ value = apply_reltrans(value, disabled);
- nan_check(value);
+ {
+ // CAVEAT translation only, due to tcomp
+ for (int i = 0; i < 3; i++)
+ value(i) = map(value(i), m(i));
+ nan_check(value);
+ }
goto ok;
@@ -444,21 +479,20 @@ nan:
// for widget last value display
for (int i = 0; i < 6; i++)
- (void) map(raw_6dof(i), m(i));
+ (void)map(raw_6dof(i), m(i));
}
ok:
+ set(f_center, false);
+
if (get(f_zero))
for (int i = 0; i < 6; i++)
value(i) = 0;
- // custom zero position
- for (int i = 0; i < 6; i++)
- value(i) += m(i).opts.zero * (m(i).opts.invert ? -1 : 1);
+ value = apply_zero_pos(value);
ev.run_events(EV::ev_finished, value);
-
libs.pProtocol->pose(value);
QMutexLocker foo(&mtx);
@@ -553,16 +587,15 @@ void pipeline::raw_and_mapped_pose(double* mapped, double* raw) const
}
}
-void pipeline::center() { set(f_center, true); }
+void pipeline::set_center() { set(f_center, true); }
-void pipeline::set_toggle(bool value) { set(f_enabled_h, value); }
+void pipeline::set_enabled(bool value) { set(f_enabled_h, value); }
void pipeline::set_zero(bool value) { set(f_zero, value); }
-void pipeline::zero() { negate(f_zero); }
+void pipeline::toggle_zero() { negate(f_zero); }
void pipeline::toggle_enabled() { negate(f_enabled_p); }
-
-void bits::set(bits::flags flag_, bool val_)
+void bits::set(flags flag_, bool val_)
{
const unsigned flag = unsigned(flag_);
const unsigned val = unsigned(val_);
@@ -578,7 +611,7 @@ void bits::set(bits::flags flag_, bool val_)
}
}
-void bits::negate(bits::flags flag_)
+void bits::negate(flags flag_)
{
const unsigned flag = unsigned(flag_);
@@ -594,7 +627,7 @@ void bits::negate(bits::flags flag_)
}
}
-bool bits::get(bits::flags flag)
+bool bits::get(flags flag)
{
return !!(b & flag);
}
--
cgit v1.2.3