UT/UT_Quaternion.h

Go to the documentation of this file.

/*
 * PROPRIETARY INFORMATION.  This software is proprietary to
 * Side Effects Software Inc., and is not to be reproduced,
 * transmitted, or disclosed in any way without written permission.
 *
 * Produced by:
 *      Amit Dubey
 *      Side Effects
 *      477 Richmond Street West
 *      Toronto, Ontario
 *      Canada   M5V 3E7
 *      416-504-9876
 *
 * NAME:        UT library (C++)
 *
 * COMMENTS:    This class implements quaternions.
 *
 * WARNING:
 *      This class should NOT contain any virtual methods, nor should it
 *      define more member data. The size of UT_Quaternion must always be 
 *      16 bytes (4 floats).
 *
 */

#ifndef __UT_Quaternion_h__
#define __UT_Quaternion_h__

#include "UT_API.h"
#include <SYS/SYS_Math.h>
#include "UT_Vector3.h"

class UT_Matrix3;
class UT_DMatrix3;
class UT_DMatrix4;
class UT_Matrix4;
class UT_XformOrder;
class UT_IStream;

class UT_API UT_Quaternion
{
    inline friend UT_Quaternion operator*(const UT_Quaternion&,
                                          const UT_Quaternion&);
    inline friend UT_Quaternion operator+(const UT_Quaternion&,
                                          const UT_Quaternion&);
    inline friend UT_Quaternion operator-(const UT_Quaternion&,
                                          const UT_Quaternion&);
    inline friend UT_Quaternion operator-(const UT_Quaternion&);
    inline friend UT_Quaternion operator*(const UT_Quaternion&, fpreal scalar);
    inline friend UT_Quaternion operator*(fpreal scalar, const UT_Quaternion&);
    inline friend UT_Quaternion operator/(const UT_Quaternion&,
                                          const UT_Quaternion&);
    inline friend UT_Quaternion operator/(const UT_Quaternion&, fpreal scalar);

public:
                                 UT_Quaternion(fpreal qx=0, fpreal qy=0,
                                               fpreal qz=0, fpreal qw=0)
                                 {
                                     vec[0] = qx;  vec[1] = qy;
                                     vec[2] = qz;  vec[3] = qw;
                                 }
                                 UT_Quaternion( fpreal angle, 
                                                const UT_Vector3 &axis, 
                                                int donormalize=1);
                                 UT_Quaternion(const UT_Quaternion &q)
                                 {
                                     vec[0] = q.vec[0];
                                     vec[1] = q.vec[1];
                                     vec[2] = q.vec[2];
                                     vec[3] = q.vec[3];
                                 }
                                 UT_Quaternion(const UT_Vector3 &rot,
                                               const UT_XformOrder &order)
                                 {
                                     updateFromEuler(rot, order);
                                 }

    UT_Quaternion               &operator*=(const UT_Quaternion &q);
    UT_Quaternion               &operator*=(fpreal scalar);
    UT_Quaternion               &operator/=(const UT_Quaternion &quat);
    UT_Quaternion               &operator/=(fpreal scalar);
    UT_Quaternion               &operator+=(const UT_Quaternion &quat);
    bool                         operator==(const UT_Quaternion &quat) const;
    bool                         operator!=(const UT_Quaternion &quat) const;
    fpreal                       operator()(int idx) const
                                 { return vec[idx]; }
    float                       &operator()(int idx)
                                 { return vec[idx]; }
    fpreal                       operator[](int idx) const
                                 { return vec[idx]; }
    float                       &operator[](int idx)
                                 { return vec[idx]; }

    // Does a comparison with a tolerance.
    bool                         isEqual(const UT_Quaternion &quat,
                                         fpreal tol = FP32_TOLERANCE) const;

    // The rotation this quaternion represents as a matrix
    void                         getRotationMatrix(UT_Matrix3 &mat) const;
    void                         getRotationMatrix(UT_DMatrix3 &mat) const;

