From 5487719b70fa89fdffae413b2d00f2a928cfc2a7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 26 Oct 2015 13:19:01 +0100 Subject: ucoo/math: add quaternion for vector rotation --- ucoo/math/quaternion.hh | 51 +++++++++++++++++++++++++++++ ucoo/math/quaternion.py | 46 ++++++++++++++++++++++++++ ucoo/math/quaternion.tcc | 73 +++++++++++++++++++++++++++++++++++++++++ ucoo/math/test/test_math.cc | 78 +++++++++++++++++++++++++++++++++++++++++--- ucoo/math/yaw_pitch_roll.hh | 45 +++++++++++++++++++++++++ ucoo/math/yaw_pitch_roll.tcc | 44 +++++++++++++++++++++++++ 6 files changed, 333 insertions(+), 4 deletions(-) create mode 100644 ucoo/math/quaternion.hh create mode 100644 ucoo/math/quaternion.py create mode 100644 ucoo/math/quaternion.tcc create mode 100644 ucoo/math/yaw_pitch_roll.hh create mode 100644 ucoo/math/yaw_pitch_roll.tcc (limited to 'ucoo/math') 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 +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 &ypr); + /// Rotate a vector by a unit quaternion. + vect3d rotate (const vect3d &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 + +namespace ucoo { + +template +constexpr +Quaternion::Quaternion () +{ +} + +template +constexpr +Quaternion::Quaternion (T w_, T x_, T y_, T z_) + : w (w_), x (x_), y (y_), z (z_) +{ +} + +template +Quaternion::Quaternion (const YawPitchRoll &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 +vect3d +Quaternion::rotate (const vect3d &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 (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 +bool +almost_eq_vect (const ucoo::vect3d v, T x, T y, T z) +{ + return almost_eq (v.x, x) + && almost_eq (v.y, y) + && almost_eq (v.z, z); } template @@ -152,6 +166,60 @@ test_group_vect3d (ucoo::TestSuite &tsuite, const char *tname) } } +template +void +test_group_quaternion (ucoo::TestSuite &tsuite, const char *tname) +{ + tsuite.group (tname); + ucoo::vect3d x (1, 0, 0); + ucoo::vect3d y (0, 1, 0); + ucoo::vect3d z (0, 0, 1); + do { + ucoo::Test test (tsuite, "rotate x"); + ucoo::Quaternion q (ucoo::YawPitchRoll (0, 0, M_PI_2)); + ucoo::vect3d r; + r = q.rotate (x); + test_fail_break_unless (test, almost_eq_vect (r, 1, 0, 0)); + r = q.rotate (y); + test_fail_break_unless (test, almost_eq_vect (r, 0, 0, 1)); + r = q.rotate (z); + test_fail_break_unless (test, almost_eq_vect (r, 0, -1, 0)); + } while (0); + do { + ucoo::Test test (tsuite, "rotate y"); + ucoo::Quaternion q (ucoo::YawPitchRoll (0, M_PI_2, 0)); + ucoo::vect3d r; + r = q.rotate (x); + test_fail_break_unless (test, almost_eq_vect (r, 0, 0, -1)); + r = q.rotate (y); + test_fail_break_unless (test, almost_eq_vect (r, 0, 1, 0)); + r = q.rotate (z); + test_fail_break_unless (test, almost_eq_vect (r, 1, 0, 0)); + } while (0); + do { + ucoo::Test test (tsuite, "rotate z"); + ucoo::Quaternion q (ucoo::YawPitchRoll (M_PI_2, 0, 0)); + ucoo::vect3d r; + r = q.rotate (x); + test_fail_break_unless (test, almost_eq_vect (r, 0, 1, 0)); + r = q.rotate (y); + test_fail_break_unless (test, almost_eq_vect (r, -1, 0, 0)); + r = q.rotate (z); + test_fail_break_unless (test, almost_eq_vect (r, 0, 0, 1)); + } while (0); + do { + ucoo::Test test (tsuite, "rotate zyx"); + ucoo::Quaternion q (ucoo::YawPitchRoll (M_PI_2, M_PI_2, M_PI_2)); + ucoo::vect3d r; + r = q.rotate (x); + test_fail_break_unless (test, almost_eq_vect (r, 0, 0, -1)); + r = q.rotate (y); + test_fail_break_unless (test, almost_eq_vect (r, 0, 1, 0)); + r = q.rotate (z); + test_fail_break_unless (test, almost_eq_vect (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 (tsuite, "vect3d int"); test_group_vect3d (tsuite, "vect3d float"); test_group_vect3d (tsuite, "vect3d double"); + test_group_quaternion (tsuite, "quaternion float"); + test_group_quaternion (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 +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 +constexpr +YawPitchRoll::YawPitchRoll () +{ +} + +template +constexpr +YawPitchRoll::YawPitchRoll (T yaw_, T pitch_, T roll_) + : yaw (yaw_), pitch (pitch_), roll (roll_) +{ +} + +} // namespace ucoo + +#endif // ucoo_math_yaw_pitch_roll_tcc -- cgit v1.2.3