path: root/indra/llmath/llquaternion.h

/**
 * @file llquaternion.h
 * @brief LLQuaternion class header file.
 *
 * $LicenseInfo:firstyear=2000&license=viewerlgpl$
 * Second Life Viewer Source Code
 * Copyright (C) 2010, Linden Research, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation;
 * version 2.1 of the License only.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 * $/LicenseInfo$
 */

#ifndef LLQUATERNION_H
#define LLQUATERNION_H

#include <iostream>
#include "llsd.h"

#ifndef LLMATH_H //enforce specific include order to avoid tangling inline dependencies
#error "Please include llmath.h first."
#endif

class LLVector4;
class LLVector3;
class LLVector3d;
class LLMatrix4;
class LLMatrix3;

//  NOTA BENE: Quaternion code is written assuming Unit Quaternions!!!!
//             Moreover, it is written assuming that all vectors and matricies
//             passed as arguments are normalized and unitary respectively.
//             VERY VERY VERY VERY BAD THINGS will happen if these assumptions fail.

static const U32 LENGTHOFQUAT = 4;

class LLQuaternion
{
public:
    F32 mQ[LENGTHOFQUAT];

    static const LLQuaternion DEFAULT;

    LLQuaternion();                                 // Initializes Quaternion to (0,0,0,1)
    explicit LLQuaternion(const LLMatrix4 &mat);                // Initializes Quaternion from Matrix4
    explicit LLQuaternion(const LLMatrix3 &mat);                // Initializes Quaternion from Matrix3
    LLQuaternion(F32 x, F32 y, F32 z, F32 w);       // Initializes Quaternion to normalize(x, y, z, w)
    LLQuaternion(F32 angle, const LLVector4 &vec);  // Initializes Quaternion to axis_angle2quat(angle, vec)
    LLQuaternion(F32 angle, const LLVector3 &vec);  // Initializes Quaternion to axis_angle2quat(angle, vec)
    LLQuaternion(const F32 *q);                     // Initializes Quaternion to normalize(x, y, z, w)
    LLQuaternion(const LLVector3 &x_axis,
                 const LLVector3 &y_axis,
                 const LLVector3 &z_axis);          // Initializes Quaternion from Matrix3 = [x_axis ; y_axis ; z_axis]
    explicit LLQuaternion(const LLSD &sd);          // Initializes Quaternion from LLSD array.

    LLSD getValue() const;
    void setValue(const LLSD& sd);

    BOOL isIdentity() const;
    BOOL isNotIdentity() const;
    BOOL isFinite() const;                                  // checks to see if all values of LLQuaternion are finite
    void quantize16(F32 lower, F32 upper);                  // changes the vector to reflect quatization
    void quantize8(F32 lower, F32 upper);                           // changes the vector to reflect quatization
    void loadIdentity();                                            // Loads the quaternion that represents the identity rotation

    bool isEqualEps(const LLQuaternion &quat, F32 epsilon) const;
    bool isNotEqualEps(const LLQuaternion &quat, F32 epsilon) const;

    const LLQuaternion& set(F32 x, F32 y, F32 z, F32 w);        // Sets Quaternion to normalize(x, y, z, w)
    const LLQuaternion& set(const LLQuaternion &quat);          // Copies Quaternion
    const LLQuaternion& set(const F32 *q);                      // Sets Quaternion to normalize(quat[VX], quat[VY], quat[VZ], quat[VW])
    const LLQuaternion& set(const LLMatrix3 &mat);              // Sets Quaternion to mat2quat(mat)
    const LLQuaternion& set(const LLMatrix4 &mat);              // Sets Quaternion to mat2quat(mat)
    const LLQuaternion& setFromAzimuthAndAltitude(F32 azimuth, F32 altitude);

