summaryrefslogtreecommitdiffhomepage
path: root/logic
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2018-01-06 01:45:44 +0100
committerStanislaw Halik <sthalik@misaki.pl>2018-01-10 03:17:08 +0100
commit5ad2275e0e50be8a4e9f506fb3f8dab2b02d0420 (patch)
tree24af38431505363977e08ebcd652d1aebe79fed9 /logic
parentdbacc00b52e294f65a8d292dbcce0f4fc93b5bf0 (diff)
logic/pipeline: conditionalize
Will activate only when looking down or backward. When activating or deactivating, will slowly slide into the new position, no instant movement involved. Issue: #712
Diffstat (limited to 'logic')
-rw-r--r--logic/pipeline.cpp231
-rw-r--r--logic/pipeline.hpp31
2 files changed, 174 insertions, 88 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 7d073b28..43cc74c7 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -27,37 +27,16 @@
#endif
using namespace euler;
-using namespace gui_tracker_impl;
using namespace time_units;
+using namespace gui_tracker_impl;
-constexpr double pipeline::r2d;
-constexpr double pipeline::d2r;
+static constexpr inline double r2d = 180. / M_PI;
+static constexpr inline double d2r = M_PI / 180.;
-pipeline::pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger) :
- m(m),
- ev(ev),
- libs(libs),
- logger(logger)
-{
-}
+reltrans::reltrans() {}
-pipeline::~pipeline()
-{
- requestInterruption();
- wait();
-}
-
-double pipeline::map(double pos, Map& axis)
-{
- bool altp = (pos < 0) && axis.opts.altp;
- axis.spline_main.set_tracking_active( !altp );
- axis.spline_alt.set_tracking_active( altp );
- auto& fc = altp ? axis.spline_alt : axis.spline_main;
- return double(fc.get_value(pos));
-}
-
-void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& output,
- bool disable_tx, bool disable_ty, bool disable_tz)
+euler_t reltrans::rotate(const rmat& rmat, const euler_t& xyz,
+ bool disable_tx, bool disable_ty, bool disable_tz) const
{
enum { tb_Z, tb_X, tb_Y };
@@ -65,6 +44,8 @@ void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& outpu
// 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));
+ euler_t output;
+
if (disable_tz)
output(TZ) = xyz(TZ);
else
@@ -79,6 +60,112 @@ void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& outpu
output(TX) = xyz(TX);
else
output(TX) = -ret(tb_X);
+
+ return output;
+}
+
+Pose reltrans::apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6, 1>& disable)
+{
+ if (enable)
+ {
+ euler_t rel { value(TX), value(TY), value(TZ) };
+
+ {
+ const bool yaw_in_zone = std::fabs(value(Yaw)) > 135;
+ const bool pitch_in_zone = value(Pitch) < -30;
+ const bool tcomp_in_zone_ = yaw_in_zone || pitch_in_zone;
+
+ if (!tcomp_state && tcomp_in_zone != tcomp_in_zone_)
+ {
+ //qDebug() << "tcomp-interp: START";
+ tcomp_state = true;
+ tcomp_interp_timer.start();
+ }
+
+ tcomp_in_zone = tcomp_in_zone_;
+ }
+
+ // only when looking behind or downward
+ if (tcomp_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));
+ }
+
+ if (tcomp_state)
+ {
+ const double dt = tcomp_interp_timer.elapsed_seconds();
+ tcomp_interp_timer.start();
+
+ constexpr double RC = .1;
+ const double alpha = dt/(dt+RC);
+
+ constexpr double eps = .05;
+
+ tcomp_interp_pos = tcomp_interp_pos * (1-alpha) + rel * alpha;
+
+ const euler_t tmp = rel - tcomp_interp_pos;
+ rel = tcomp_interp_pos;
+ const double delta = std::fabs(tmp(0)) + std::fabs(tmp(0)) + std::fabs(tmp(0));
+
+ //qDebug() << "tcomp-interp: delta" << delta;
+
+ if (delta < eps)
+ {
+ //qDebug() << "tcomp-interp: STOP";
+ tcomp_state = false;
+ }
+ }
+ else
+ {
+ tcomp_interp_pos = rel;
+ }
+
+ return { rel(0), rel(1), rel(2), value(Yaw), value(Pitch), value(Roll) };
+ }
+ else
+ {
+ tcomp_state = false;
+ tcomp_in_zone = false;
+
+ return value;
+ }
+}
+
+pipeline::pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger) :
+ m(m),
+ ev(ev),
+ libs(libs),
+ logger(logger)
+{
+}
+
+pipeline::~pipeline()
+{
+ requestInterruption();
+ wait();
+}
+
+double pipeline::map(double pos, Map& axis)
+{
+ bool altp = (pos < 0) && axis.opts.altp;
+ axis.spline_main.set_tracking_active( !altp );
+ axis.spline_alt.set_tracking_active( altp );
+ auto& fc = altp ? axis.spline_alt : axis.spline_main;
+ return double(fc.get_value(pos));
}
template<int u, int w>
@@ -109,14 +196,20 @@ static inline bool nan_check_(const x& datum)
template<typename>
static bool nan_check_() = delete;
+static never_inline
+void emit_nan_check_msg(const char* text, int line)
+{
+ once_only(qDebug() << "nan check failed"
+ << "line:" << text
+ << "for:" << line);
+}
+
#define nan_check(...) \
do \
{ \
if (!nan_check_(__VA_ARGS__)) \
{ \
- once_only(qDebug() << "nan check failed" \
- << "line:" << __LINE__ \
- << "for:" << #__VA_ARGS__); \
+ emit_nan_check_msg(#__VA_ARGS__, __LINE__); \
goto nan; \
} \
} while (false)
@@ -129,6 +222,7 @@ void pipeline::logic()
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();
@@ -167,7 +261,7 @@ void pipeline::logic()
using std::copysign;
using std::fabs;
- value(i) = std::fmod(value(i), 360);
+ value(i) = fmod(value(i), 360);
const double x = value(i);
if (fabs(x) - 1e-2 > 180)
@@ -188,10 +282,8 @@ void pipeline::logic()
if (!tracking_started)
{
- using std::fabs;
-
for (int i = 0; i < 6; i++)
- if (fabs(newpose(i)) != 0)
+ if (std::fabs(newpose(i)) != 0)
{
tracking_started = true;
break;
@@ -240,7 +332,7 @@ void pipeline::logic()
case 1:
// camera
rotation = rotation * scaled_rotation.rot_center;
- t_compensate(real_rotation.rot_center, pos, pos, false, false, false);
+ pos = rel.rotate(real_rotation.rot_center, pos, false, false, false);
break;
}
@@ -285,62 +377,41 @@ void pipeline::logic()
{
ev.run_events(EV::ev_before_mapping, value);
- euler_t neck, rel;
-
- if (s.neck_enable)
{
- double nz = -s.neck_z;
+ euler_t neck;
- if (nz != 0)
+ if (s.neck_enable)
{
- const rmat R = euler_to_rmat(
- euler_t(value(Yaw) * d2r,
- value(Pitch) * d2r,
- value(Roll) * d2r));
- euler_t xyz(0, 0, nz);
- t_compensate(R, xyz, xyz, false, false, false);
- 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)));
+ double nz = -s.neck_z;
+
+ 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);
+ }
}
- }
- // CAVEAT rotation only, due to tcomp
- for (int i = 3; i < 6; i++)
- value(i) = map(value(i), m(i));
+ // CAVEAT rotation only, due to tcomp
+ for (int i = 3; i < 6; i++)
+ value(i) = map(value(i), m(i));
- nan_check(value);
+ nan_check(value);
- if (s.tcomp_p)
- {
- const double tcomp_c[] =
- {
- double(!s.tcomp_disable_src_yaw),
- double(!s.tcomp_disable_src_pitch),
- double(!s.tcomp_disable_src_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]));
- euler_t ret;
- t_compensate(R,
- euler_t(value(TX), value(TY), value(TZ)),
- ret,
- s.tcomp_disable_tx,
- s.tcomp_disable_ty,
- s.tcomp_disable_tz);
+ value = rel.apply_pipeline(s.tcomp_p, value, {
+ !!s.tcomp_disable_src_yaw, !!s.tcomp_disable_src_pitch, !!s.tcomp_disable_src_roll,
+ !!s.tcomp_disable_tx, !!s.tcomp_disable_ty, !!s.tcomp_disable_tz
+ });
for (int i = 0; i < 3; i++)
- rel(i) = ret(i) - value(i);
+ value(i) += neck(i);
}
- // don't t_compensate existing compensated values
- for (int i = 0; i < 3; i++)
- value(i) += neck(i) + rel(i);
-
// relative translation can move it
for (unsigned k = 0; k < 6; k++)
if (disabled[k])
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 0cb828e7..dc04d44b 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -32,6 +32,27 @@
namespace gui_tracker_impl {
+using rmat = euler::rmat;
+using euler_t = euler::euler_t;
+
+class reltrans
+{
+ euler_t tcomp_interp_pos, tcomp_last_value;
+ Timer tcomp_interp_timer;
+ bool tcomp_state = false;
+ bool tcomp_in_zone = false;
+
+public:
+ reltrans();
+
+ warn_result_unused
+ euler_t rotate(const rmat& rmat, const euler_t& xyz,
+ bool disable_tx, bool disable_ty, bool disable_tz) const;
+
+ warn_result_unused
+ Pose apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6, 1>& disable);
+};
+
using namespace time_units;
struct OTR_LOGIC_EXPORT bits
@@ -55,9 +76,6 @@ class OTR_LOGIC_EXPORT pipeline : private QThread, private bits
{
Q_OBJECT
private:
- using rmat = euler::rmat;
- using euler_t = euler::euler_t;
-
QMutex mtx;
main_settings s;
Mappings& m;
@@ -82,6 +100,8 @@ private:
{}
};
+ reltrans rel;
+
state real_rotation, scaled_rotation;
euler_t t_center;
@@ -91,13 +111,8 @@ private:
double map(double pos, Map& axis);
void logic();
- void t_compensate(const rmat& rmat, const euler_t& ypr, euler_t& output,
- bool disable_tx, bool disable_ty, bool disable_tz);
void run() override;
- static constexpr double r2d = 180. / M_PI;
- static constexpr double d2r = M_PI / 180.;
-
// note: float exponent base is 2
static constexpr double c_mult = 16;
static constexpr double c_div = 1./c_mult;