diff --git a/quaternion.cpp b/quaternion.cpp index cd30ded5..45af0857 100644 --- a/quaternion.cpp +++ b/quaternion.cpp @@ -34,7 +34,7 @@ template<> const quaternion<4, double> quaternion<4, double>::IDENTITY = { 1, 0, /////////////////////////////////////////////////////////////////////////////// template quaternion -quaternion::rotation (const T radians, const vector<3,T> axis) +quaternion::angle_axis (const T radians, const vector<3,T> axis) { CHECK (is_normalised (axis)); @@ -50,19 +50,56 @@ quaternion::rotation (const T radians, const vector<3,T> axis) //----------------------------------------------------------------------------- template quaternion -quaternion::rotation (const vector<3,T> src, const vector<3,T> dst) +quaternion::from_euler (vector<3,T> angles) { - auto v = util::cross (src, dst); + auto half = angles / 2; + + auto c = cos (half); + auto s = sin (half); return { - std::acos (dot (src, dst)), - v.x, - v.y, - v.z + c.x * c.y * c.z - s.x * s.y * s.z, + s.x * c.y * c.z + c.x * s.y * s.z, + c.x * s.y * c.z - s.x * c.y * s.z, + c.x * c.y * s.z + s.x * s.y * c.z, }; } +/////////////////////////////////////////////////////////////////////////////// +// vector-to-vector rotation algorithm from: +// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final +template +quaternion +quaternion::from_to (const vector<3,T> u, const vector<3,T> v) +{ + CHECK (is_normalised (u)); + CHECK (is_normalised (v)); + + auto norm_u_norm_v = std::sqrt(dot(u, u) * dot(v, v)); + auto real_part = norm_u_norm_v + dot(u, v); + util::vector<3,T> w; + + if (real_part < 1.e-6f * norm_u_norm_v) + { + /* If u and v are exactly opposite, rotate 180 degrees + * around an arbitrary orthogonal axis. Axis normalisation + * can happen later, when we normalise the quaternion. */ + real_part = 0.0f; + w = std::abs(u.x) > std::abs(u.z) ? + util::vector3 (-u.y, u.x, 0.f) : + util::vector3 (0.f, -u.z, u.y); + } + else + { + /* Otherwise, build quaternion the standard way. */ + w = cross(u, v); + } + + return normalised (util::quaternion<4,T> (real_part, w.x, w.y, w.z)); +} + + /////////////////////////////////////////////////////////////////////////////// template quaternion<4,T> @@ -72,6 +109,7 @@ util::conjugate (quaternion<4,T> q) } +//----------------------------------------------------------------------------- template quaternion<4,float> util::conjugate (quaternion<4,float>); @@ -130,6 +168,58 @@ quaternion::as_matrix (void) const } +/////////////////////////////////////////////////////////////////////////////// +// https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion +template +util::vector3 +util::rotate (vector3 v, quaternion<4,T> q) +{ + CHECK (is_normalised (v)); + +#if 1 + util::vector3 u { q.x, q.y, q.z }; + return v + 2 * cross (u, cross (u, v) + q.w * v); +#else + // Verbosely: + quaternionf p { 0, v.x, v.y, v.z }; + auto p_ = q * p * conjugate (q); + return { p_.x, p_.y, p_.z }; +#endif +} + + +//----------------------------------------------------------------------------- +template util::vector3f util::rotate (util::vector3f, util::quaternionf); +template util::vector3d util::rotate (util::vector3d, util::quaterniond); + + +/////////////////////////////////////////////////////////////////////////////// +template +quaternion +quaternion::look (vector<3,T> fwd, vector<3,T> up) +{ + CHECK (is_normalised (fwd)); + CHECK (is_normalised (up)); + + constexpr util::vector3 RH_FWD { 0, 0, -1 }; + constexpr util::vector3 RH_UP { 0, 1, 0 }; + + // rotate the right-handed fwd to face the given fwd + auto q1 = from_to (RH_FWD, fwd); + + // can't retain the up direction if fwd is the same as up + if (norm2 (cross (fwd, up)) < 1e-6) + return q1; + + // find the right-handed up rotated to the given fwd + auto new_up = rotate (RH_UP, q1); + + // rotate first to the new forward, then align the right handed up with + // the given up. + return from_to (new_up, up) * q1; +} + + /////////////////////////////////////////////////////////////////////////////// template std::ostream& diff --git a/quaternion.hpp b/quaternion.hpp index d872fa8e..58f8193d 100644 --- a/quaternion.hpp +++ b/quaternion.hpp @@ -29,7 +29,7 @@ namespace util { // quaternions must be 4 elements, but we include a size parameter so it // fits with the generic coord infrastructure more easily. // - // specifically: + // specifically: // large regions of base code require a template template parameter with // size and type arguments, which is annoying to work around for this one // case. @@ -41,8 +41,11 @@ namespace util { using coord::base::base; - static quaternion rotation (T radians, vector<3,T> axis); - static quaternion rotation (vector<3,T> src, vector<3,T> dst); + static quaternion angle_axis (T radians, vector<3,T> axis); + static quaternion from_euler (vector<3,T>); + + static quaternion from_to (vector<3,T>, vector<3,T>); + static quaternion look (vector<3,T> fwd, vector<3,T> up); matrix4 as_matrix (void) const; @@ -61,7 +64,12 @@ namespace util { quaternion operator/ (const quaternion, const quaternion); - typedef quaternion<4,float> quaternionf; + typedef quaternion<4,float> quaternionf; + typedef quaternion<4,double> quaterniond; + + template + vector3 + rotate (vector3, quaternion<4,T>); template std::ostream& diff --git a/test/affine.cpp b/test/affine.cpp index 4ecd4dfa..c55fdccf 100644 --- a/test/affine.cpp +++ b/test/affine.cpp @@ -54,7 +54,7 @@ test_mq_axis (util::TAP::logger &tap) for (auto t: TESTS) { auto m = util::matrix4f::rotation (1, t.euler); - auto q = util::quaternionf::rotation (1, t.euler); + auto q = util::quaternionf::angle_axis (1, t.euler); auto diff = sum (abs (m - q.as_matrix ())); tap.expect_le (diff, 1e-6f, "matrix/quaternion rotation identities, %s", t.msg); @@ -83,9 +83,9 @@ test_mq_euler (util::TAP::logger &tap) util::matrix4f::rotation (t.euler[1], { 0, 1, 0 }) * util::matrix4f::rotation (t.euler[2], { 0, 0, 1 }); auto q = ( - util::quaternionf::rotation (t.euler[0], { 1, 0, 0 }) * - util::quaternionf::rotation (t.euler[1], { 0, 1, 0 }) * - util::quaternionf::rotation (t.euler[2], { 0, 0, 1 }) + util::quaternionf::angle_axis (t.euler[0], { 1, 0, 0 }) * + util::quaternionf::angle_axis (t.euler[1], { 0, 1, 0 }) * + util::quaternionf::angle_axis (t.euler[2], { 0, 0, 1 }) ).as_matrix (); auto diff = util::sum (abs (m - q)); diff --git a/test/matrix.cpp b/test/matrix.cpp index a7042c9c..f9ddd170 100644 --- a/test/matrix.cpp +++ b/test/matrix.cpp @@ -192,9 +192,9 @@ main (void) constexpr auto PI2 = 2 * util::PI; auto matrix = ( - util::quaternionf::rotation (t.euler[2], { 0, 0, 1 }) * - util::quaternionf::rotation (t.euler[1], { 0, 1, 0 }) * - util::quaternionf::rotation (t.euler[0], { 1, 0, 0 }) + util::quaternionf::angle_axis (t.euler[2], { 0, 0, 1 }) * + util::quaternionf::angle_axis (t.euler[1], { 0, 1, 0 }) * + util::quaternionf::angle_axis (t.euler[0], { 1, 0, 0 }) ).as_matrix (); auto euler = to_euler (matrix); diff --git a/test/quaternion.cpp b/test/quaternion.cpp index d82ade7a..2e9b93e9 100644 --- a/test/quaternion.cpp +++ b/test/quaternion.cpp @@ -76,7 +76,7 @@ main (void) for (size_t i = 0; i < elems (ROTATIONS); ++i) { const auto &r = ROTATIONS[i]; - auto q = quaternionf::rotation (r.mag, r.axis).as_matrix (); + auto q = quaternionf::angle_axis (r.mag, r.axis).as_matrix (); auto m = util::matrix4f::rotation (r.mag, r.axis); auto diff = util::abs (q - m); @@ -87,7 +87,7 @@ main (void) auto m = util::matrix4f::IDENTITY; for (auto r: ROTATIONS) { - q = q.rotation (r.mag, r.axis) * q; + q = q.angle_axis (r.mag, r.axis) * q; m = m.rotation (r.mag, r.axis) * m; }