summaryrefslogtreecommitdiff
path: root/ucoo
diff options
context:
space:
mode:
authorNicolas Schodet2015-12-23 10:52:43 +0100
committerNicolas Schodet2019-10-07 00:44:57 +0200
commitbd32e6457b51e4f3e94d6cd5af450695e97a36ef (patch)
tree09834a707674ebc135105900cd031cdd896346f2 /ucoo
parent5a0c96b930c0dc9b14caea2557779e9a7bad8dfe (diff)
ucoo/math: add quaternion multiplication
Diffstat (limited to 'ucoo')
-rw-r--r--ucoo/math/quaternion.hh2
-rw-r--r--ucoo/math/quaternion.tcc12
-rw-r--r--ucoo/math/test/test_math.cc14
3 files changed, 28 insertions, 0 deletions
diff --git a/ucoo/math/quaternion.hh b/ucoo/math/quaternion.hh
index 5e60cde..556fa1f 100644
--- a/ucoo/math/quaternion.hh
+++ b/ucoo/math/quaternion.hh
@@ -48,6 +48,8 @@ struct Quaternion
T pitch () const;
/// Compute Roll.
T roll () const;
+ /// Multiply by another quaternion.
+ Quaternion<T> operator* (const Quaternion<T> &r) const;
};
} // namespace ucoo
diff --git a/ucoo/math/quaternion.tcc b/ucoo/math/quaternion.tcc
index e648974..1db845b 100644
--- a/ucoo/math/quaternion.tcc
+++ b/ucoo/math/quaternion.tcc
@@ -89,6 +89,18 @@ Quaternion<T>::roll () const
return std::atan2 (2 * (y * z + w * x), w * w - x * x - y * y + z * z);
}
+template<typename T>
+Quaternion<T>
+Quaternion<T>::operator* (const Quaternion<T> &r) const
+{
+ return Quaternion<T> (
+ w * r.w - x * r.x - y * r.y - z * r.z,
+ w * r.x + x * r.w + y * r.z - z * r.y,
+ w * r.y - x * r.z + y * r.w + z * r.x,
+ w * r.z + x * r.y - y * r.x + z * r.w
+ );
+}
+
} // namespace ucoo
#endif // ucoo_math_quaternion_tcc
diff --git a/ucoo/math/test/test_math.cc b/ucoo/math/test/test_math.cc
index 4fb3f16..65be6e2 100644
--- a/ucoo/math/test/test_math.cc
+++ b/ucoo/math/test/test_math.cc
@@ -222,6 +222,20 @@ test_group_quaternion (ucoo::TestSuite &tsuite, const char *tname)
test_fail_break_unless (test, almost_eq_vect<T> (r, 1, 0, 0));
} while (0);
do {
+ ucoo::Test test (tsuite, "multiplication");
+ ucoo::Quaternion<T> q1 (ucoo::YawPitchRoll<T> (M_PI_2, 0, 0));
+ ucoo::Quaternion<T> q2 (ucoo::YawPitchRoll<T> (0, M_PI_2, 0));
+ ucoo::Quaternion<T> q3 (ucoo::YawPitchRoll<T> (0, 0, M_PI_2));
+ ucoo::Quaternion<T> q = q1 * q2 * q3;
+ ucoo::vect3d<T> r;
+ r = q.rotate (x);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 0, 0, -1));
+ r = q.rotate (y);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 0, 1, 0));
+ 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 },