ik2Bsolver/AwQuaternion.cpp

//-
// ==========================================================================
// Copyright 1995,2006,2008 Autodesk, Inc. All rights reserved.
//
// Use of this software is subject to the terms of the Autodesk
// license agreement provided at the time of installation or download,
// or which otherwise accompanies this software in either electronic
// or hard copy form.
// ==========================================================================
//+
//
//  CLASS:    AwQuaternion
//
// *****************************************************************************
//
//  CLASS DESCRIPTION (AwQuaternion)
//
//  Quaternions can be used to specify orientations and rotations of 3-D
//  objects relative to a starting reference, similar to the way that cartesian
//  vectors can be used to specify positions and translations of 3-D objects
//  relative to an origin.  Quaternions represent orientations as a single
//  rotation, just as rectangular co-ordinates represent position as a single
//  vector.
//
// *****************************************************************************

#include <math.h>
#include <assert.h>
#include <maya/MIOStream.h>

#define  COMPILE_OUTSIDE_MAYA
#include <AwMath.h>
#include <AwVector.h>
#include <AwMatrix.h>
#include <AwQuaternion.h>

// Initialize constants
//
const AwQuaternion AwQuaternion::identity;

AwQuaternion::AwQuaternion(const AwVector &a, const AwVector &b)
//
//  Description:
//      This constructor creates a new quaternion that will rotate vector
//      a into vector b about their mutually perpendicular axis. (if one
//      exists)
//
: w(1.0), x(0.0), y(0.0), z(0.0)
{
    double factor = a.length() * b.length();
    
    if (fabs(factor) > kFloatEpsilon) {
        // Vectors have length > 0
        AwVector pivotVector;
        double dot = a.dotProduct(b) / factor;
        double theta = acos(clamp(dot, -1.0, 1.0));
        
        pivotVector = a^b;
        if (dot < 0.0 && pivotVector.length() < kFloatEpsilon) {
            // Vectors parallel and opposite direction, therefore a rotation
            // of 180 degrees about any vector perpendicular to this vector
            // will rotate vector a onto vector b.
            //
            // The following guarantees the dot-product will be 0.0.
            //
            size_t dominantIndex = a.dominantAxis();
            pivotVector[dominantIndex] = -a[(dominantIndex+1)%3];
            pivotVector[(dominantIndex+1)%3] = a[dominantIndex];
            pivotVector[(dominantIndex+2)%3] = 0.0;
        }       
        setAxisAngle(pivotVector, theta);
    }
}

AwQuaternion & AwQuaternion::operator=(const AwMatrix &tm)
//
//  Description:
//      Convert the given 4X4 homogeneous rotation matrix to a
//      quaternion of unit length.
//
//  Notes:
//      This methods always returns a quaternion of unit length IF it
//      is given a proper orthogonal matrix.  A proper othogonal
//      matrix is one such that the determinant of the matrix is one.
//      (If the determinant were -1, this would imply that the
//      orthogonal matrix is also producing a reflection, in addition
//      to a rotation.)
//
{
    double trace, s;
    int i, j, k;
    
    // vectors are multiplied on the left (pre-multipy).
    //

    trace = tm.matrix[0][0] + tm.matrix[1][1] + tm.matrix[2][2];
    if (trace > 0.0) {
        s = sqrt(trace + 1.0);
        w = s * 0.5;
        assert(s > kExtendedEpsilon);
        s = 0.5/s;
        x = (tm.matrix[1][2] - tm.matrix[2][1]) * s;
        y = (tm.matrix[2][0] - tm.matrix[0][2]) * s;
        z = (tm.matrix[0][1] - tm.matrix[1][0]) * s;
    } else {        
        i = 0;
        if (tm.matrix[1][1] > tm.matrix[0][0])
            i = 1;
        if (tm.matrix[2][2] > tm.matrix[i][i])
            i = 2;
        j = (i+1)%3;
        k = (j+1)%3;
        s = sqrt(tm.matrix[i][i] - (tm.matrix[j][j] + tm.matrix[k][k]) + 1.0);
        (*this)[i] = s * 0.5;
        assert(s > kExtendedEpsilon);
        s = 0.5 / s;
        w = (tm.matrix[j][k] - tm.matrix[k][j]) * s;
        (*this)[j] = (tm.matrix[j][i] + tm.matrix[i][j]) * s;
        (*this)[k] = (tm.matrix[k][i] + tm.matrix[i][k]) * s;
    }
    
    return *this;
}

AwQuaternion AwQuaternion::operator*(const AwQuaternion &rhs) const
//
//  Description:
//      This operator returns the product of two quaternions.
//      "This" quaternion is the left quaternion in the product.
//
//  Note:
//      In general, if p and q are quaternions, pq != qp, i.e.,
//      multiplication does not commute!
//
//      Quaternions in Maya multiply on the right (post-multiply)
//      the same as matrices. Many popular quaternion papers (Shoemake)
//      use pre-multiplication where quaternions pre-multiply on the left
//      so you must be aware of this when using quaternions.
//
{
    AwQuaternion result;    
    result.w = rhs.w * w - (rhs.x * x + rhs.y * y + rhs.z * z);
    result.x = rhs.w * x +  rhs.x * w + rhs.y * z - rhs.z * y;
    result.y = rhs.w * y +  rhs.y * w + rhs.z * x - rhs.x * z;
    result.z = rhs.w * z +  rhs.z * w + rhs.x * y - rhs.y * x;
    return result;
}