    const LLQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z);   // Sets Quaternion to axis_angle2quat(angle, x, y, z)
    const LLQuaternion& setAngleAxis(F32 angle, const LLVector3 &vec);  // Sets Quaternion to axis_angle2quat(angle, vec)
    const LLQuaternion& setAngleAxis(F32 angle, const LLVector4 &vec);  // Sets Quaternion to axis_angle2quat(angle, vec)
    const LLQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw);   // Sets Quaternion to euler2quat(pitch, yaw, roll)

    const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w);    // deprecated
    const LLQuaternion& setQuat(const LLQuaternion &quat);          // deprecated
    const LLQuaternion& setQuat(const F32 *q);                      // deprecated
    const LLQuaternion& setQuat(const LLMatrix3 &mat);              // deprecated
    const LLQuaternion& setQuat(const LLMatrix4 &mat);              // deprecated
    const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z);    // deprecated
    const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec);   // deprecated
    const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec);   // deprecated
    const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw);      // deprecated

    LLMatrix4   getMatrix4(void) const;                         // Returns the Matrix4 equivalent of Quaternion
    LLMatrix3   getMatrix3(void) const;                         // Returns the Matrix3 equivalent of Quaternion
    void        getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const; // returns rotation in radians about axis x,y,z
    void        getAngleAxis(F32* angle, LLVector3 &vec) const;
    void        getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
    void        getAzimuthAndAltitude(F32 &azimuth, F32 &altitude);

    F32 normalize();    // Normalizes Quaternion and returns magnitude
    F32 normQuat();     // deprecated

    const LLQuaternion& conjugate(void);    // Conjugates Quaternion and returns result
    const LLQuaternion& conjQuat(void);     // deprecated

    // Other useful methods
    const LLQuaternion& transpose();        // transpose (same as conjugate)
    const LLQuaternion& transQuat();        // deprecated

    void            shortestArc(const LLVector3 &a, const LLVector3 &b);    // shortest rotation from a to b
    const LLQuaternion& constrain(F32 radians);                     // constrains rotation to a cone angle specified in radians

    // Standard operators
    friend std::ostream& operator<<(std::ostream &s, const LLQuaternion &a);                    // Prints a
    friend LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b);    // Addition
    friend LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b);    // Subtraction
    friend LLQuaternion operator-(const LLQuaternion &a);                           // Negation
    friend LLQuaternion operator*(F32 a, const LLQuaternion &q);                    // Scale
    friend LLQuaternion operator*(const LLQuaternion &q, F32 b);                    // Scale
    friend LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b);    // Returns a * b
    friend LLQuaternion operator~(const LLQuaternion &a);                           // Returns a* (Conjugate of a)
    bool operator==(const LLQuaternion &b) const;           // Returns a == b
    bool operator!=(const LLQuaternion &b) const;           // Returns a != b
    F64 operator[](int idx) const { return mQ[idx]; }

    friend const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b);  // Returns a * b

    friend LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot);        // Rotates a by rot
    friend LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot);        // Rotates a by rot
    friend LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot);      // Rotates a by rot

    // Non-standard operators
    friend F32 dot(const LLQuaternion &a, const LLQuaternion &b);
    friend LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);      // linear interpolation (t = 0 to 1) from p to q
    friend LLQuaternion lerp(F32 t, const LLQuaternion &q);                             // linear interpolation (t = 0 to 1) from identity to q
    friend LLQuaternion slerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);     // spherical linear interpolation from p to q
    friend LLQuaternion slerp(F32 t, const LLQuaternion &q);                            // spherical linear interpolation from identity to q
    friend LLQuaternion nlerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);     // normalized linear interpolation from p to q
    friend LLQuaternion nlerp(F32 t, const LLQuaternion &q);                            // normalized linear interpolation from p to q

    LLVector3   packToVector3() const;                      // Saves space by using the fact that our quaternions are normalized
    void        unpackFromVector3(const LLVector3& vec);    // Saves space by using the fact that our quaternions are normalized

    enum Order {
        XYZ = 0,
        YZX = 1,
        ZXY = 2,
        XZY = 3,
        YXZ = 4,
        ZYX = 5
    };
    // Creates a quaternions from maya's rotation representation,
    // which is 3 rotations (in DEGREES) in the specified order
    friend LLQuaternion mayaQ(F32 x, F32 y, F32 z, Order order);

    // Conversions between Order and strings like "xyz" or "ZYX"
    friend const char *OrderToString( const Order order );
    friend Order StringToOrder( const char *str );

    static BOOL parseQuat(const std::string& buf, LLQuaternion* value);

    // For debugging, only
    //static U32 mMultCount;
};

inline LLSD LLQuaternion::getValue() const
{
    LLSD ret;
    ret[0] = mQ[0];
    ret[1] = mQ[1];
    ret[2] = mQ[2];
    ret[3] = mQ[3];
    return ret;
}

