summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2015-12-22 21:56:32 +0100
committerNicolas Schodet2019-10-07 00:44:57 +0200
commit5a0c96b930c0dc9b14caea2557779e9a7bad8dfe (patch)
tree35519957b1e29114e947485fdbdab77283626fdc
parent5b4e922da076f4d0b91a65dc62ac1fe547f92a1d (diff)
ucoo/math: add quaternion to yaw, pitch, roll conversion
-rw-r--r--ucoo/math/quaternion.hh6
-rw-r--r--ucoo/math/quaternion.tcc21
-rw-r--r--ucoo/math/test/test_math.cc32
3 files changed, 57 insertions, 2 deletions
diff --git a/ucoo/math/quaternion.hh b/ucoo/math/quaternion.hh
index fd22c96..5e60cde 100644
--- a/ucoo/math/quaternion.hh
+++ b/ucoo/math/quaternion.hh
@@ -42,6 +42,12 @@ struct Quaternion
Quaternion (const YawPitchRoll<T> &ypr);
/// Rotate a vector by a unit quaternion.
vect3d<T> rotate (const vect3d<T> &v) const;
+ /// Compute Yaw.
+ T yaw () const;
+ /// Compute Pitch.
+ T pitch () const;
+ /// Compute Roll.
+ T roll () const;
};
} // namespace ucoo
diff --git a/ucoo/math/quaternion.tcc b/ucoo/math/quaternion.tcc
index 19fe7af..e648974 100644
--- a/ucoo/math/quaternion.tcc
+++ b/ucoo/math/quaternion.tcc
@@ -68,6 +68,27 @@ Quaternion<T>::rotate (const vect3d<T> &v) const
return vect3d<T> (nx, ny, nz);
}
+template<typename T>
+T
+Quaternion<T>::yaw () const
+{
+ return std::atan2 (2 * (x * y + w * z), w * w + x * x - y * y - z * z);
+}
+
+template<typename T>
+T
+Quaternion<T>::pitch () const
+{
+ return std::asin (-2 * (x * z - w * y));
+}
+
+template<typename T>
+T
+Quaternion<T>::roll () const
+{
+ return std::atan2 (2 * (y * z + w * x), w * w - x * x - y * y + z * z);
+}
+
} // namespace ucoo
#endif // ucoo_math_quaternion_tcc
diff --git a/ucoo/math/test/test_math.cc b/ucoo/math/test/test_math.cc
index 6380dc8..4fb3f16 100644
--- a/ucoo/math/test/test_math.cc
+++ b/ucoo/math/test/test_math.cc
@@ -29,6 +29,7 @@
#include <cmath>
#include <cfloat>
+#include <cstdio>
template<typename T>
bool
@@ -45,10 +46,11 @@ template<>
bool
almost_eq (float a, double b)
{
+ float pfa = std::fabs (a);
float fb = b;
float pfb = std::fabs (fb);
float diff = std::fabs (a - fb);
- if (pfb >= FLT_MIN)
+ if (pfa >= FLT_MIN && pfb >= FLT_MIN)
return diff <= pfb * 4 * FLT_EPSILON;
else
return diff <= 4 * FLT_EPSILON;
@@ -58,9 +60,10 @@ template<>
bool
almost_eq (double a, double b)
{
+ double pa = std::fabs (a);
double pb = std::fabs (b);
double diff = std::fabs (a - b);
- if (pb >= DBL_MIN)
+ if (pa >= DBL_MIN && pb >= DBL_MIN)
return diff <= pb * 4 * DBL_EPSILON;
else
return diff <= 4 * DBL_EPSILON;
@@ -218,6 +221,31 @@ test_group_quaternion (ucoo::TestSuite &tsuite, const char *tname)
r = q.rotate (z);
test_fail_break_unless (test, almost_eq_vect<T> (r, 1, 0, 0));
} while (0);
+ do {
+ ucoo::Test test (tsuite, "yaw-pitch-roll to quaternion and back");
+ ucoo::YawPitchRoll<T> yprs[] = {
+ { M_PI_4, 0, 0 },
+ { 0, M_PI_4, 0 },
+ { 0, 0, M_PI_4 },
+ { M_PI / 3, M_PI_4, 0 },
+ { M_PI / 3, 0, M_PI_4 },
+ { 0, M_PI / 3, M_PI_4 },
+ { M_PI / 3, M_PI_4, M_PI / 6 },
+ { -M_PI_4, 0, 0 },
+ { 0, -M_PI_4, 0 },
+ { 0, 0, -M_PI_4 },
+ { M_PI_2 + M_PI_4, 0, 0 },
+ // Pitch is between -pi/2 and +pi/2.
+ { 0, 0, M_PI_2 + M_PI_4 },
+ };
+ for (auto &ypr : yprs)
+ {
+ ucoo::Quaternion<T> q (ypr);
+ test_fail_break_unless (test, almost_eq (ypr.yaw, q.yaw ()));
+ test_fail_break_unless (test, almost_eq (ypr.pitch, q.pitch ()));
+ test_fail_break_unless (test, almost_eq (ypr.roll, q.roll ()));
+ }
+ } while (0);
}
int