gls_quaternion.h

Go to the documentation of this file.

/*! \file
  \brief  The disti::GlsQuaternion class.

  \par Copyright Information

  Copyright (c) 2017 by The DiSTI Corporation.<br>
  11301 Corporate Blvd., Suite 100<br>
  Orlando, Florida 32817<br>
  USA<br>
  <br>
  All rights reserved.<br>

  This Software contains proprietary trade secrets of DiSTI and may not be
reproduced, in whole or part, in any form, or by any means of electronic,
mechanical, or otherwise, without the written permission of DiSTI.  Said
permission may be derived through the purchase of applicable DiSTI product
licenses which detail the distribution rights of this content and any
Derivative Works based on this or other copyrighted DiSTI Software.

  NO WARRANTY. THE SOFTWARE IS PROVIDED "AS-IS," WITHOUT WARRANTY OF ANY KIND,
AND ANY USE OF THIS SOFTWARE PRODUCT IS AT YOUR OWN RISK. TO THE MAXIMUM EXTENT
PERMITTED BY APPLICABLE LAW, DISTI AND ITS SUPPLIERS DISCLAIM ALL WARRANTIES
AND CONDITIONS, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
IMPLIED WARRANTIES AND CONDITIONS OF MERCHANTABILITY AND/OR FITNESS FOR A
PARTICULAR PURPOSE, TITLE, AND NON-INFRINGEMENT, WITH REGARD TO THE SOFTWARE.

  LIMITATION OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW,
IN NO EVENT SHALL DISTI OR ITS SUPPLIERS BE LIABLE FOR ANY SPECIAL, INCIDENTAL,
INDIRECT, OR CONSEQUENTIAL DAMAGES WHATSOEVER (INCLUDING, WITHOUT LIMITATION,
DAMAGES FOR LOSS OF BUSINESS PROFITS, BUSINESS INTERRUPTION, LOSS OF BUSINESS
INFORMATION, OR ANY OTHER PECUNIARY LOSS) ARISING OUT OF THE USE OF OR
INABILITY TO USE THE SOFTWARE, EVEN IF DISTI HAS BEEN ADVISED OF THE POSSIBILITY
OF SUCH DAMAGES.  DISTI'S ENTIRE LIABILITY AND YOUR EXCLUSIVE REMEDY SHALL NOT
EXCEED FIVE DOLLARS (US$5.00).

  The aforementioned terms and restrictions are governed by the laws of the
State of Florida and the United States of America.

*/

#ifndef GLS_QUATERNION_H
#define GLS_QUATERNION_H

#include "gls_cpp_lang_support.h"
#include "vertex.h"

