summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--opentrack/tracker.cpp47
1 files changed, 24 insertions, 23 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index f19b96f2..b9b43ee5 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -115,61 +115,62 @@ void Tracker::logic()
static constexpr double pi = 3.141592653;
static constexpr double d2r = pi / 180.;
- Pose mapped_pose;
-
- for (int i = 0; i < 6; i++)
- mapped_pose(i) = map(filtered_pose(i), inverts[i], m(i));
-
if (centerp)
{
centerp = false;
for (int i = 0; i < 3; i++)
t_b[i] = filtered_pose(i);
- q_b = Quat::from_euler_rads(mapped_pose(Yaw) * d2r,
- mapped_pose(Pitch) * d2r,
- mapped_pose(Roll) * d2r);
+ q_b = Quat::from_euler_rads(filtered_pose(Yaw) * d2r,
+ filtered_pose(Pitch) * d2r,
+ filtered_pose(Roll) * d2r);
}
- Pose centered;
+ Pose raw_centered;
{
- const Quat q(mapped_pose(Yaw)*d2r,
- mapped_pose(Pitch)*d2r,
- mapped_pose(Roll)*d2r);
+ const Quat q(filtered_pose(Yaw)*d2r,
+ filtered_pose(Pitch)*d2r,
+ filtered_pose(Roll)*d2r);
const Quat q_ = q * q_b.inv();
double ypr[3];
q_.to_euler_degrees(ypr[0], ypr[1], ypr[2]);
for (int i = 0; i < 3; i++)
{
- centered(i) = mapped_pose(i) - t_b[i];
- centered(i+3) = ypr[i];
+ raw_centered(i) = filtered_pose(i) - t_b[i];
+ raw_centered(i+3) = ypr[i];
}
}
- Pose centered_ = centered;
+ Pose mapped_pose_precomp;
+
+ for (int i = 0; i < 6; i++)
+ mapped_pose_precomp(i) = map(raw_centered(i), inverts[i], m(i));
+
+ Pose mapped_pose_ = mapped_pose_precomp;
if (s.tcomp_p)
- t_compensate(euler_to_rmat(&centered[Yaw]),
- centered,
- centered_,
+ t_compensate(euler_to_rmat(&mapped_pose_precomp[Yaw]),
+ mapped_pose_precomp,
+ mapped_pose_,
s.tcomp_tz);
- Pose mapped;
+ Pose mapped_pose;
for (int i = 0; i < 6; i++)
{
auto& axis = m(i);
int k = axis.opts.src;
if (k < 0 || k >= 6)
- mapped(i) = 0;
+ mapped_pose(i) = 0;
else
- mapped(i) = centered_(i);
+ mapped_pose(i) = mapped_pose_(i);
}
- libs.pProtocol->pose(mapped);
+
+ libs.pProtocol->pose(mapped_pose);
QMutexLocker foo(&mtx);
- output_pose = mapped;
+ output_pose = mapped_pose;
raw_6dof = final_raw;
}