summaryrefslogtreecommitdiffhomepage
path: root/opentrack/tracker.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-11-07 06:53:53 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-11-07 06:53:53 +0100
commit4814dc521b43a0d9b54c5c56300872fd3667189d (patch)
tree4a7b64031687ddb7c9fa597f5e17c644c3798170 /opentrack/tracker.cpp
parent2cf1d750c2e834596c0556a16ea2e4b3fdd7003f (diff)
impl centering not confused on 90deg boundaries
atan2 returned wrong quadrants, as per <http://en.wikipedia.org/wiki/Atan2#Definition_and_computation> breakage on sign change. Issue: #63
Diffstat (limited to 'opentrack/tracker.cpp')
-rw-r--r--opentrack/tracker.cpp54
1 files changed, 35 insertions, 19 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index b9b43ee5..8e8b1e7f 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -28,6 +28,7 @@ Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) :
enabledp(true),
should_quit(false),
libs(libs),
+ r_b(dmat<3,3>::eye()),
t_b {0,0,0}
{
}
@@ -47,13 +48,24 @@ double Tracker::map(double pos, bool invertp, Mapping& axis)
return fc.getValue(pos) + axis.opts.zero;
}
+// http://stackoverflow.com/a/18436193
+static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R)
+{
+ static constexpr double pi = 3.141592653;
+ // don't use atan2 here, confuses quadrants. see issue #63 -sh
+ double pitch = atan( -R(0, 2) / sqrt(R(1,2)*R(1,2) + R(2,2)*R(2,2)) );
+ double roll = atan(R(1, 2) / R(2, 2));
+ double yaw = atan(R(0, 1) / R(0, 0));
+ return dmat<3, 1>({yaw, pitch, roll});
+}
+
// tait-bryan angles, not euler
static dmat<3, 3> euler_to_rmat(const double* input)
{
static constexpr double pi = 3.141592653;
- const auto H = input[1] * pi / 180;
- const auto P = input[0] * pi / 180;
- const auto B = input[2] * pi / 180;
+ auto H = input[0] * pi / 180;
+ auto P = input[1] * pi / 180;
+ auto B = input[2] * pi / 180;
const auto c1 = cos(H);
const auto s1 = sin(H);
@@ -63,10 +75,18 @@ static dmat<3, 3> euler_to_rmat(const double* input)
const auto s3 = sin(B);
double foo[] = {
- //Tait-Bryan XYZ
- c2*c3, -c2*s3, s2,
- c1*s3+c3*s1*s2, c1*c3-s1*s2*s3, -c2*s1,
- s1*s3-c1*c3*s2, c3*s1+c1*s2*s3, c1*c2,
+ // z
+ c1 * c2,
+ c1 * s2 * s3 - c3 * s1,
+ s1 * s3 + c1 * c3 * s2,
+ // y
+ c2 * s1,
+ c1 * c3 + s1 * s2 * s3,
+ c3 * s1 * s2 - c1 * s3,
+ // x
+ -s2,
+ c2 * s3,
+ c2 * c3
};
return dmat<3, 3>(foo);
@@ -74,7 +94,7 @@ static dmat<3, 3> euler_to_rmat(const double* input)
void Tracker::t_compensate(const dmat<3, 3>& rmat, const double* xyz, double* output, bool rz)
{
- static constexpr int p_x = 0, p_y = 1, p_z = 2;
+ static constexpr int p_x = 2, p_y = 0, p_z = 1;
const double xyz_[3] = { -xyz[p_x], -xyz[p_y], xyz[p_z] };
dmat<3, 1> tvec(xyz_);
const dmat<3, 1> ret = rmat * tvec;
@@ -113,31 +133,27 @@ void Tracker::logic()
filtered_pose[i] *= inverts[i] ? -1. : 1.;
static constexpr double pi = 3.141592653;
- static constexpr double d2r = pi / 180.;
+ static constexpr double r2d = 180. / pi;
if (centerp)
{
centerp = false;
for (int i = 0; i < 3; i++)
t_b[i] = filtered_pose(i);
- q_b = Quat::from_euler_rads(filtered_pose(Yaw) * d2r,
- filtered_pose(Pitch) * d2r,
- filtered_pose(Roll) * d2r);
+ r_b = euler_to_rmat(&filtered_pose[Yaw]);
}
Pose raw_centered;
{
- 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]);
+ const dmat<3, 3> rmat = euler_to_rmat(&filtered_pose[Yaw]);
+ const dmat<3, 3> m_ = r_b.t() * rmat;
+ //const dmat<3, 3> m_ = (r_b.t() * rmat) * r_b * r_b.t();
+ const dmat<3, 1> euler = rmat_to_euler(m_);
for (int i = 0; i < 3; i++)
{
raw_centered(i) = filtered_pose(i) - t_b[i];
- raw_centered(i+3) = ypr[i];
+ raw_centered(i+3) = euler(i, 0) * r2d;
}
}