AwQuaternion::operator AwMatrix() const
//
//  Description: 
//      This casts a AwQuaternion to an AwMatrix by making use of the
//      convertToMatrix method.
//
{
    AwMatrix m;
    convertToMatrix(m);
    return m;
}

void AwQuaternion::convertToMatrix(AwMatrix &tm) const
//
//  Description:
//      Convert quaternion to a 4X4 homogeneous rotation matrix and pass
//      result back in 'tm'.  The construction of the matrix assumes that the
//      vectors are going to be multiplied on the left side of the matrix.
//      If the quaternion's length has degenerated, this method will still
//      produce a well behaved matrix.
//
//  Notes:
//      For an explanation of the following implementation see
//      Shoemake, K.,  "Quaternion Calculus and Fast Animation".
//
{
    // Common subexpressions
    //
    double ww = w*w, xx = x*x, yy = y*y, zz = z*z;
    double s = 2.0/(ww + xx + yy + zz);
    double xy = x*y, xz = x*z, yz = y*z, wx = w*x, wy = w*y, wz = w*z;
    
    // vectors are multiplied on the left (pre-multipy).
    //
    tm.matrix[0][0] = 1.0 - s * (yy + zz);
    tm.matrix[1][0] = s * (xy - wz);
    tm.matrix[2][0] = s * (xz + wy);
    tm.matrix[3][0] = 0.0;
    tm.matrix[0][1] = s * (xy + wz);
    tm.matrix[1][1] = 1.0 - s * (xx + zz);
    tm.matrix[2][1] = s * (yz - wx);
    tm.matrix[3][1] = 0.0;
    tm.matrix[0][2] = s * (xz - wy);
    tm.matrix[1][2] = s * (yz + wx);
    tm.matrix[2][2] = 1.0 - s * (xx + yy);
    tm.matrix[3][2] = 0.0;
    tm.matrix[0][3] = 0.0;
    tm.matrix[1][3] = 0.0;
    tm.matrix[2][3] = 0.0;
    tm.matrix[3][3] = 1.0;
}

AwQuaternion &AwQuaternion::setAxisAngle(const AwVector &axis, double theta)
//
//  Description:
//      This function sets this quaternion to be the rotation as expressed by a
//      pivot axis and a rotation theta (in radians) about that axis.  If the
//      axis is too small the quaternion returned will be the identity
//      quaternion.
//
{
    double sumOfSquares = 
        (double) axis.x * axis.x +
        (double) axis.y * axis.y +
        (double) axis.z * axis.z;
        
    if (sumOfSquares <= kDoubleEpsilon) {
        // Axis too small.
        *this = identity;
    } else {
        theta *= 0.5;
        w = cos(theta);
        double commonFactor = sin(theta);
        if (!::equivalent(sumOfSquares, 1.0)) 
            commonFactor /= sqrt(sumOfSquares);
        x = commonFactor * (double) axis.x;
        y = commonFactor * (double) axis.y;
        z = commonFactor * (double) axis.z;
    }
    return *this;
}

bool AwQuaternion::getAxisAngle(AwVector &axis, double &theta) const
//
//  Description:
//      This function converts this quaternion into a user understandable
//      representation.  That is, the quaternion is represented as a pivot
//      vector 'axis' and a rotation 'theta' (in radians) about that pivot
//      vector.
//
//  Returns:
//      true    - angle != 0
//      false   - angle == 0    (uses arbitrary axis, if given axis not valid)
//
//  Notes:
//      If the identity unit quaternion is attempted to be converted to the
//      pivot axis and angle representation it will be set to a zero degree
//      rotation about the axis that was passed in.  (Note that any axis will
//      do, since an infinity of axis with rotation of zero satisfy the identity
//      rotation.)  If the axis is zero length, then an arbitrary axis will be
//      chosen (z-axis).
//
{
    bool result;
    double inverseOfSinThetaByTwo, thetaExtended;
    
    if (::equivalent(w, (double) 1.0)) {
        theta = 0.0;
        if (axis.length() < kDoubleEpsilon) {
            //  Passed in axis invalid, choose an arbitrary axis.
            axis.set(0.0,0.0,1.0);
        }
        result = false;
    }
    else {
        thetaExtended = acos(clamp(w,-1.0,1.0));
        theta = thetaExtended * 2.0;
        
        inverseOfSinThetaByTwo = 1.0 / sin(thetaExtended);
        axis.x = x * inverseOfSinThetaByTwo;
        axis.y = y * inverseOfSinThetaByTwo;
        axis.z = z * inverseOfSinThetaByTwo;

        result = true;
    }

    return result;
}

/*friend*/ ostream &operator<<(ostream &os, const AwQuaternion &q)
//
//  Description:
//      Stream output.
//
{
    os << "[" 
       << "x: " << q.x << ", " 
       << "y: " << q.y << ", " 
       << "z: " << q.z << ", " 
       << "w: " << q.w << "]";
    
    return os;
}