summaryrefslogtreecommitdiff
path: root/ucoo
diff options
context:
space:
mode:
authorNicolas Schodet2015-10-26 13:19:01 +0100
committerNicolas Schodet2019-10-07 00:44:50 +0200
commit5487719b70fa89fdffae413b2d00f2a928cfc2a7 (patch)
tree3302797031138116df0fc86fe6f0b7db4e9ed9b4 /ucoo
parent9b163537e83eea2c65358135dc180e7bf2da1a51 (diff)
ucoo/math: add quaternion for vector rotation
Diffstat (limited to 'ucoo')
-rw-r--r--ucoo/math/quaternion.hh51
-rw-r--r--ucoo/math/quaternion.py46
-rw-r--r--ucoo/math/quaternion.tcc73
-rw-r--r--ucoo/math/test/test_math.cc78
-rw-r--r--ucoo/math/yaw_pitch_roll.hh45
-rw-r--r--ucoo/math/yaw_pitch_roll.tcc44
6 files changed, 333 insertions, 4 deletions
diff --git a/ucoo/math/quaternion.hh b/ucoo/math/quaternion.hh
new file mode 100644
index 0000000..fd22c96
--- /dev/null
+++ b/ucoo/math/quaternion.hh
@@ -0,0 +1,51 @@
+#ifndef ucoo_math_quaternion_hh
+#define ucoo_math_quaternion_hh
+// ucoolib - Microcontroller object oriented library. {{{
+//
+// Copyright (C) 2015 Nicolas Schodet
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+// }}}
+#include "ucoo/math/yaw_pitch_roll.hh"
+#include "ucoo/math/vect3d.hh"
+
+namespace ucoo {
+
+/// Quaternion: w + ix + jy + kz.
+template<typename T>
+struct Quaternion
+{
+ T w, x, y, z;
+ public:
+ /// Default constructor, values are default initialized.
+ constexpr Quaternion ();
+ /// Construct from quaternion values.
+ constexpr Quaternion (T w_, T x_, T y_, T z_);
+ /// Construct a rotation quaternion from Yaw, Pitch and Roll.
+ Quaternion (const YawPitchRoll<T> &ypr);
+ /// Rotate a vector by a unit quaternion.
+ vect3d<T> rotate (const vect3d<T> &v) const;
+};
+
+} // namespace ucoo
+
+#include "quaternion.tcc"
+
+#endif // ucoo_math_quaternion_hh
diff --git a/ucoo/math/quaternion.py b/ucoo/math/quaternion.py
new file mode 100644
index 0000000..fe48cc6
--- /dev/null
+++ b/ucoo/math/quaternion.py
@@ -0,0 +1,46 @@
+import sympy as sp
+
+sp.init_printing()
+
+def qmul(q1, q2):
+ a1, b1, c1, d1 = q1
+ a2, b2, c2, d2 = q2
+ return (
+ a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2,
+ a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2,
+ a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2,
+ a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2
+ )
+
+def rotate(q, v):
+ qv = (0, v[0], v[1], v[2])
+ # q is unitary.
+ qinv = (q[0], -q[1], -q[2], -q[3])
+ return qmul(q, qmul(qv, qinv))[1:4]
+
+if 0:
+ yaw, pitch, roll = sp.symbols('yaw pitch roll')
+ cyaw = sp.cos(yaw/2)
+ syaw = sp.sin(yaw/2)
+ cpitch = sp.cos(pitch/2)
+ spitch = sp.sin(pitch/2)
+ croll = sp.cos(roll/2)
+ sroll = sp.sin(roll/2)
+else:
+ cyaw, syaw, cpitch, spitch, croll, sroll = \
+ sp.symbols('cyaw syaw cpitch spitch croll sroll')
+
+print "yaw, pitch, roll to quaternion"
+q1 = (cyaw, 0, 0, syaw)
+q2 = (cpitch, 0, spitch, 0)
+q3 = (croll, sroll, 0, 0)
+qt = qmul(q1, qmul(q2, q3))
+for n, l in zip(('w', 'x', 'y', 'z'), qt):
+ print '%s = %s;' % (n, l)
+
+print "rotation"
+v = sp.symbols('v.x v.y v.z')
+q = sp.symbols('w x y z')
+rv = rotate(q, v)
+for n, l in zip(('x', 'y', 'z'), rv):
+ print 'n%s = %s;' % (n, sp.expand(l))
diff --git a/ucoo/math/quaternion.tcc b/ucoo/math/quaternion.tcc
new file mode 100644
index 0000000..19fe7af
--- /dev/null
+++ b/ucoo/math/quaternion.tcc
@@ -0,0 +1,73 @@
+#ifndef ucoo_math_quaternion_tcc
+#define ucoo_math_quaternion_tcc
+// ucoolib - Microcontroller object oriented library. {{{
+//
+// Copyright (C) 2015 Nicolas Schodet
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+// }}}
+#include <cmath>
+
+namespace ucoo {
+
+template<typename T>
+constexpr
+Quaternion<T>::Quaternion ()
+{
+}
+
+template<typename T>
+constexpr
+Quaternion<T>::Quaternion (T w_, T x_, T y_, T z_)
+ : w (w_), x (x_), y (y_), z (z_)
+{
+}
+
+template<typename T>
+Quaternion<T>::Quaternion (const YawPitchRoll<T> &ypr)
+{
+ T cyaw = std::cos (ypr.yaw / 2);
+ T syaw = std::sin (ypr.yaw / 2);
+ T cpitch = std::cos (ypr.pitch / 2);
+ T spitch = std::sin (ypr.pitch / 2);
+ T croll = std::cos (ypr.roll / 2);
+ T sroll = std::sin (ypr.roll / 2);
+ w = cpitch * croll * cyaw + spitch * sroll * syaw;
+ x = cpitch * cyaw * sroll - croll * spitch * syaw;
+ y = cpitch * sroll * syaw + croll * cyaw * spitch;
+ z = cpitch * croll * syaw - cyaw * spitch * sroll;
+}
+
+template<typename T>
+vect3d<T>
+Quaternion<T>::rotate (const vect3d<T> &v) const
+{
+ T nx = w*w*v.x + 2*y*w*v.z - 2*z*w*v.y + x*x*v.x
+ + 2*y*x*v.y + 2*z*x*v.z - z*z*v.x - y*y*v.x;
+ T ny = 2*x*y*v.x + y*y*v.y + 2*z*y*v.z + 2*w*z*v.x
+ - z*z*v.y + w*w*v.y - 2*x*w*v.z - x*x*v.y;
+ T nz = 2*x*z*v.x + 2*y*z*v.y + z*z*v.z - 2*w*y*v.x
+ - y*y*v.z + 2*w*x*v.y - x*x*v.z + w*w*v.z;
+ return vect3d<T> (nx, ny, nz);
+}
+
+} // namespace ucoo
+
+#endif // ucoo_math_quaternion_tcc
diff --git a/ucoo/math/test/test_math.cc b/ucoo/math/test/test_math.cc
index 685387e..6380dc8 100644
--- a/ucoo/math/test/test_math.cc
+++ b/ucoo/math/test/test_math.cc
@@ -22,6 +22,7 @@
//
// }}}
#include "ucoo/math/vect3d.hh"
+#include "ucoo/math/quaternion.hh"
#include "ucoo/arch/arch.hh"
#include "ucoo/base/test/test.hh"
#include "ucoo/common.hh"
@@ -46,9 +47,11 @@ almost_eq (float a, double b)
{
float fb = b;
float pfb = std::fabs (fb);
- ucoo::assert (pfb >= FLT_MIN);
float diff = std::fabs (a - fb);
- return diff <= pfb * 4 * FLT_EPSILON;
+ if (pfb >= FLT_MIN)
+ return diff <= pfb * 4 * FLT_EPSILON;
+ else
+ return diff <= 4 * FLT_EPSILON;
}
template<>
@@ -56,9 +59,20 @@ bool
almost_eq (double a, double b)
{
double pb = std::fabs (b);
- ucoo::assert (pb >= DBL_MIN);
double diff = std::fabs (a - b);
- return diff <= pb * 4 * DBL_EPSILON;
+ if (pb >= DBL_MIN)
+ return diff <= pb * 4 * DBL_EPSILON;
+ else
+ return diff <= 4 * DBL_EPSILON;
+}
+
+template<typename T>
+bool
+almost_eq_vect (const ucoo::vect3d<T> v, T x, T y, T z)
+{
+ return almost_eq (v.x, x)
+ && almost_eq (v.y, y)
+ && almost_eq (v.z, z);
}
template<typename T>
@@ -152,6 +166,60 @@ test_group_vect3d (ucoo::TestSuite &tsuite, const char *tname)
}
}
+template<typename T>
+void
+test_group_quaternion (ucoo::TestSuite &tsuite, const char *tname)
+{
+ tsuite.group (tname);
+ ucoo::vect3d<T> x (1, 0, 0);
+ ucoo::vect3d<T> y (0, 1, 0);
+ ucoo::vect3d<T> z (0, 0, 1);
+ do {
+ ucoo::Test test (tsuite, "rotate x");
+ ucoo::Quaternion<T> q (ucoo::YawPitchRoll<T> (0, 0, M_PI_2));
+ ucoo::vect3d<T> r;
+ r = q.rotate (x);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 1, 0, 0));
+ r = q.rotate (y);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 0, 0, 1));
+ r = q.rotate (z);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 0, -1, 0));
+ } while (0);
+ do {
+ ucoo::Test test (tsuite, "rotate y");
+ ucoo::Quaternion<T> q (ucoo::YawPitchRoll<T> (0, M_PI_2, 0));
+ 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, "rotate z");
+ ucoo::Quaternion<T> q (ucoo::YawPitchRoll<T> (M_PI_2, 0, 0));
+ ucoo::vect3d<T> r;
+ r = q.rotate (x);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 0, 1, 0));
+ r = q.rotate (y);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, -1, 0, 0));
+ r = q.rotate (z);
+ test_fail_break_unless (test, almost_eq_vect<T> (r, 0, 0, 1));
+ } while (0);
+ do {
+ ucoo::Test test (tsuite, "rotate zyx");
+ ucoo::Quaternion<T> q (ucoo::YawPitchRoll<T> (M_PI_2, M_PI_2, M_PI_2));
+ 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);
+}
+
int
main (int argc, const char **argv)
{
@@ -160,5 +228,7 @@ main (int argc, const char **argv)
test_group_vect3d<int> (tsuite, "vect3d int");
test_group_vect3d<float> (tsuite, "vect3d float");
test_group_vect3d<double> (tsuite, "vect3d double");
+ test_group_quaternion<float> (tsuite, "quaternion float");
+ test_group_quaternion<double> (tsuite, "quaternion double");
return tsuite.report () ? 0 : 1;
}
diff --git a/ucoo/math/yaw_pitch_roll.hh b/ucoo/math/yaw_pitch_roll.hh
new file mode 100644
index 0000000..d571b12
--- /dev/null
+++ b/ucoo/math/yaw_pitch_roll.hh
@@ -0,0 +1,45 @@
+#ifndef ucoo_math_yaw_pitch_roll_hh
+#define ucoo_math_yaw_pitch_roll_hh
+// ucoolib - Microcontroller object oriented library. {{{
+//
+// Copyright (C) 2015 Nicolas Schodet
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+// }}}
+
+namespace ucoo {
+
+/// Represent a rotation in space using nautical angles.
+template<typename T>
+struct YawPitchRoll
+{
+ T yaw, pitch, roll;
+ public:
+ /// Default constructor, angles are default initialized.
+ constexpr YawPitchRoll ();
+ /// Constructor from angles.
+ constexpr YawPitchRoll (T yaw_, T pitch_, T roll_);
+};
+
+} // namespace ucoo
+
+#include "yaw_pitch_roll.tcc"
+
+#endif // ucoo_math_yaw_pitch_roll_hh
diff --git a/ucoo/math/yaw_pitch_roll.tcc b/ucoo/math/yaw_pitch_roll.tcc
new file mode 100644
index 0000000..e2b8764
--- /dev/null
+++ b/ucoo/math/yaw_pitch_roll.tcc
@@ -0,0 +1,44 @@
+#ifndef ucoo_math_yaw_pitch_roll_tcc
+#define ucoo_math_yaw_pitch_roll_tcc
+// ucoolib - Microcontroller object oriented library. {{{
+//
+// Copyright (C) 2015 Nicolas Schodet
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+// }}}
+
+namespace ucoo {
+
+template<typename T>
+constexpr
+YawPitchRoll<T>::YawPitchRoll ()
+{
+}
+
+template<typename T>
+constexpr
+YawPitchRoll<T>::YawPitchRoll (T yaw_, T pitch_, T roll_)
+ : yaw (yaw_), pitch (pitch_), roll (roll_)
+{
+}
+
+} // namespace ucoo
+
+#endif // ucoo_math_yaw_pitch_roll_tcc