    void                         getTransformMatrix(UT_Matrix4 &mat) const;
    void                         getTransformMatrix(UT_DMatrix4 &mat) const;

    // Interpolates between this quat (t==0) and the target (t==1)
    UT_Quaternion                interpolate(const UT_Quaternion &target,
                                             fpreal t, fpreal b = 0.0f) const;

    void                         assign(fpreal qx, fpreal qy,
                                        fpreal qz, fpreal qw)
                                    {
                                        vec[0] = qx;  vec[1] = qy;
                                        vec[2] = qz;  vec[3] = qw;
                                    }
    void                         identity()
                                    {
                                        vec[0] = vec[1] = vec[2] = 0.0f;
                                        vec[3] = 1.0f;
                                    }
    void                         conjugate()
                                    {
                                        vec[0] = -vec[0];
                                        vec[1] = -vec[1];
                                        vec[2] = -vec[2];
                                    }
    void                         negate()
                                    {
                                        vec[0] = -vec[0];
                                        vec[1] = -vec[1];
                                        vec[2] = -vec[2];
                                        vec[3] = -vec[3];
                                    }
    fpreal                       normal() const
                                    {
                                        return  vec[0] * vec[0] +
                                                vec[1] * vec[1] +
                                                vec[2] * vec[2] +
                                                vec[3] * vec[3];
                                    }
    void                         normalize()
                                    {
                                        fpreal dn = normal();
                                        if ( dn != 1.0F )
                                        {
                                            dn = SYSsqrt(dn);
                                            *this /= dn;
                                        }
                                    }
    fpreal                       length() const
                                    {
                                        return SYSsqrt(normal());
                                    }
    void                         invert()
                                    {
                                        fpreal n = 1.0F/normal();
                                        conjugate();
                                        vec[0] *= n;
                                        vec[1] *= n;
                                        vec[2] *= n;
                                        vec[3] *= n;
                                    }

    // Form the quaternion which takes v1 and rotates it to v2. 
    // v1 and v2 are assumed normalized
    void                         updateFromVectors(const UT_Vector3 &v1,
                                                   const UT_Vector3 &v2);

    // Form the quaternion from a rotation matrix
    void                         updateFromRotationMatrix(const UT_Matrix3 &);
    void                         updateFromRotationMatrix(const UT_DMatrix3 &);

    // Form the quaternion from an angle/axis
    void                         updateFromAngleAxis(fpreal angle, 
                                                     const UT_Vector3 &axis, 
                                                     int normalize=1);

    void                         getAngleAxis(fpreal &angle, UT_Vector3 &axis) const;

    void                         updateFromLogMap(const UT_Vector3 &v);
    void                         getLogMap(UT_Vector3 &v) const;

    // Form the quaternion from euler rotation angles (given in radians)
    void                         updateFromEuler(const UT_Vector3 &rot,
                                                 const UT_XformOrder &order);

    // Given the angular velocity omega, compute our derivative into q_prime
    void                         computeDerivative(const UT_Vector3 &omega,
                                                   UT_Quaternion &q_prime);

    // Returns the angular velocity required to move from the rotation of
    // this quaternion to the destination quaternion in a given time.
    UT_Vector3                   computeAngVel(const UT_Quaternion &dest,
                                               fpreal time) const;

    // Integrates this quaternion by the given angular velocity and time
    // step.  There are two appraoches to doing this.  For small
    // angular velocity/timesteps, one can compute the derivative
    // implied by the angular velocity, apply linearly, and renormalize.
    // Alternatively, one can construct the proper quaternion for the given
    // angular velocity and rotate by that.  Which method is controlled
    // by the accurate flag.
    void                         integrate(const UT_Vector3 &angvel, 
                                           fpreal timestep,
                                           bool accurate = true);

    // Returns the rx/ry/rz eualer rotation representation of the quaternion.
    // The returned rotations are in radians.
    UT_Vector3                   computeRotations(const UT_XformOrder &) const;

    /// Rotates a vector by this quaternion
    /// Requires that this is normalized.
    UT_Vector3                   rotate(const UT_Vector3 &) const;

