summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2018-03-03 10:49:43 +0100
committerStanislaw Halik <sthalik@misaki.pl>2018-03-03 13:05:54 +0100
commit7072e072f4bb8388b40d6599a2a509d84d41a51d (patch)
tree58b063c59a872f27acf6c06f7ccdc9136b0b16f6
parent4d2106c3c6f51c574fbfad69d6086cbe74af1179 (diff)
logic/pipeline: centering fix
The `scaled_rotation' gimbal lock "fix" didn't take into account sign changes, making center close to +-180 useless. remove the "fix". This leaves with a gimbal lock problem at +-90 yaw. Fix in another commit.
-rw-r--r--logic/pipeline.cpp44
-rw-r--r--logic/pipeline.hpp14
2 files changed, 32 insertions, 26 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 654a9f09..f691a653 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -17,6 +17,7 @@
#include "compat/meta.hpp"
#include "pipeline.hpp"
+#include "logic/shortcuts.h"
#include <cmath>
#include <algorithm>
@@ -246,10 +247,10 @@ bool maybe_nan(const char* text, const char* fun, int line, const xs&... vals)
do \
{ \
if (maybe_nan(#__VA_ARGS__, OTR_FUNNAME, __LINE__, __VA_ARGS__)) \
- goto nan; \
+ goto error; \
} while (false)
-void pipeline::maybe_enable_center_on_tracking_started()
+bool pipeline::maybe_enable_center_on_tracking_started()
{
if (!tracking_started)
{
@@ -261,15 +262,20 @@ void pipeline::maybe_enable_center_on_tracking_started()
}
if (tracking_started && s.center_at_startup)
+ {
set(f_center, true);
+ return true;
+ }
}
+
+ return false;
}
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);
+ //scaled_rotation.rotation = euler_to_rmat(c_div * tmp);
+ rotation.rotation = euler_to_rmat(tmp);
if (get(f_center))
{
@@ -278,15 +284,15 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
if (own_center_logic)
{
- scaled_rotation.rot_center = rmat::eye();
- real_rotation.rot_center = rmat::eye();
+ //scaled_rotation.rot_center = rmat::eye();
+ rotation.inv_rot_center = rmat::eye();
t_center = euler_t();
}
else
{
- real_rotation.rot_center = real_rotation.rotation.t();
- scaled_rotation.rot_center = scaled_rotation.rotation.t();
+ rotation.inv_rot_center = rotation.rotation.t();
+ //scaled_rotation.rot_center = scaled_rotation.rotation.t();
t_center = euler_t(static_cast<const double*>(value));
}
@@ -316,27 +322,26 @@ Pose pipeline::clamp_value(Pose value) const
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);
+ euler_t T = euler_t(value) - t_center;
+ euler_t R = r2d * rmat_to_euler(rotation.rotation * rotation.inv_rot_center);
- pos = rel.rotate(real_rotation.rot_center, pos, vec3_bool());
+ T = rel.rotate(rotation.inv_rot_center, T, vec3_bool());
for (int i = 0; i < 3; i++)
{
- // don't invert after t_compensate
+ // don't invert after reltrans
// inverting here doesn't break centering
if (m(i+3).opts.invert)
- rot(i) = -rot(i);
+ R(i) = -R(i);
if (m(i).opts.invert)
- pos(i) = -pos(i);
+ T(i) = -T(i);
}
for (int i = 0; i < 3; i++)
{
- value(i) = pos(i);
- value(i+3) = rot(i);
+ value(i) = T(i);
+ value(i+3) = R(i);
}
return value;
@@ -442,7 +447,8 @@ void pipeline::logic()
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
+ // "corrected" - after various transformations to account for camera position
+ logger.write_pose(value);
}
{
@@ -470,7 +476,7 @@ void pipeline::logic()
goto ok;
-nan:
+error:
{
QMutexLocker foo(&mtx);
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 45d5b2b2..d51655b3 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -98,16 +98,16 @@ private:
struct state
{
- rmat rot_center;
+ rmat inv_rot_center;
rmat rotation;
- state() : rot_center(rmat::eye())
+ state() : inv_rot_center(rmat::eye())
{}
};
reltrans rel;
- state real_rotation, scaled_rotation;
+ state rotation;
euler_t t_center;
ns backlog_time = ns(0);
@@ -117,7 +117,7 @@ private:
double map(double pos, Map& axis);
void logic();
void run() override;
- void maybe_enable_center_on_tracking_started();
+ bool maybe_enable_center_on_tracking_started();
void maybe_set_center_pose(const Pose& value, bool own_center_logic);
Pose clamp_value(Pose value) const;
Pose apply_center(Pose value) const;
@@ -126,9 +126,9 @@ private:
Pose apply_reltrans(Pose value, vec6_bool disabled);
Pose apply_zero_pos(Pose value) const;
- // note: float exponent base is 2
- static constexpr inline double c_mult = 16;
- static constexpr inline double c_div = 1./c_mult;
+ // reminder: float exponent base is 2
+ //static constexpr inline double c_mult = 16;
+ //static constexpr inline double c_div = 1./c_mult;
public:
pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger);
~pipeline();