inline void LLQuaternion::setValue(const LLSD& sd)
{
    mQ[0] = sd[0].asReal();
    mQ[1] = sd[1].asReal();
    mQ[2] = sd[2].asReal();
    mQ[3] = sd[3].asReal();
}

// checker
inline BOOL LLQuaternion::isFinite() const
{
    return (llfinite(mQ[VX]) && llfinite(mQ[VY]) && llfinite(mQ[VZ]) && llfinite(mQ[VS]));
}

inline BOOL LLQuaternion::isIdentity() const
{
    return
        ( mQ[VX] == 0.f ) &&
        ( mQ[VY] == 0.f ) &&
        ( mQ[VZ] == 0.f ) &&
        ( mQ[VS] == 1.f );
}

inline BOOL LLQuaternion::isNotIdentity() const
{
    return
        ( mQ[VX] != 0.f ) ||
        ( mQ[VY] != 0.f ) ||
        ( mQ[VZ] != 0.f ) ||
        ( mQ[VS] != 1.f );
}



inline LLQuaternion::LLQuaternion(void)
{
    mQ[VX] = 0.f;
    mQ[VY] = 0.f;
    mQ[VZ] = 0.f;
    mQ[VS] = 1.f;
}

inline LLQuaternion::LLQuaternion(F32 x, F32 y, F32 z, F32 w)
{
    mQ[VX] = x;
    mQ[VY] = y;
    mQ[VZ] = z;
    mQ[VS] = w;

    //RN: don't normalize this case as its used mainly for temporaries during calculations
    //normalize();
    /*
    F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
    mag -= 1.f;
    mag = fabs(mag);
    llassert(mag < 10.f*FP_MAG_THRESHOLD);
    */
}

inline LLQuaternion::LLQuaternion(const F32 *q)
{
    mQ[VX] = q[VX];
    mQ[VY] = q[VY];
    mQ[VZ] = q[VZ];
    mQ[VS] = q[VW];

    normalize();
    /*
    F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
    mag -= 1.f;
    mag = fabs(mag);
    llassert(mag < FP_MAG_THRESHOLD);
    */
}


inline void LLQuaternion::loadIdentity()
{
    mQ[VX] = 0.0f;
    mQ[VY] = 0.0f;
    mQ[VZ] = 0.0f;
    mQ[VW] = 1.0f;
}

inline bool LLQuaternion::isEqualEps(const LLQuaternion &quat, F32 epsilon) const
{
    return ( fabs(mQ[VX] - quat.mQ[VX]) < epsilon
        &&   fabs(mQ[VY] - quat.mQ[VY]) < epsilon
        &&   fabs(mQ[VZ] - quat.mQ[VZ]) < epsilon
        &&   fabs(mQ[VS] - quat.mQ[VS]) < epsilon );
}

inline bool LLQuaternion::isNotEqualEps(const LLQuaternion &quat, F32 epsilon) const
{
    return (  fabs(mQ[VX] - quat.mQ[VX]) > epsilon
        ||    fabs(mQ[VY] - quat.mQ[VY]) > epsilon
        ||    fabs(mQ[VZ] - quat.mQ[VZ]) > epsilon
        ||    fabs(mQ[VS] - quat.mQ[VS]) > epsilon );
}

inline const LLQuaternion&  LLQuaternion::set(F32 x, F32 y, F32 z, F32 w)
{
    mQ[VX] = x;
    mQ[VY] = y;
    mQ[VZ] = z;
    mQ[VS] = w;
    normalize();
    return (*this);
}

inline const LLQuaternion&  LLQuaternion::set(const LLQuaternion &quat)
{
    mQ[VX] = quat.mQ[VX];
    mQ[VY] = quat.mQ[VY];
    mQ[VZ] = quat.mQ[VZ];
    mQ[VW] = quat.mQ[VW];
    normalize();
    return (*this);
}

inline const LLQuaternion&  LLQuaternion::set(const F32 *q)
{
    mQ[VX] = q[VX];
    mQ[VY] = q[VY];
    mQ[VZ] = q[VZ];
    mQ[VS] = q[VW];
    normalize();
    return (*this);
}