namespace disti
{
template<class Type>
class GlsMatrixAffine;

/** The GlsQuaternion class */
template<class Type>
class GlsQuaternion
{
public:
    //------------------------------------------------------------------------
    /** 
    * Default constructor. This will initialize the quaternion to zero.
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion()
    {
        _v[ 0 ] = 0;
        _v[ 1 ] = 0;
        _v[ 2 ] = 0;
        _v[ 3 ] = 0;
    }

    //------------------------------------------------------------------------
    /** 
    * Constructor.  Sets quaternion from 4 constants.
    * \param x  Initial X value
    * \param y  Initial Y value
    * \param z  Initial Z value
    * \param w  Initial W value
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion( Type x, Type y, Type z, Type w )
    {
        _v[ 0 ] = x;
        _v[ 1 ] = y;
        _v[ 2 ] = z;
        _v[ 3 ] = w;
    }

    //------------------------------------------------------------------------
    /** 
    * Constructor.  Sets quaternion from angle about an arbitrary axis
    * \param angle  Rotation angle in radians
    * \param axis   Unit vector representing arbitrary axis 
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion( Type angle, const Vector& axis )
    {
        SetFromAngleAxis( angle, axis );
    }

    //------------------------------------------------------------------------
    /** 
    * Constructor.  Sets quaternion from 3 angles about 3 axes
    * \param angle1  Rotation angle about first axis in radians
    * \param axis1   Unit vector representing first arbitrary axis 
    * \param angle2  Rotation angle about second axis in radians
    * \param axis2   Unit vector representing second arbitrary axis 
    * \param angle3  Rotation angle about third axis in radians
    * \param axis3   Unit vector representing third arbitrary axis 
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion( Type angle1, const Vector& axis1,
        Type angle2, const Vector& axis2,
        Type angle3, const Vector& axis3 )
    {
        SetFromAngleAxis( angle1, axis1, angle2, axis2, angle3, axis3 );
    }

    //------------------------------------------------------------------------
    /** 
    * Constructor.  Sets quaternion from 3 angles (Euler Angles)
    * \param angleX  Rotation angle about X axis in radians
    * \param angleY  Rotation angle about Y axis in radians
    * \param angleZ  Rotation angle about Z axis in radians
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion( Type angleX, Type angleY, Type angleZ )
    {
        SetFromEulerAngles( angleX, angleY, angleZ );
    }

    //------------------------------------------------------------------------
    /** 
    * Constructor.  Sets quaternion from 3 angles (Euler Angles)
    * \param anglesVec  A vector where each component of the vector is an
    *                   Euler angle component in radians.
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion( const Vector& anglesVec )
    {
        SetFromEulerAngles( anglesVec.x, anglesVec.y, anglesVec.z );
    }

    //------------------------------------------------------------------------
    /** 
    * Constructor.  Sets quaternion from a rotation matrix
    * \param matrix  An affine matrix representing a rotation matrix
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion( const GlsMatrixAffine<Type>& matrix )
    {
        SetFromRotationMatrix( matrix );
    }

    //------------------------------------------------------------------------
    /** 
    * Accessor method providing access to quaternion data.
    * \return  A pointer to the quaternion data.
    */
    //------------------------------------------------------------------------
    inline Type* Data()
    {
        return _v;
    }

    //------------------------------------------------------------------------
    /** 
    * Accessor method providing access to quaternion data.
    * \return  A pointer to the quaternion data.
    */
    //------------------------------------------------------------------------
    inline const Type* Data() const
    {
        return _v;
    }

    //------------------------------------------------------------------------
    /** 
    * Boolean equivalence operator
    * \param  q  Quaternion to compare against
    * \return  True if the two quaternions are exactly equal
    */
    //------------------------------------------------------------------------
    inline bool operator==( const GlsQuaternion& q ) const
    {
        return _v[ 0 ] == q._v[ 0 ] && _v[ 1 ] == q._v[ 1 ] && _v[ 2 ] == q._v[ 2 ] && _v[ 3 ] == q._v[ 3 ];
    }

    //------------------------------------------------------------------------
    /** 
    * Boolean non-equivalence operator
    * \param  q  Quaternion to compare against
    * \return  True if the two quaternions are not exactly equal
    */
    //------------------------------------------------------------------------
    inline bool operator!=( const GlsQuaternion& q ) const
    {
        return _v[ 0 ] != q._v[ 0 ] || _v[ 1 ] != q._v[ 1 ] || _v[ 2 ] != q._v[ 2 ] || _v[ 3 ] != q._v[ 3 ];
    }

