summaryrefslogtreecommitdiffhomepage
path: root/logic
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2018-01-03 14:27:09 +0100
committerStanislaw Halik <sthalik@misaki.pl>2018-01-03 14:35:50 +0100
commit672e1df0c4a6fe4f5cca0bd76172feb86060b2a4 (patch)
treef74034d8e376d4ec6d42ef8e7a280930942b356a /logic
parente181756fa0fcd4c1c79fc1b84590491106797729 (diff)
logic/pipeline: simplify NaN check control flow
Diffstat (limited to 'logic')
-rw-r--r--logic/pipeline.cpp88
1 files changed, 52 insertions, 36 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index bab7f5e1..7d073b28 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -57,7 +57,7 @@ double pipeline::map(double pos, Map& axis)
}
void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& output,
- bool disable_tx, bool disable_ty, bool disable_tz)
+ bool disable_tx, bool disable_ty, bool disable_tz)
{
enum { tb_Z, tb_X, tb_Y };
@@ -81,17 +81,6 @@ void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& outpu
output(TX) = -ret(tb_X);
}
-static inline double elide_nan(double value, double def)
-{
- if (nanp(value))
- {
- if (nanp(def))
- return 0;
- return def;
- }
- return value;
-}
-
template<int u, int w>
static bool is_nan(const dmat<u,w>& r)
{
@@ -105,6 +94,32 @@ static bool is_nan(const dmat<u,w>& r)
constexpr double pipeline::c_mult;
constexpr double pipeline::c_div;
+template<typename x, typename y, typename... xs>
+static inline bool nan_check_(const x& datum, const y& next, const xs&... rest)
+{
+ return !is_nan(datum) && nan_check_(next, rest...);
+}
+
+template<typename x>
+static inline bool nan_check_(const x& datum)
+{
+ return !is_nan(datum);
+}
+
+template<typename>
+static bool nan_check_() = delete;
+
+#define nan_check(...) \
+ do \
+ { \
+ if (!nan_check_(__VA_ARGS__)) \
+ { \
+ once_only(qDebug() << "nan check failed" \
+ << "line:" << __LINE__ \
+ << "for:" << #__VA_ARGS__); \
+ goto nan; \
+ } \
+ } while (false)
void pipeline::logic()
{
@@ -118,17 +133,19 @@ void pipeline::logic()
set(f_center, false);
const bool own_center_logic = center_ordered && libs.pTracker->center();
+ Pose value, raw;
+
{
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) = elide_nan(tmp(i), newpose(i));
+ newpose(i) = tmp(i);
}
- Pose value, raw;
bool disabled[6];
for (int i = 0; i < 6; i++)
@@ -161,8 +178,6 @@ void pipeline::logic()
logger.write_pose(raw); // raw
- bool nanp = is_nan(raw) | is_nan(value);
-
// TODO split this function, it's too big
{
@@ -171,8 +186,6 @@ void pipeline::logic()
real_rotation.rotation = euler_to_rmat(tmp);
}
- nanp |= is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation);
-
if (!tracking_started)
{
using std::fabs;
@@ -184,8 +197,6 @@ void pipeline::logic()
break;
}
- tracking_started &= !nanp;
-
if (tracking_started && s.center_at_startup)
{
set(f_center, true);
@@ -220,17 +231,17 @@ void pipeline::logic()
//switch (s.center_method)
switch (1)
{
- // inertial
case 0:
+ // inertial
rotation = scaled_rotation.rot_center * rotation;
- break;
- // camera
+ break;
+
default:
case 1:
+ // camera
rotation = rotation * scaled_rotation.rot_center;
t_compensate(real_rotation.rot_center, pos, pos, false, false, false);
-
- break;
+ break;
}
euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
@@ -253,26 +264,24 @@ void pipeline::logic()
}
}
- ev.run_events(EV::ev_before_filter, value);
-
logger.write_pose(value); // "corrected" - after various transformations to account for camera position
- nanp |= is_nan(value);
+ ev.run_events(EV::ev_before_filter, value);
{
{
Pose tmp(value);
// nan/inf values will corrupt filter internal state
- if (!nanp && libs.pFilter)
+ if (libs.pFilter)
libs.pFilter->filter(tmp, value);
+ nan_check(value);
+
logger.write_pose(value); // "filtered"
}
}
- nanp |= is_nan(value);
-
{
ev.run_events(EV::ev_before_mapping, value);
@@ -293,6 +302,8 @@ void pipeline::logic()
neck(TX) = xyz(TX);
neck(TY) = xyz(TY);
neck(TZ) = xyz(TZ) - nz;
+
+ nan_check(Pose(0, 0, 0, neck(TX), neck(TY), neck(TZ)));
}
}
@@ -300,6 +311,8 @@ void pipeline::logic()
for (int i = 3; i < 6; i++)
value(i) = map(value(i), m(i));
+ nan_check(value);
+
if (s.tcomp_p)
{
const double tcomp_c[] =
@@ -332,15 +345,17 @@ void pipeline::logic()
for (unsigned k = 0; k < 6; k++)
if (disabled[k])
value(k) = 0;
-
- nanp |= is_nan(neck) | is_nan(rel) | is_nan(value);
}
// CAVEAT translation only, due to tcomp
for (int i = 0; i < 3; i++)
value(i) = map(value(i), m(i));
- if (nanp)
+ nan_check(value);
+
+ goto ok;
+
+nan:
{
QMutexLocker foo(&mtx);
@@ -352,6 +367,8 @@ void pipeline::logic()
(void) map(raw_6dof(i), m(i));
}
+ok:
+
if (get(f_zero))
for (int i = 0; i < 6; i++)
value(i) = 0;
@@ -362,8 +379,7 @@ void pipeline::logic()
ev.run_events(EV::ev_finished, value);
- if (!nanp)
- libs.pProtocol->pose(value);
+ libs.pProtocol->pose(value);
QMutexLocker foo(&mtx);
output_pose = value;