// deprecated
inline const LLQuaternion&  LLQuaternion::setQuatInit(F32 x, F32 y, F32 z, F32 w)
{
    mQ[VX] = x;
    mQ[VY] = y;
    mQ[VZ] = z;
    mQ[VS] = w;
    normalize();
    return (*this);
}

// deprecated
inline const LLQuaternion&  LLQuaternion::setQuat(const LLQuaternion &quat)
{
    mQ[VX] = quat.mQ[VX];
    mQ[VY] = quat.mQ[VY];
    mQ[VZ] = quat.mQ[VZ];
    mQ[VW] = quat.mQ[VW];
    normalize();
    return (*this);
}

// deprecated
inline const LLQuaternion&  LLQuaternion::setQuat(const F32 *q)
{
    mQ[VX] = q[VX];
    mQ[VY] = q[VY];
    mQ[VZ] = q[VZ];
    mQ[VS] = q[VW];
    normalize();
    return (*this);
}

inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
{
    F32 v = sqrtf(mQ[VX] * mQ[VX] + mQ[VY] * mQ[VY] + mQ[VZ] * mQ[VZ]); // length of the vector-component
    if (v > FP_MAG_THRESHOLD)
    {
        F32 oomag = 1.0f / v;
        F32 w = mQ[VW];
        if (w < 0.0f)
        {
            w = -w; // make VW positive
            oomag = -oomag; // invert the axis
        }
        *x = mQ[VX] * oomag; // normalize the axis
        *y = mQ[VY] * oomag;
        *z = mQ[VZ] * oomag;
        *angle = 2.0f * atan2f(v, w); // get the angle
    }
    else
    {
        *angle = 0.0f; // no rotation
        *x = 0.0f; // around some dummy axis
        *y = 0.0f;
        *z = 1.0f;
    }
}

inline const LLQuaternion& LLQuaternion::conjugate()
{
    mQ[VX] *= -1.f;
    mQ[VY] *= -1.f;
    mQ[VZ] *= -1.f;
    return (*this);
}

inline const LLQuaternion& LLQuaternion::conjQuat()
{
    mQ[VX] *= -1.f;
    mQ[VY] *= -1.f;
    mQ[VZ] *= -1.f;
    return (*this);
}

// Transpose
inline const LLQuaternion& LLQuaternion::transpose()
{
    mQ[VX] *= -1.f;
    mQ[VY] *= -1.f;
    mQ[VZ] *= -1.f;
    return (*this);
}

// deprecated
inline const LLQuaternion& LLQuaternion::transQuat()
{
    mQ[VX] *= -1.f;
    mQ[VY] *= -1.f;
    mQ[VZ] *= -1.f;
    return (*this);
}


inline LLQuaternion     operator+(const LLQuaternion &a, const LLQuaternion &b)
{
    return LLQuaternion(
        a.mQ[VX] + b.mQ[VX],
        a.mQ[VY] + b.mQ[VY],
        a.mQ[VZ] + b.mQ[VZ],
        a.mQ[VW] + b.mQ[VW] );
}


inline LLQuaternion     operator-(const LLQuaternion &a, const LLQuaternion &b)
{
    return LLQuaternion(
        a.mQ[VX] - b.mQ[VX],
        a.mQ[VY] - b.mQ[VY],
        a.mQ[VZ] - b.mQ[VZ],
        a.mQ[VW] - b.mQ[VW] );
}


inline LLQuaternion     operator-(const LLQuaternion &a)
{
    return LLQuaternion(
        -a.mQ[VX],
        -a.mQ[VY],
        -a.mQ[VZ],
        -a.mQ[VW] );
}


inline LLQuaternion     operator*(F32 a, const LLQuaternion &q)
{
    return LLQuaternion(
        a * q.mQ[VX],
        a * q.mQ[VY],
        a * q.mQ[VZ],
        a * q.mQ[VW] );
}


inline LLQuaternion     operator*(const LLQuaternion &q, F32 a)
{
    return LLQuaternion(
        a * q.mQ[VX],
        a * q.mQ[VY],
        a * q.mQ[VZ],
        a * q.mQ[VW] );
}

inline LLQuaternion operator~(const LLQuaternion &a)
{
    LLQuaternion q(a);
    q.conjQuat();
    return q;
}