    //------------------------------------------------------------------------
    /** 
    * Boolean less than operator
    * \param  q  Quaternion to compare against
    * \return  True if the this quaternion is less than the provided one
    */
    //------------------------------------------------------------------------
    inline bool operator<( const GlsQuaternion& q ) const
    {
        if( _v[ 0 ] < q._v[ 0 ] )
            return true;
        else if( _v[ 0 ] > q._v[ 0 ] )
            return false;
        else if( _v[ 1 ] < q._v[ 1 ] )
            return true;
        else if( _v[ 1 ] > q._v[ 1 ] )
            return false;
        else if( _v[ 2 ] < q._v[ 2 ] )
            return true;
        else if( _v[ 2 ] > q._v[ 2 ] )
            return false;
        else
            return ( _v[ 3 ] < q._v[ 3 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Scalar multiplication operator
    * \param  s  A scalar to multiply against
    * \return  This quaternion multiplied by the scalar
    */
    //------------------------------------------------------------------------
    inline const GlsQuaternion operator*( Type s ) const
    {
        return GlsQuaternion( _v[ 0 ] * s, _v[ 1 ] * s, _v[ 2 ] * s, _v[ 3 ] * s );
    }

    //------------------------------------------------------------------------
    /** 
    * Scalar multiplication operator and assignment operator
    * \param  s  A scalar to multiply against
    * \return  This quaternion multiplied by the scalar
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion& operator*=( Type s )
    {
        _v[ 0 ] *= s;
        _v[ 1 ] *= s;
        _v[ 2 ] *= s;
        _v[ 3 ] *= s;
        return *this; // enable nesting
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion multiplication operator  
    * \param q  A quaternion to multiply against
    * \return  This quaternion multiplied by the quaternion
    */
    //------------------------------------------------------------------------
    inline const GlsQuaternion operator*( const GlsQuaternion& q ) const
    {
        return GlsQuaternion( q._v[ 3 ] * _v[ 0 ] + q._v[ 0 ] * _v[ 3 ] + q._v[ 1 ] * _v[ 2 ] - q._v[ 2 ] * _v[ 1 ],
            q._v[ 3 ] * _v[ 1 ] - q._v[ 0 ] * _v[ 2 ] + q._v[ 1 ] * _v[ 3 ] + q._v[ 2 ] * _v[ 0 ],
            q._v[ 3 ] * _v[ 2 ] + q._v[ 0 ] * _v[ 1 ] - q._v[ 1 ] * _v[ 0 ] + q._v[ 2 ] * _v[ 3 ],
            q._v[ 3 ] * _v[ 3 ] - q._v[ 0 ] * _v[ 0 ] - q._v[ 1 ] * _v[ 1 ] - q._v[ 2 ] * _v[ 2 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion multiplication operator  
    * \param q  A quaternion to multiply against
    * \return  This quaternion multiplied by the quaternion
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion& operator*=( const GlsQuaternion& q )
    {
        Type x  = q._v[ 3 ] * _v[ 0 ] + q._v[ 0 ] * _v[ 3 ] + q._v[ 1 ] * _v[ 2 ] - q._v[ 2 ] * _v[ 1 ];
        Type y  = q._v[ 3 ] * _v[ 1 ] - q._v[ 0 ] * _v[ 2 ] + q._v[ 1 ] * _v[ 3 ] + q._v[ 2 ] * _v[ 0 ];
        Type z  = q._v[ 3 ] * _v[ 2 ] + q._v[ 0 ] * _v[ 1 ] - q._v[ 1 ] * _v[ 0 ] + q._v[ 2 ] * _v[ 3 ];
        _v[ 3 ] = q._v[ 3 ] * _v[ 3 ] - q._v[ 0 ] * _v[ 0 ] - q._v[ 1 ] * _v[ 1 ] - q._v[ 2 ] * _v[ 2 ];

        _v[ 2 ] = z;
        _v[ 1 ] = y;
        _v[ 0 ] = x;

        return ( *this ); // enable nesting
    }

    //------------------------------------------------------------------------
    /** 
    * Scalar division operator
    * \param  s  A scalar to divide
    * \return  This quaternion divided by the scalar
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion operator/( Type s ) const
    {
        Type div = 1.0 / s;
        return GlsQuaternion( _v[ 0 ] * div, _v[ 1 ] * div, _v[ 2 ] * div, _v[ 3 ] * div );
    }

    //------------------------------------------------------------------------
    /** 
    * Scalar division and assignment operator
    * \param  s  A scalar to divide
    * \return  This quaternion divided by the scalar
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion& operator/=( Type s )
    {
        Type div = 1 / s;
        _v[ 0 ] *= div;
        _v[ 1 ] *= div;
        _v[ 2 ] *= div;
        _v[ 3 ] *= div;
        return *this;
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion division operator  
    * \param q  A quaternion to division by
    * \return  This quaternion divided by the quaternion
    */
    //------------------------------------------------------------------------
    inline const GlsQuaternion operator/( const GlsQuaternion& q ) const
    {
        return ( ( *this ) * q.Inverse() );
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion division operator  
    * \param q  A quaternion to division by
    * \return  This quaternion divided by the quaternion
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion& operator/=( const GlsQuaternion& q )
    {
        ( *this ) = ( *this ) * q.Inverse();
        return ( *this ); // enable nesting
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion addition operator  
    * \param q  A quaternion to add
    * \return  This quaternion plus the supplied quaternion
    */
    //------------------------------------------------------------------------
    inline const GlsQuaternion operator+( const GlsQuaternion& q ) const
    {
        return GlsQuaternion( _v[ 0 ] + q._v[ 0 ], _v[ 1 ] + q._v[ 1 ],
            _v[ 2 ] + q._v[ 2 ], _v[ 3 ] + q._v[ 3 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion addition and assignment operator  
    * \param q  A quaternion to add
    * \return  This quaternion plus the supplied quaternion
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion& operator+=( const GlsQuaternion& q )
    {
        _v[ 0 ] += q._v[ 0 ];
        _v[ 1 ] += q._v[ 1 ];
        _v[ 2 ] += q._v[ 2 ];
        _v[ 3 ] += q._v[ 3 ];
        return *this; // enable nesting
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion subtrtaction operator  
    * \param q  A quaternion to subtract
    * \return  This quaternion minus the supplied quaternion
    */
    //------------------------------------------------------------------------
    inline const GlsQuaternion operator-( const GlsQuaternion& q ) const
    {
        return GlsQuaternion( _v[ 0 ] - q._v[ 0 ], _v[ 1 ] - q._v[ 1 ],
            _v[ 2 ] - q._v[ 2 ], _v[ 3 ] - q._v[ 3 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Quaternion subtraction and assignment operator  
    * \param q  A quaternion to subtract
    * \return  This quaternion minus the supplied quaternion
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion& operator-=( const GlsQuaternion& q )
    {
        _v[ 0 ] -= q._v[ 0 ];
        _v[ 1 ] -= q._v[ 1 ];
        _v[ 2 ] -= q._v[ 2 ];
        _v[ 3 ] -= q._v[ 3 ];
        return *this; // enable nesting
    }

    /** Negation operator - returns the negative of the quaternion.
    Basically just calls operator - () on the Vec4 */
    inline const GlsQuaternion operator-() const
    {
        return GlsQuaternion( -_v[ 0 ], -_v[ 1 ], -_v[ 2 ], -_v[ 3 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Returns the magnitude of the quaternion
    * \return  The magnitude of the quaternion
    */
    //------------------------------------------------------------------------
    inline Type Magnitude() const
    {
        return sqrt( _v[ 0 ] * _v[ 0 ] + _v[ 1 ] * _v[ 1 ] + _v[ 2 ] * _v[ 2 ] + _v[ 3 ] * _v[ 3 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Returns the magnitude squared of the quaternion
    * \return  The magnitude squared of the quaternion
    */
    //------------------------------------------------------------------------
    inline Type MagnitudeSquared() const
    {
        return _v[ 0 ] * _v[ 0 ] + _v[ 1 ] * _v[ 1 ] + _v[ 2 ] * _v[ 2 ] + _v[ 3 ] * _v[ 3 ];
    }

    //------------------------------------------------------------------------
    /** 
    * Returns the conjugate of the quaternion
    * \return  The conjugate of the quaternion
    */
    //------------------------------------------------------------------------
    inline GlsQuaternion Conjugate() const
    {
        return GlsQuaternion( -_v[ 0 ], -_v[ 1 ], -_v[ 2 ], _v[ 3 ] );
    }

    //------------------------------------------------------------------------
    /** 
    * Returns the inverse of the quaternion
    * \return  The inverse of the quaternion
    */
    //------------------------------------------------------------------------
    inline const GlsQuaternion Inverse() const
    {
        return Conjugate() / MagnitudeSquared();
    }

    //------------------------------------------------------------------------
    /** 
    * Returns the dot product of the quaternion and a supplied quaternion
    * \param q  A quaternion to multiply against this quaternion
    * \return The dot product of the quaternion and a supplied quaternion
    */
    //------------------------------------------------------------------------
    inline Type DotProduct( const GlsQuaternion& q ) const
    {
        return ( ( _v[ 0 ] * q._v[ 0 ] ) + ( _v[ 1 ] * q._v[ 1 ] ) + ( _v[ 2 ] * q._v[ 2 ] ) + ( _v[ 3 ] * q._v[ 3 ] ) );
    }

    //------------------------------------------------------------------------
    /** 
    * Normalizes this quaternion
    */
    //------------------------------------------------------------------------
    inline void Normalize()
    {
        *this /= Magnitude();
    }

    //------------------------------------------------------------------------
    /** 
    * Sets the component values for this quaternion
    * \param x  The X value
    * \param y  The Y value
    * \param z  The Z value
    * \param w  The W value
    */
    //------------------------------------------------------------------------
    inline void Set( Type x, Type y, Type z, Type w )
    {
        _v[ 0 ] = x;
        _v[ 1 ] = y;
        _v[ 2 ] = z;
        _v[ 3 ] = w;
    }

    //------------------------------------------------------------------------
    /** 
    * Sets quaternion from angle about an arbitrary axis
    * \param angle  Rotation angle in radians
    * \param axis   Unit vector representing arbitrary axis 
    */
    //------------------------------------------------------------------------
    void SetFromAngleAxis( Type angle, const Vector& axis )
    {
        Type half_angle     = angle * (Type)0.5;
        Type sin_half_angle = sin( half_angle );

        _v[ 0 ] = sin_half_angle * axis.x;
        _v[ 1 ] = sin_half_angle * axis.y;
        _v[ 2 ] = sin_half_angle * axis.z;
        _v[ 3 ] = cos( half_angle );
    }

    //------------------------------------------------------------------------
    /** 
    * Sets quaternion from three angle-axis pairs.
    * \param angle1  Rotation angle in radians
    * \param axis1   Unit vector representing arbitrary axis 
    * \param angle2  Rotation angle in radians
    * \param axis2   Unit vector representing arbitrary axis 
    * \param angle3  Rotation angle in radians
    * \param axis3   Unit vector representing arbitrary axis 
    */
    //------------------------------------------------------------------------
    void SetFromAngleAxis( Type angle1, const Vector& axis1,
        Type angle2, const Vector& axis2,
        Type angle3, const Vector& axis3 )
    {
        GlsQuaternion q1( angle1, axis1 );
        GlsQuaternion q2( angle2, axis2 );
        SetFromAngleAxis( angle3, axis3 );

        *this = q1 * q2 * *this;
    }

    //------------------------------------------------------------------------
    /** 
    * Sets quaternion from the Euler angles.
    * \param angleX The X Euler angle in radians
    * \param angleY The Y Euler angle in radians
    * \param angleZ The Z Euler angle in radians
    */
    //------------------------------------------------------------------------
    void SetFromEulerAngles( Type angleX, Type angleY, Type angleZ )
    {
        GlsQuaternion<Type> qx, qy, qz;

        // precompute half angles
        Type xOver2 = angleX * (Type)0.5;
        Type yOver2 = angleY * (Type)0.5;
        Type zOver2 = angleZ * (Type)0.5;

        // set the pitch quat
        qx._v[ 0 ] = sin( xOver2 );
        qx._v[ 1 ] = (Type)0.0;
        qx._v[ 2 ] = (Type)0.0;
        qx._v[ 3 ] = cos( xOver2 );

        // set the yaw quat
        qy._v[ 0 ] = (Type)0.0;
        qy._v[ 1 ] = sin( yOver2 );
        qy._v[ 2 ] = (Type)0.0;
        qy._v[ 3 ] = cos( yOver2 );

        // set the roll quat
        qz._v[ 0 ] = (Type)0.0;
        qz._v[ 1 ] = (Type)0.0;
        qz._v[ 2 ] = sin( zOver2 );
        qz._v[ 3 ] = cos( zOver2 );

        // compose the three in x y z order...
        *this = qx * qy * qz;

        this->Normalize();
    }

    //------------------------------------------------------------------------
    /** 
    * Sets this quaternion to a rotation that rotates 'to' into 'from'.
    * \param to The rotation destination
    * \param from The rotation start point
    */
    //------------------------------------------------------------------------
    void SetFromVectors( const Vector& to, const Vector& from )
    {
        // dot product vec1*vec2
        Type cosangle = from.DotProduct( to ) / ( from.Magnitude() * to.Magnitude() );

        if( IsVeryCloseToZero( cosangle - 1 ) )
        {
            // cosangle is close to 1, so the vectors are close to being coincident
            // Need to generate an angle of zero with any vector we like
            // We'll choose (1,0,0)
            SetFromAngleAxis( 0.0, Vector( 1.0, 0.0, 0.0 ) );
        }
        else if( IsVeryCloseToZero( cosangle + 1.0 ) )
        {
            // vectors are close to being opposite, so will need to find a
            // vector orthongonal to from to rotate about.
            Vector tmp;
            if( fabs( from.x ) < fabs( from.y ) )
                if( fabs( from.x ) < fabs( from.z ) )
                    tmp = Vector( 1.0, 0.0, 0.0 ); // use x axis.
                else
                    tmp = Vector( 0.0, 0.0, 1.0 );
            else if( fabs( from.y ) < fabs( from.z ) )
                tmp = Vector( 0.0, 1.0, 0.0 );
            else
                tmp = Vector( 0.0, 0.0, 1.0 );

            // find orthogonal axis.
            Vector axis( from.CrossProduct( tmp ) );
            axis.Normalize();

            _v[ 0 ] = axis.x; // sin of half angle of PI is 1.0.
            _v[ 1 ] = axis.y; // sin of half angle of PI is 1.0.
            _v[ 2 ] = axis.z; // sin of half angle of PI is 1.0.
            _v[ 3 ] = 0;      // cos of half angle of PI is zero.
        }
        else
        {
            // This is the usual situation - take a cross-product of vec1 and vec2
            // and that is the axis around which to rotate.
            Vector axis( from.CrossProduct( to ) );
            axis.Normalize();
            Type angle = acos( cosangle );
            SetFromAngleAxis( angle, axis );
        }
    }

    //------------------------------------------------------------------------
    /** 
    * Sets this quaternion from the specified rotation matrix.
    * \param matrix The rotation matrix
    */
    //------------------------------------------------------------------------
    void SetFromRotationMatrix( const GlsMatrixAffine<Type>& matrix )
    {
        Type        tr, s;
        Type        tq[ 4 ];
        int         i, j, k;
        const Type* mat = const_cast<GlsMatrixAffine<Type>&>( matrix ).Data();

        int nxt[ 3 ] = { 1, 2, 0 };

        tr = mat[ 0 * 4 + 0 ] + mat[ 1 * 4 + 1 ] + mat[ 2 * 4 + 2 ];

        // check the diagonal
        if( tr > 0.0 )
        {
            s       = (Type)sqrt( tr + 1.0 );
            _v[ 3 ] = s / 2.0f;
            s       = 0.5f / s;
            _v[ 0 ] = ( mat[ 1 * 4 + 2 ] - mat[ 2 * 4 + 1 ] ) * s;
            _v[ 1 ] = ( mat[ 2 * 4 + 0 ] - mat[ 0 * 4 + 2 ] ) * s;
            _v[ 2 ] = ( mat[ 0 * 4 + 1 ] - mat[ 1 * 4 + 0 ] ) * s;
        }
        else
        {
            // diagonal is negative
            i = 0;
            if( mat[ 1 * 4 + 1 ] > mat[ 0 * 4 + 0 ] )
                i = 1;
            if( mat[ 2 * 4 + 2 ] > mat[ i * 4 + i ] )
                i = 2;
            j = nxt[ i ];
            k = nxt[ j ];

            s = (Type)sqrt( ( mat[ i * 4 + i ] - ( mat[ j * 4 + j ] + mat[ k * 4 + k ] ) ) + 1.0 );

            tq[ i ] = s * 0.5f;

            if( s != 0.0f )
                s = 0.5f / s;

            tq[ 3 ] = ( mat[ j * 4 + k ] - mat[ k * 4 + j ] ) * s;
            tq[ j ] = ( mat[ i * 4 + j ] + mat[ j * 4 + i ] ) * s;
            tq[ k ] = ( mat[ i * 4 + k ] + mat[ k * 4 + i ] ) * s;

            _v[ 0 ] = tq[ 0 ];
            _v[ 1 ] = tq[ 1 ];
            _v[ 2 ] = tq[ 2 ];
            _v[ 3 ] = tq[ 3 ];
        }
    }

    /** Returns the rotation about the X axis
      * \return The rotation about the X axis
      */
    Type GetRoll( void ) const
    {
        return atan2( 2 * ( _v[ 1 ] * _v[ 2 ] + _v[ 3 ] * _v[ 0 ] ), _v[ 3 ] * _v[ 3 ] - _v[ 0 ] * _v[ 0 ] - _v[ 1 ] * _v[ 1 ] + _v[ 2 ] * _v[ 2 ] );
    }

    /** Returns the rotation about the Y axis
      * \return The rotation about the Y axis
      */
    Type GetPitch( void ) const
    {
        Type tmp = -2 * ( _v[ 0 ] * _v[ 2 ] - _v[ 3 ] * _v[ 1 ] );

        if( tmp > 1.0 )
        {
            tmp = 1.0;
        }
        else if( tmp < -1.0 )
        {
            tmp = -1.0;
        }

        return asin( tmp );
    }
    /** Returns the rotation about the Z axis
      * \return The rotation about the Z axis
      */
    Type GetYaw( void ) const
    {
        return atan2( 2 * ( _v[ 0 ] * _v[ 1 ] + _v[ 3 ] * _v[ 2 ] ), _v[ 3 ] * _v[ 3 ] + _v[ 0 ] * _v[ 0 ] - _v[ 1 ] * _v[ 1 ] - _v[ 2 ] * _v[ 2 ] );
    }

    /** Returns the Euler Angles as a Vector (Warning: SLOW)
      * \return The Euler Angles as a Vector
      */
    Vector GetEulerAngles()
    {
        // This is slow
        return Vector( float( GetRoll() ), float( GetPitch() ), float( GetYaw() ) );
    }

    /** Gets the rotation angle (radians) and the axis about which the angle is rotated.
      * \param angle  Returns the angle of rotation of the quaternion
      * \param axis   Returns the axis of rotation of the quaternion
      */
    void GetAngleAxis( Type* angle, Vector* axis )
    {
// set sure we don't get a NaN result from acos...
#ifdef _WIN32
        if( abs( _v[ 3 ] ) > (Type)1.0 )
#else
        if( fabs( (double)_v[ 3 ] ) > 1.0 )
#endif
        {
            this->Normalize();
        }

        // set the angle - aCos is mathematically defined to be between 0 and PI
        Type rad = acos( _v[ 3 ] ) * (Type)2.0;

        if( angle )
        {
            *angle = rad;
        }

        // set the axis: (use sin(rad) instead of asin(w))
        if( axis )
        {
            Type sin_half_angle = sin( rad * (Type)0.5 );
            if( sin_half_angle >= (Type)0.0001 ) // because (PI >= rad >= 0)
            {
                Type sin_half_angle_inv = Type( 1.0 ) / sin_half_angle;
                axis->x                 = float( _v[ 0 ] * sin_half_angle_inv );
                axis->y                 = float( _v[ 1 ] * sin_half_angle_inv );
                axis->z                 = float( _v[ 2 ] * sin_half_angle_inv );
                axis->Normalize();
            }

            // avoid NAN
            else
            {
                // one of the terms should be a 1,
                // so we can maintain unit-ness
                // in case w is 0 (which here w is 0)
                axis->x = (float)1.0;
                axis->y = (float)0.0;
                axis->z = (float)0.0;
            }
        }
    }

    /** Returns a rotation matrix representation of the quaternion.
      * \return The rotation matrix computed from the quaternion 
      */
    GlsMatrixAffine<Type> GetRotationMatrix()
    {
        Type wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
        Type mat[ 4 ][ 4 ];

        x2 = _v[ 0 ] + _v[ 0 ];
        y2 = _v[ 1 ] + _v[ 1 ];
        z2 = _v[ 2 ] + _v[ 2 ];

        xx = _v[ 0 ] * x2;
        xy = _v[ 0 ] * y2;
        xz = _v[ 0 ] * z2;

        yy = _v[ 1 ] * y2;
        yz = _v[ 1 ] * z2;
        zz = _v[ 2 ] * z2;

        wx = _v[ 3 ] * x2;
        wy = _v[ 3 ] * y2;
        wz = _v[ 3 ] * z2;

        mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
        mat[ 0 ][ 1 ] = xy - wz;
        mat[ 0 ][ 2 ] = xz + wy;
        mat[ 0 ][ 3 ] = 0.0f;

        mat[ 1 ][ 0 ] = xy + wz;
        mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
        mat[ 1 ][ 2 ] = yz - wx;
        mat[ 1 ][ 3 ] = 0.0f;

        mat[ 2 ][ 0 ] = xz - wy;
        mat[ 2 ][ 1 ] = yz + wx;
        mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
        mat[ 2 ][ 3 ] = 0.0f;

        mat[ 3 ][ 0 ] = 0;
        mat[ 3 ][ 1 ] = 0;
        mat[ 3 ][ 2 ] = 0;
        mat[ 3 ][ 3 ] = 1;

        return GlsMatrixAffine<Type>( mat );
    }

    /** Sets this quaternion to an interpolated rotation between the
      * from and to vectors using spherical linear interpolation (slerp).
       * \param t     Percent of interpelation (0.0 -> 1.0)
       * \param from  The starting quaternion
       * \param to    The ending quaternion
       * \param adjustSign  Whether or not to negate the sign of the to quaternion
       */
    void SetFromSlerp( float t, const GlsQuaternion& from, const GlsQuaternion& to, bool adjustSign = true )
    {
        const GlsQuaternion& p = from;

        // calc cosine theta
        Type cosom = from.DotProduct( to );

        // adjust signs (if necessary)
        GlsQuaternion<Type> q;
        if( adjustSign && ( cosom < (Type)0.0 ) )
        {
            cosom     = -cosom;
            q._v[ 0 ] = -to._v[ 0 ]; // Reverse all signs
            q._v[ 1 ] = -to._v[ 1 ];
            q._v[ 2 ] = -to._v[ 2 ];
            q._v[ 3 ] = -to._v[ 3 ];
        }
        else
        {
            q = to;
        }

        // Calculate coefficients
        Type sclp, sclq;
        if( ( (Type)1.0 - cosom ) > (Type)0.0001 ) // 0.0001 -> some epsillon
        {
            // Standard case (slerp)
            Type omega, sinom;
            omega = acos( cosom ); // extract theta from dot product's cos theta
            sinom = sin( omega );
            sclp  = sin( ( (Type)1.0 - t ) * omega ) / sinom;
            sclq  = sin( t * omega ) / sinom;
        }
        else
        {
            // Very close, do linear interp (because it's faster)
            sclp = (Type)1.0 - t;
            sclq = t;
        }

        _v[ 0 ] = sclp * p._v[ 0 ] + sclq * q._v[ 0 ];
        _v[ 1 ] = sclp * p._v[ 1 ] + sclq * q._v[ 1 ];
        _v[ 2 ] = sclp * p._v[ 2 ] + sclq * q._v[ 2 ];
        _v[ 3 ] = sclp * p._v[ 3 ] + sclq * q._v[ 3 ];
    }

protected:
    Type _v[ 4 ];

    // Circular dependancies prevent us from using VeryCloseToZero in util.h
    static bool IsVeryCloseToZero( double value ) { return fabs( value ) < 0.0001; }
};

typedef GlsQuaternion<float>  GlsQuaternionF;
typedef GlsQuaternion<double> GlsQuaternionD;

template<class Type>
inline std::ostream& operator<<( std::ostream& outstr, const GlsQuaternion<Type>& quat )
{
    outstr.precision( MaxDigits10<Type>::value );

    outstr.width( 1 );
    outstr << quat.Data()[ 0 ] << " ";
    outstr.width( 8 );
    outstr << quat.Data()[ 1 ] << " ";
    outstr.width( 8 );
    outstr << quat.Data()[ 2 ] << " ";
    outstr.width( 8 );
    outstr << quat.Data()[ 3 ];

    return outstr;
}

template<class Type>
inline std::istream& operator>>( std::istream& instr, GlsQuaternion<Type>& quat )
{
    instr >> quat.Data()[ 0 ] >> quat.Data()[ 1 ] >> quat.Data()[ 2 ] >> quat.Data()[ 3 ];
    return instr;
}

} // end namespace disti

#endif