    /// rotates a vector by the inverse of this quaternion.
    /// Requires that this is normalized.
    UT_Vector3                   rotateInverse(const UT_Vector3 &) const;

    // Multiply this quarternion's "real world" Euler angles by the given
    // scalar s.  That is, if this quaternion is 
    // [ cos(a), n1 sin(a), n2 sin(a), n3 sin(a) ]  it is replaced by
    // [ cos(a*s), n1 sin(a*s), n2 sin(a*s), n3 sin(a*s) ] 
    void                        multAngle( fpreal s );

    float                       &x() { return vec[0]; }
    float                       &y() { return vec[1]; }
    float                       &z() { return vec[2]; }
    float                       &w() { return vec[3]; }

    fpreal                      x() const { return vec[0]; }
    fpreal                      y() const { return vec[1]; }
    fpreal                      z() const { return vec[2]; }
    fpreal                      w() const { return vec[3]; }

    void                        save(ostream &os, int binary=0) const;
    bool                        load(UT_IStream &is);

protected:
    void                         initialize(fpreal qx = 0, fpreal qy = 0, 
                                            fpreal qz = 0, fpreal qw = 0)
                                     {
                                         vec[0] = qx; vec[1] = qy; 
                                         vec[2] = qz; vec[3] = qw;
                                     }
private:
    // I/O friends:
    friend ostream      &operator<<(ostream &os, const UT_Quaternion &v)
                        {
                            v.save(os);
                            return os;
                        }
    float               vec[4];
};

inline UT_Quaternion
operator*(const UT_Quaternion &q1, const UT_Quaternion &q2)
{
    UT_Quaternion        product = q1;

    product *= q2;

    return UT_Quaternion(product);
}

inline UT_Quaternion
operator+(const UT_Quaternion &q1, const UT_Quaternion &q2)
{
    return UT_Quaternion(q1.vec[0] + q2.vec[0],
                         q1.vec[1] + q2.vec[1],
                         q1.vec[2] + q2.vec[2],
                         q1.vec[3] + q2.vec[3]);
}

inline UT_Quaternion
operator-(const UT_Quaternion &q1, const UT_Quaternion &q2)
{
    return UT_Quaternion(q1.vec[0] - q2.vec[0],
                         q1.vec[1] - q2.vec[1],
                         q1.vec[2] - q2.vec[2],
                         q1.vec[3] - q2.vec[3]);
}

inline UT_Quaternion
operator-(const UT_Quaternion &q)
{
    return UT_Quaternion(-q.vec[0],
                         -q.vec[1],
                         -q.vec[2],
                         -q.vec[3]);
}

inline UT_Quaternion
operator*(const UT_Quaternion &q, fpreal scalar)
{
    return UT_Quaternion(q.vec[0] * scalar,
                         q.vec[1] * scalar,
                         q.vec[2] * scalar,
                         q.vec[3] * scalar);
}

inline UT_Quaternion
operator*(fpreal scalar, const UT_Quaternion &q)
{
    return UT_Quaternion(q.vec[0] * scalar,
                         q.vec[1] * scalar,
                         q.vec[2] * scalar,
                         q.vec[3] * scalar);
}

inline UT_Quaternion
operator/(const UT_Quaternion &a, const UT_Quaternion &b)
{
    UT_Quaternion       a1 = a;
    UT_Quaternion       b1 = b;

    b1.invert();
    a1 *= b1;

    return UT_Quaternion(a1);
}

inline UT_Quaternion
operator/(const UT_Quaternion &q, fpreal scalar)
{
    fpreal d = 1.0F/scalar;
    return UT_Quaternion(q.vec[0]*d, q.vec[1]*d, q.vec[2]*d, q.vec[3]*d);
}

inline fpreal
dot( const UT_Quaternion &q1, const UT_Quaternion &q2 )
{
    return q1.x()*q2.x() + q1.y()*q2.y() + q1.z()*q2.z() + q1.w()*q2.w();
}

#endif

---