inline bool LLQuaternion::operator==(const LLQuaternion &b) const
{
    return (  (mQ[VX] == b.mQ[VX])
            &&(mQ[VY] == b.mQ[VY])
            &&(mQ[VZ] == b.mQ[VZ])
            &&(mQ[VS] == b.mQ[VS]));
}

inline bool LLQuaternion::operator!=(const LLQuaternion &b) const
{
    return (  (mQ[VX] != b.mQ[VX])
            ||(mQ[VY] != b.mQ[VY])
            ||(mQ[VZ] != b.mQ[VZ])
            ||(mQ[VS] != b.mQ[VS]));
}

inline const LLQuaternion&  operator*=(LLQuaternion &a, const LLQuaternion &b)
{
#if 1
    LLQuaternion q(
        b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
        b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
        b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
        b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
    );
    a = q;
#else
    a = a * b;
#endif
    return a;
}

const F32 ONE_PART_IN_A_MILLION = 0.000001f;

inline F32  LLQuaternion::normalize()
{
    F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);

    if (mag > FP_MAG_THRESHOLD)
    {
        // Floating point error can prevent some quaternions from achieving
        // exact unity length.  When trying to renormalize such quaternions we
        // can oscillate between multiple quantized states.  To prevent such
        // drifts we only renomalize if the length is far enough from unity.
        if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
        {
            F32 oomag = 1.f/mag;
            mQ[VX] *= oomag;
            mQ[VY] *= oomag;
            mQ[VZ] *= oomag;
            mQ[VS] *= oomag;
        }
    }
    else
    {
        // we were given a very bad quaternion so we set it to identity
        mQ[VX] = 0.f;
        mQ[VY] = 0.f;
        mQ[VZ] = 0.f;
        mQ[VS] = 1.f;
    }

    return mag;
}

// deprecated
inline F32  LLQuaternion::normQuat()
{
    F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);

    if (mag > FP_MAG_THRESHOLD)
    {
        if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
        {
            // only renormalize if length not close enough to 1.0 already
            F32 oomag = 1.f/mag;
            mQ[VX] *= oomag;
            mQ[VY] *= oomag;
            mQ[VZ] *= oomag;
            mQ[VS] *= oomag;
        }
    }
    else
    {
        mQ[VX] = 0.f;
        mQ[VY] = 0.f;
        mQ[VZ] = 0.f;
        mQ[VS] = 1.f;
    }

    return mag;
}

LLQuaternion::Order StringToOrder( const char *str );

// Some notes about Quaternions

// What is a Quaternion?
// ---------------------
// A quaternion is a point in 4-dimensional complex space.
// Q = { Qx, Qy, Qz, Qw }
//
//
// Why Quaternions?
// ----------------
// The set of quaternions that make up the the 4-D unit sphere
// can be mapped to the set of all rotations in 3-D space.  Sometimes
// it is easier to describe/manipulate rotations in quaternion space
// than rotation-matrix space.
//
//
// How Quaternions?
// ----------------
// In order to take advantage of quaternions we need to know how to
// go from rotation-matricies to quaternions and back.  We also have
// to agree what variety of rotations we're generating.
//
// Consider the equation...   v' = v * R
//
// There are two ways to think about rotations of vectors.
// 1) v' is the same vector in a different reference frame
// 2) v' is a new vector in the same reference frame
//
// bookmark -- which way are we using?
//
//
// Quaternion from Angle-Axis:
// ---------------------------
// Suppose we wanted to represent a rotation of some angle (theta)
// about some axis ({Ax, Ay, Az})...
//
// axis of rotation = {Ax, Ay, Az}
// angle_of_rotation = theta
//
// s = sin(0.5 * theta)
// c = cos(0.5 * theta)
// Q = { s * Ax, s * Ay, s * Az, c }
//
//
// 3x3 Matrix from Quaternion
// --------------------------
//
//     |                                                                    |
//     | 1 - 2 * (y^2 + z^2)   2 * (x * y + z * w)     2 * (y * w - x * z)  |
//     |                                                                    |
// M = | 2 * (x * y - z * w)   1 - 2 * (x^2 + z^2)     2 * (y * z + x * w)  |
//     |                                                                    |
//     | 2 * (x * z + y * w)   2 * (y * z - x * w)     1 - 2 * (x^2 + y^2)  |
//     |                                                                    |

#endif