/* Copyright (c) 2009 Christopher A. Taylor. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of LibCat nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /* Based on code from "Physics for Game Developers", David M. Bourg slerp() code adapted from this website: http://www.number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/index.html */ #ifndef CAT_QUATERNION_HPP #define CAT_QUATERNION_HPP #include #include namespace cat { // Generic quaternion class for linear algebra template class Quaternion { protected: // Backed by a 4D vector with elements arranged as Vector<4, Scalar, Double> _v; public: // Short-hand for the current quaternion type typedef Quaternion mytype; // Short-hand for the vector type typedef Vector<3, Scalar, Double> vectype; // Uninitialized quaternion is not cleared Quaternion() { } // Copy constructor Quaternion(const mytype &u) : _v(u._v) { } // Initialization constructor Quaternion(const vectype &v, Scalar w) : _v(v) { } // Initialization constructor Quaternion(Scalar x, Scalar y, Scalar z, Scalar w) : _v(x, y, z, w) { } // Assignment operator mytype &operator=(const mytype &u) { _v.copy(u._v); return *this; } // Convert from Euler angle representation // Pre-condition: angles in radians (see Deg2Rad in Scalar.hpp) void setFromEulerAngles(f32 xroll, f32 ypitch, f32 zyaw) { f64 croll = cos(0.5f * xroll); f64 cpitch = cos(0.5f * ypitch); f64 cyaw = cos(0.5f * zyaw); f64 sroll = sin(0.5f * xroll); f64 spitch = sin(0.5f * ypitch); f64 syaw = sin(0.5f * zyaw); f64 cyawcpitch = cyaw * cpitch; f64 syawspitch = syaw * spitch; f64 cyawspitch = cyaw * spitch; f64 syawcpitch = syaw * cpitch; _v(0) = static_cast( cyawcpitch * sroll - syawspitch * croll ); _v(1) = static_cast( cyawspitch * croll + syawcpitch * sroll ); _v(2) = static_cast( syawcpitch * croll - cyawspitch * sroll ); _v(3) = static_cast( cyawcpitch * croll + syawspitch * sroll ); _v.normalize(); } // Convert from axis-angle representation // Pre-condition: axis must be unit-length void setFromAxisAngle(const vectype &axis, f32 angle) { angle *= 0.5f; Scalar theta = static_cast( sin(angle) ); _v(0) = theta * axis(0); _v(1) = theta * axis(1); _v(2) = theta * axis(2); _v(3) = static_cast( cos(angle) ); } // Conjugate mytype operator~() const { return mytype(-_v(0), -_v(1), -_v(2), _v(3)); } // Conjugate in-place mytype &conjugate() const { _v(0) = -_v(0); _v(1) = -_v(1); _v(2) = -_v(2); return *this; } // Multiply by quaternion mytype operator*(const mytype &u) { // Cache each of the elements since each is used 4 times Double x1 = _v(0), x2 = u._v(0); Double y1 = _v(1), y2 = u._v(1); Double z1 = _v(2), z2 = u._v(2); Double w1 = _v(3), w2 = u._v(3); // Quaternion multiplication formula: Scalar x3 = static_cast( w1*x2 + x1*w2 + y1*z2 - z1*y2 ); Scalar y3 = static_cast( w1*y2 - x1*z2 + y1*w2 + z1*x2 ); Scalar z3 = static_cast( w1*z2 + x1*y2 - y1*x2 + z1*w2 ); Scalar w3 = static_cast( w1*w2 - x1*x2 - y1*y2 - z1*z2 ); return mytype(x3, y3, z3, w3); } // Multiply by quaternion in-place: this = this * u mytype &operator*=(const mytype &u) { // Cache each of the elements since each is used 4 times Double x1 = _v(0), x2 = u._v(0); Double y1 = _v(1), y2 = u._v(1); Double z1 = _v(2), z2 = u._v(2); Double w1 = _v(3), w2 = u._v(3); // Quaternion multiplication formula: _v(0) = static_cast( w1*x2 + x1*w2 + y1*z2 - z1*y2 ); _v(1) = static_cast( w1*y2 - x1*z2 + y1*w2 + z1*x2 ); _v(2) = static_cast( w1*z2 + x1*y2 - y1*x2 + z1*w2 ); _v(3) = static_cast( w1*w2 - x1*x2 - y1*y2 - z1*z2 ); return *this; } // Multiply by quaternion in-place: this = u * this mytype &postMultiply(const mytype &u) { // Cache each of the elements since each is used 4 times Double x2 = _v(0), x1 = u._v(0); Double y2 = _v(1), y1 = u._v(1); Double z2 = _v(2), z1 = u._v(2); Double w2 = _v(3), w1 = u._v(3); // Quaternion multiplication formula: _v(0) = static_cast( w1*x2 + x1*w2 + y1*z2 - z1*y2 ); _v(1) = static_cast( w1*y2 - x1*z2 + y1*w2 + z1*x2 ); _v(2) = static_cast( w1*z2 + x1*y2 - y1*x2 + z1*w2 ); _v(3) = static_cast( w1*w2 - x1*x2 - y1*y2 - z1*z2 ); return *this; } // Rotate given vector in-place void rotate(vectype &u) { // Implements the simple formula: (q1 * u2 * ~q1).getVector() // Cache each of the elements since each is used 4 times Double x1 = _v(0), x2 = u(0); Double y1 = _v(1), y2 = u(1); Double z1 = _v(2), z2 = u(2); Double w1 = _v(3); // Quaternion-vector multiplication formula (q3 = q1 * u2): Double x3 = w1*x2 + y1*z2 - z1*y2; Double y3 = w1*y2 - x1*z2 + z1*x2; Double z3 = w1*z2 + x1*y2 - y1*x2; Double w3 = -(x1*x2 + y1*y2 + z1*z2); // Quaternion multiplication formula (q2' = q3 * ~q1): u(0) = static_cast( w1*x3 - x1*w3 + y1*z3 - z1*y3 ); u(1) = static_cast( w1*y3 - x1*z3 - y1*w3 + z1*x3 ); u(2) = static_cast( w1*z3 + x1*y3 - y1*x3 - z1*w3 ); } // NLERP: Very fast, non-constant velocity and torque-minimal // Precondition: q1, q2 are unit length friend void nlerp(const mytype &q1, const mytype &q2, Scalar t, mytype &result) { // Linearly interpolate and normalize result // This formula is a little more work than "q1 + (q2 - q1) * t" // but less likely to lose precision when it matters // Cosine of phi, the angle between the two vectors Double cos_phi = q1._v.dotProduct(q2._v); // I have read this may try to rotate around the "long way" sometimes and to // fix that we check if cos(phi) is negative and invert one of the inputs. if (cos_phi < 0.0) { result._v = -q1._v; } else { result._v = q1._v; } // Simple linear interpolation Scalar scale0 = static_cast(1) - t; Scalar scale1 = t; result._v *= scale0; result._v += q2._v * scale1; result._v.normalize(); } // SLERP: Slower, constant velocity and torque-minimal // Precondition: q1, q2 are unit length friend void slerp(const mytype &q1, const mytype &q2, Scalar t, mytype &result) { // Cosine of phi, the angle between the two vectors Double cos_phi = q1._v.dotProduct(q2._v); // I have read this may try to rotate around the "long way" sometimes and to // fix that we check if cos(phi) is negative and invert one of the inputs. if (cos_phi < 0.0) { cos_phi = -cos_phi; result._v = -q1._v; } else { result._v = q1._v; } // Default to simple linear interpolation Scalar scale0 = static_cast(1) - t; Scalar scale1 = t; // If the distance is not small we need to do full slerp: if (cos_phi < 0.9995) { // cos_phi is guaranteed to be within the domain of acos(), 0..1 Double phi = static_cast( acos(cos_phi) ); Double inv_sin_phi = static_cast(1) / sin(phi); scale0 = static_cast( sin(scale0 * phi) * inv_sin_phi ); scale1 = static_cast( sin(scale1 * phi) * inv_sin_phi ); } result._v *= scale0; result._v += q2._v * scale1; result._v.normalize(); } // Get angle of rotation Scalar getAngle() { return static_cast( 2 ) * static_cast( acos(_v(3)) ); } // Get axis of rotation vectype getAxis() { return vectype(_v(0), _v(1), _v(2)).normalize(); } // Get matrix form of the rotation represented by this quaternion void getMatrix4x4(Matrix<4, 4, Scalar> &result) { Double dx = _v(0); Double dy = _v(1); Double dz = _v(2); Double dw = _v(3); Double x2 = dx * dx; Double y2 = dy * dy; Double z2 = dz * dz; Double xy = dx * dy; Double yz = dy * dz; Double zx = dz * dx; Double xw = dx * dw; Double yw = dy * dw; Double zw = dz * dw; const Double ONE = static_cast( 1 ); const Double TWO = static_cast( 2 ); // Result is written in OpenGL column-major matrix order: result( 0) = static_cast( ONE - TWO * (y2 + z2) ); result( 1) = static_cast( TWO * (xy - zw) ); result( 2) = static_cast( TWO * (zx + yw) ); result( 3) = static_cast( 0 ); result( 4) = static_cast( TWO * (xy + zw) ); result( 5) = static_cast( ONE - TWO * (x2 + z2) ); result( 6) = static_cast( TWO * (yz - xw) ); result( 7) = static_cast( 0 ); result( 8) = static_cast( TWO * (zx - yw) ); result( 9) = static_cast( TWO * (yz + xw) ); result(10) = static_cast( ONE - TWO * (x2 + y2) ); result(11) = static_cast( 0 ); result(12) = static_cast( 0 ); result(13) = static_cast( 0 ); result(14) = static_cast( 0 ); result(15) = static_cast( ONE ); } // Get Euler angles vectype Quaternion::getEulerAngles() { Double dx = _v(0); Double dy = _v(1); Double dz = _v(2); Double dw = _v(3); Double x2 = dx * dx; Double y2 = dy * dy; Double z2 = dz * dz; Double w2 = dw * dw; Double xy = dx * dy; Double yz = dy * dz; Double xw = dx * dw; Double yw = dy * dw; Double zw = dz * dw; const Double TWO = static_cast( 2 ); Double r11 = w2 + x2 - y2 - z2; Double r21 = TWO * (xy + zw); Double r31 = TWO * (xy - yw); Double r32 = TWO * (yz + xw); Double r33 = w2 - x2 - y2 + z2; Double tmp = fabs(r31); const Double LIMIT = static_cast( 0.999999 ); if (tmp > LIMIT) { Double xz = dx * dz; Double r12 = TWO * (yz - zw); Double r13 = TWO * (xz + yw); return vectype(static_cast( 0 ), static_cast( -CAT_HALF_PI_64 * r31 / tmp ), static_cast( atan2(-r12, -r31 * r13) )); } return vectype(static_cast( atan2(r32, r33) ), static_cast( asin(-r31) ), static_cast( atan2(r21, r11) )); } }; // Short-hand for common usages: typedef Quaternion Quaternion4f; typedef Quaternion Quaternion4d; } // namespace cat #endif // CAT_QUATERNION_HPP