summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--facetracknoir/pose.hpp30
-rw-r--r--facetracknoir/tracker.cpp26
2 files changed, 17 insertions, 39 deletions
diff --git a/facetracknoir/pose.hpp b/facetracknoir/pose.hpp
index d925c0a8..41e984f5 100644
--- a/facetracknoir/pose.hpp
+++ b/facetracknoir/pose.hpp
@@ -33,34 +33,12 @@ public:
return ret;
}
- Pose operator-(const Pose& B) const
+ Pose operator&(const Pose& B) const
{
- const Quat q = (quat() * B.quat().inv());
+ const Quat q = quat() * B.quat().inv();
Pose ret = fromQuat(q);
- for (int i = TX; i < Yaw; i++)
- ret(i) = B(i);
- return ret;
- }
-
- Pose operator+(const Pose& B) const
- {
- const Quat q = quat() * B.quat();
- Pose ret = fromQuat(q);
- for (int i = TX; i < Yaw; i++)
- ret(i) = B(i);
- return ret;
- }
-
- Pose operator|(const Pose& replacement) const
- {
- Pose ret = *this;
- for (int i = 0; i < 6; i++)
- {
- static constexpr double eps = 1e-3;
- // NB replace zero-valued elements with argument's
- if (std::abs(ret(i)) < eps)
- ret(i) = replacement(i);
- }
+ for (int i = TX; i < TX + 3; i++)
+ ret(i) = axes[i] - B.axes[i];
return ret;
}
};
diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp
index 504cd003..4a80c722 100644
--- a/facetracknoir/tracker.cpp
+++ b/facetracknoir/tracker.cpp
@@ -98,7 +98,7 @@ void Tracker::run() {
Libraries->pTracker->GetHeadPoseData(newpose);
- Pose final_raw, final_mapped;
+ Pose final_raw, filtered;
for (int i = 0; i < 6; i++)
{
@@ -110,34 +110,34 @@ void Tracker::run() {
final_raw(i) = newpose[k];
}
- if (centerp) {
- centerp = false;
- pose_offset = final_raw;
- }
-
{
if (enabledp)
unstopped_pose = final_raw;
if (Libraries->pFilter)
- Libraries->pFilter->FilterHeadPoseData(unstopped_pose, final_mapped);
+ Libraries->pFilter->FilterHeadPoseData(unstopped_pose, filtered);
else
- final_mapped = unstopped_pose;
+ filtered = unstopped_pose;
+
+ if (centerp) {
+ centerp = false;
+ pose_offset = filtered;
+ }
- final_mapped = final_mapped - pose_offset;
+ filtered = filtered & pose_offset;
for (int i = 0; i < 6; i++)
- get_curve(final_mapped(i), final_mapped(i), m(i));
+ get_curve(filtered(i), filtered(i), m(i));
}
if (s.tcomp_p)
- t_compensate(final_mapped, final_mapped, s.tcomp_tz);
+ t_compensate(filtered, filtered, s.tcomp_tz);
- Libraries->pProtocol->sendHeadposeToGame(final_mapped);
+ Libraries->pProtocol->sendHeadposeToGame(filtered);
{
QMutexLocker foo(&mtx);
- output_pose = final_mapped;
+ output_pose = filtered;
raw_6dof = final_raw;
}