Quaternion.hpp

Go to the documentation of this file.

/*
    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 <cat/gfx/Vector.hpp>
#include <cat/gfx/Matrix.hpp>

namespace cat {


// Generic quaternion class for linear algebra
template <typename Scalar, typename Double>
class Quaternion
{
protected:
    // Backed by a 4D vector with elements arranged as <x,y,z,w>
    Vector<4, Scalar, Double> _v;

public:
    // Short-hand for the current quaternion type
    typedef Quaternion<Scalar, Double> 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<Scalar>( cyawcpitch * sroll - syawspitch * croll );
        _v(1) = static_cast<Scalar>( cyawspitch * croll + syawcpitch * sroll );
        _v(2) = static_cast<Scalar>( syawcpitch * croll - cyawspitch * sroll );
        _v(3) = static_cast<Scalar>( 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<Scalar>( sin(angle) );

        _v(0) = theta * axis(0);
        _v(1) = theta * axis(1);
        _v(2) = theta * axis(2);
        _v(3) = static_cast<Scalar>( 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<Scalar>( w1*x2 + x1*w2 + y1*z2 - z1*y2 );
        Scalar y3 = static_cast<Scalar>( w1*y2 - x1*z2 + y1*w2 + z1*x2 );
        Scalar z3 = static_cast<Scalar>( w1*z2 + x1*y2 - y1*x2 + z1*w2 );
        Scalar w3 = static_cast<Scalar>( 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<Scalar>( w1*x2 + x1*w2 + y1*z2 - z1*y2 );
        _v(1) = static_cast<Scalar>( w1*y2 - x1*z2 + y1*w2 + z1*x2 );
        _v(2) = static_cast<Scalar>( w1*z2 + x1*y2 - y1*x2 + z1*w2 );
        _v(3) = static_cast<Scalar>( 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<Scalar>( w1*x2 + x1*w2 + y1*z2 - z1*y2 );
        _v(1) = static_cast<Scalar>( w1*y2 - x1*z2 + y1*w2 + z1*x2 );
        _v(2) = static_cast<Scalar>( w1*z2 + x1*y2 - y1*x2 + z1*w2 );
        _v(3) = static_cast<Scalar>( 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<Scalar>( w1*x3 - x1*w3 + y1*z3 - z1*y3 );
        u(1) = static_cast<Scalar>( w1*y3 - x1*z3 - y1*w3 + z1*x3 );
        u(2) = static_cast<Scalar>( 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<Scalar>(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<Scalar>(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<Double>( acos(cos_phi) );
            Double inv_sin_phi = static_cast<Double>(1) / sin(phi);

            scale0 = static_cast<Scalar>( sin(scale0 * phi) * inv_sin_phi );
            scale1 = static_cast<Scalar>( 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<Scalar>( 2 ) * static_cast<Scalar>( 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<Double>( 1 );
        const Double TWO = static_cast<Double>( 2 );

        // Result is written in OpenGL column-major matrix order:
        result( 0) = static_cast<Scalar>( ONE - TWO * (y2 + z2) );
        result( 1) = static_cast<Scalar>( TWO * (xy - zw) );
        result( 2) = static_cast<Scalar>( TWO * (zx + yw) );
        result( 3) = static_cast<Scalar>( 0 );

        result( 4) = static_cast<Scalar>( TWO * (xy + zw) );
        result( 5) = static_cast<Scalar>( ONE - TWO * (x2 + z2) );
        result( 6) = static_cast<Scalar>( TWO * (yz - xw) );
        result( 7) = static_cast<Scalar>( 0 );

        result( 8) = static_cast<Scalar>( TWO * (zx - yw) );
        result( 9) = static_cast<Scalar>( TWO * (yz + xw) );
        result(10) = static_cast<Scalar>( ONE - TWO * (x2 + y2) );
        result(11) = static_cast<Scalar>( 0 );

        result(12) = static_cast<Scalar>( 0 );
        result(13) = static_cast<Scalar>( 0 );
        result(14) = static_cast<Scalar>( 0 );
        result(15) = static_cast<Scalar>( 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<Double>( 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<Double>( 0.999999 );

        if (tmp > LIMIT)
        {
            Double xz = dx * dz;

            Double r12 = TWO * (yz - zw);
            Double r13 = TWO * (xz + yw);

            return vectype(static_cast<Scalar>( 0 ),
                           static_cast<Scalar>( -CAT_HALF_PI_64 * r31 / tmp ),
                           static_cast<Scalar>( atan2(-r12, -r31 * r13) ));
        }

        return vectype(static_cast<Scalar>( atan2(r32, r33) ),
                       static_cast<Scalar>( asin(-r31) ),
                       static_cast<Scalar>( atan2(r21, r11) ));
    }
};


// Short-hand for common usages:

typedef Quaternion<f32, f64> Quaternion4f;
typedef Quaternion<f64, f64> Quaternion4d;


} // namespace cat

#endif // CAT_QUATERNION_HPP