summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-12-20 23:25:01 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-12-20 23:27:20 +0100
commit15f66b14de5e1bf3ecff0450040c8e1ddb77116a (patch)
tree3b297162fe542d8857e16c1189c4101084d7728a
parentd3f6ebf7eb69bc9e7508192e1aba58869589409e (diff)
apply r/t compensation before mapping translation values
Suggested-by: @FlyingCircus- Issue: #106
-rw-r--r--opentrack/tracker.cpp20
-rw-r--r--opentrack/tracker.h2
2 files changed, 12 insertions, 10 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index 31939dea..4f7e4b9e 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -40,9 +40,9 @@ Tracker::~Tracker()
wait();
}
-double Tracker::map(double pos, bool invertp, Mapping& axis)
+double Tracker::map(double pos, Mapping& axis)
{
- bool altp = (pos < 0) == !invertp && axis.opts.altp;
+ bool altp = (pos < 0) && axis.opts.altp;
axis.curve.setTrackingActive( !altp );
axis.curveAlt.setTrackingActive( altp );
auto& fc = altp ? axis.curveAlt : axis.curve;
@@ -125,9 +125,9 @@ void Tracker::logic()
value(i+3) = euler(i, 0) * r2d;
}
}
-
- for (int i = 0; i < 6; i++)
- value(i) = map(value(i), inverts[i], m(i));
+
+ for (int i = 3; i < 6; i++)
+ value(i) = map(value(i), m(i));
{
Pose tmp = value;
@@ -136,15 +136,17 @@ void Tracker::logic()
libs.pFilter->filter(tmp, value);
}
- // must invert early as euler_to_rmat's sensitive to sign change
- for (int i = 0; i < 6; i++)
- value[i] *= inverts[i] ? -1. : 1.;
-
if (s.tcomp_p)
t_compensate(rmat::euler_to_rmat(&value[Yaw]),
value,
value,
s.tcomp_tz);
+
+ for (int i = 0; i < 3; i++)
+ value(i) = map(value(i), m(i));
+
+ for (int i = 0; i < 6; i++)
+ value[i] *= inverts[i] ? -1. : 1.;
Pose output_pose_;
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index 58eb69dd..ace9fa3c 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -37,7 +37,7 @@ private:
dmat<3, 3> r_b;
double t_b[3];
- double map(double pos, bool invertp, Mapping& axis);
+ double map(double pos, Mapping& axis);
void logic();
void t_compensate(const dmat<3, 3>& rmat, const double* ypr, double* output, bool rz);