matrix3.cpp

WOW 服务模拟端 支持2.4.3版本 来自开源的ASCENT 自己REPACK

/**
 @file Matrix3.cpp

 3x3 matrix class

 @author Morgan McGuire, graphics3d.com

 @created 2001-06-02
 @edited  2006-04-06
*/

#include "G3D/platform.h"
#include "G3D/format.h"
#include <memory.h>
#include <assert.h>
#include "G3D/Matrix3.h"
#include "G3D/g3dmath.h"
#include "G3D/Quat.h"

namespace G3D {

const float Matrix3::EPSILON = 1e-06f;

const Matrix3& Matrix3::zero() {
    static Matrix3 m(0, 0, 0, 0, 0, 0, 0, 0, 0);
    return m;
}

const Matrix3& Matrix3::identity() {
    static Matrix3 m(1, 0, 0, 0, 1, 0, 0, 0, 1);
    return m;
}

// Deprecated.
const Matrix3 Matrix3::ZERO(0, 0, 0, 0, 0, 0, 0, 0, 0);
const Matrix3 Matrix3::IDENTITY(1, 0, 0, 0, 1, 0, 0, 0, 1);

const float Matrix3::ms_fSvdEpsilon = 1e-04f;
const int Matrix3::ms_iSvdMaxIterations = 32;

bool Matrix3::fuzzyEq(const Matrix3& b) const {
    for (int r = 0; r < 3; ++r) {
        for (int c = 0; c < 3; ++c) {
            if (! G3D::fuzzyEq(elt[r][c], b[r][c])) {
                return false;
            }
        }
    }
    return true;
}


bool Matrix3::isOrthonormal() const {
    Vector3 X = getColumn(0);
    Vector3 Y = getColumn(1);
    Vector3 Z = getColumn(2);

    return 
        (G3D::fuzzyEq(X.dot(Y), 0.0f) &&
         G3D::fuzzyEq(Y.dot(Z), 0.0f) &&
         G3D::fuzzyEq(X.dot(Z), 0.0f) &&
         G3D::fuzzyEq(X.squaredMagnitude(), 1.0f) &&
         G3D::fuzzyEq(Y.squaredMagnitude(), 1.0f) &&
         G3D::fuzzyEq(Z.squaredMagnitude(), 1.0f));
}

//----------------------------------------------------------------------------
Matrix3::Matrix3(const Quat& _q) {
    // Implementation from Watt and Watt, pg 362
	// See also http://www.flipcode.com/documents/matrfaq.html#Q54
    Quat q = _q.unitize();
    float xx = 2.0f * q.x * q.x;
    float xy = 2.0f * q.x * q.y;
    float xz = 2.0f * q.x * q.z;
    float xw = 2.0f * q.x * q.w;

    float yy = 2.0f * q.y * q.y;
    float yz = 2.0f * q.y * q.z;
    float yw = 2.0f * q.y * q.w;

    float zz = 2.0f * q.z * q.z;
    float zw = 2.0f * q.z * q.w;

    set(1.0f - yy - zz,    xy - zw,        xz + yw,
        xy + zw,          1.0f - xx - zz,  yz - xw,
        xz - yw,          yz + xw,        1.0f - xx - yy);
}

//----------------------------------------------------------------------------

Matrix3::Matrix3 (const float aafEntry[3][3]) {
    memcpy(elt, aafEntry, 9*sizeof(float));
}

//----------------------------------------------------------------------------
Matrix3::Matrix3 (const Matrix3& rkMatrix) {
    memcpy(elt, rkMatrix.elt, 9*sizeof(float));
}

//----------------------------------------------------------------------------
Matrix3::Matrix3(
          float fEntry00, float fEntry01, float fEntry02,
          float fEntry10, float fEntry11, float fEntry12,
          float fEntry20, float fEntry21, float fEntry22) {
    set(fEntry00, fEntry01, fEntry02,
        fEntry10, fEntry11, fEntry12,
        fEntry20, fEntry21, fEntry22);
}

void Matrix3::set(
          float fEntry00, float fEntry01, float fEntry02,
          float fEntry10, float fEntry11, float fEntry12, 
          float fEntry20, float fEntry21, float fEntry22) {

    elt[0][0] = fEntry00;
    elt[0][1] = fEntry01;
    elt[0][2] = fEntry02;
    elt[1][0] = fEntry10;
    elt[1][1] = fEntry11;
    elt[1][2] = fEntry12;
    elt[2][0] = fEntry20;
    elt[2][1] = fEntry21;
    elt[2][2] = fEntry22;
}


//----------------------------------------------------------------------------
Vector3 Matrix3::getColumn (int iCol) const {
    assert((0 <= iCol) && (iCol < 3));
    return Vector3(elt[0][iCol], elt[1][iCol],
                   elt[2][iCol]);
}

Vector3 Matrix3::getRow (int iRow) const {
    return Vector3(elt[iRow][0], elt[iRow][1], elt[iRow][2]);
}

void Matrix3::setColumn(int iCol, const Vector3 &vector) {
    debugAssert((iCol >= 0) && (iCol < 3));
    elt[0][iCol] = vector.x;
    elt[1][iCol] = vector.y;
    elt[2][iCol] = vector.z;
}


void Matrix3::setRow(int iRow, const Vector3 &vector) {
    debugAssert((iRow >= 0) && (iRow < 3));
    elt[iRow][0] = vector.x;
    elt[iRow][1] = vector.y;
    elt[iRow][2] = vector.z;
}


//----------------------------------------------------------------------------
bool Matrix3::operator== (const Matrix3& rkMatrix) const {
    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            if ( elt[iRow][iCol] != rkMatrix.elt[iRow][iCol] )
                return false;
        }
    }

    return true;
}

//----------------------------------------------------------------------------
bool Matrix3::operator!= (const Matrix3& rkMatrix) const {
    return !operator==(rkMatrix);
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const {
    Matrix3 kSum;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kSum.elt[iRow][iCol] = elt[iRow][iCol] +
                                          rkMatrix.elt[iRow][iCol];
        }
    }

    return kSum;
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const {
    Matrix3 kDiff;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kDiff.elt[iRow][iCol] = elt[iRow][iCol] -
                                           rkMatrix.elt[iRow][iCol];
        }
    }

    return kDiff;
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const {
    Matrix3 kProd;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kProd.elt[iRow][iCol] =
                elt[iRow][0] * rkMatrix.elt[0][iCol] +
                elt[iRow][1] * rkMatrix.elt[1][iCol] +
                elt[iRow][2] * rkMatrix.elt[2][iCol];
        }
    }

    return kProd;
}

Matrix3& Matrix3::operator+= (const Matrix3& rkMatrix) {
    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            elt[iRow][iCol] = elt[iRow][iCol] + rkMatrix.elt[iRow][iCol];
        }
    }

    return *this;
}

Matrix3& Matrix3::operator-= (const Matrix3& rkMatrix) {
    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            elt[iRow][iCol] = elt[iRow][iCol] - rkMatrix.elt[iRow][iCol];
        }
    }

    return *this;
}

Matrix3& Matrix3::operator*= (const Matrix3& rkMatrix) {
    Matrix3 mulMat;
    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            mulMat.elt[iRow][iCol] =
                elt[iRow][0] * rkMatrix.elt[0][iCol] +
                elt[iRow][1] * rkMatrix.elt[1][iCol] +
                elt[iRow][2] * rkMatrix.elt[2][iCol];
        }
    }

    *this = mulMat;
    return *this;
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::operator- () const {
    Matrix3 kNeg;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kNeg[iRow][iCol] = -elt[iRow][iCol];
        }
    }

    return kNeg;
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::operator* (float fScalar) const {
    Matrix3 kProd;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kProd[iRow][iCol] = fScalar * elt[iRow][iCol];
        }
    }

    return kProd;
}

//----------------------------------------------------------------------------
Matrix3 operator* (double fScalar, const Matrix3& rkMatrix) {
    Matrix3 kProd;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kProd[iRow][iCol] = fScalar * rkMatrix.elt[iRow][iCol];
        }
    }

    return kProd;
}

Matrix3 operator* (float fScalar, const Matrix3& rkMatrix) {
    return (double)fScalar * rkMatrix;
}


Matrix3 operator* (int fScalar, const Matrix3& rkMatrix) {
    return (double)fScalar * rkMatrix;
}
//----------------------------------------------------------------------------
Matrix3 Matrix3::transpose () const {
    Matrix3 kTranspose;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            kTranspose[iRow][iCol] = elt[iCol][iRow];
        }
    }

    return kTranspose;
}

//----------------------------------------------------------------------------
bool Matrix3::inverse (Matrix3& rkInverse, float fTolerance) const {
    // Invert a 3x3 using cofactors.  This is about 8 times faster than
    // the Numerical Recipes code which uses Gaussian elimination.

    rkInverse[0][0] = elt[1][1] * elt[2][2] -
                      elt[1][2] * elt[2][1];
    rkInverse[0][1] = elt[0][2] * elt[2][1] -
                      elt[0][1] * elt[2][2];
    rkInverse[0][2] = elt[0][1] * elt[1][2] -
                      elt[0][2] * elt[1][1];
    rkInverse[1][0] = elt[1][2] * elt[2][0] -
                      elt[1][0] * elt[2][2];
    rkInverse[1][1] = elt[0][0] * elt[2][2] -
                      elt[0][2] * elt[2][0];
    rkInverse[1][2] = elt[0][2] * elt[1][0] -
                      elt[0][0] * elt[1][2];
    rkInverse[2][0] = elt[1][0] * elt[2][1] -
                      elt[1][1] * elt[2][0];
    rkInverse[2][1] = elt[0][1] * elt[2][0] -
                      elt[0][0] * elt[2][1];
    rkInverse[2][2] = elt[0][0] * elt[1][1] -
                      elt[0][1] * elt[1][0];

    float fDet =
        elt[0][0] * rkInverse[0][0] +
        elt[0][1] * rkInverse[1][0] +
        elt[0][2] * rkInverse[2][0];

    if ( G3D::abs(fDet) <= fTolerance )
        return false;

    float fInvDet = 1.0 / fDet;

    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++)
            rkInverse[iRow][iCol] *= fInvDet;
    }

    return true;
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::inverse (float fTolerance) const {
    Matrix3 kInverse = Matrix3::zero();
    inverse(kInverse, fTolerance);
    return kInverse;
}

//----------------------------------------------------------------------------
float Matrix3::determinant () const {
    float fCofactor00 = elt[1][1] * elt[2][2] -
                       elt[1][2] * elt[2][1];
    float fCofactor10 = elt[1][2] * elt[2][0] -
                       elt[1][0] * elt[2][2];
    float fCofactor20 = elt[1][0] * elt[2][1] -
                       elt[1][1] * elt[2][0];

    float fDet =
        elt[0][0] * fCofactor00 +
        elt[0][1] * fCofactor10 +
        elt[0][2] * fCofactor20;

    return fDet;
}

//----------------------------------------------------------------------------
void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL,
                             Matrix3& kR) {
    float afV[3], afW[3];
    float fLength, fSign, fT1, fInvT1, fT2;
    bool bIdentity;

    // map first column to (*,0,0)
    fLength = sqrt(kA[0][0] * kA[0][0] + kA[1][0] * kA[1][0] +
                         kA[2][0] * kA[2][0]);

    if ( fLength > 0.0 ) {
        fSign = (kA[0][0] > 0.0 ? 1.0 : -1.0);
        fT1 = kA[0][0] + fSign * fLength;
        fInvT1 = 1.0 / fT1;
        afV[1] = kA[1][0] * fInvT1;
        afV[2] = kA[2][0] * fInvT1;

        fT2 = -2.0 / (1.0 + afV[1] * afV[1] + afV[2] * afV[2]);
        afW[0] = fT2 * (kA[0][0] + kA[1][0] * afV[1] + kA[2][0] * afV[2]);
        afW[1] = fT2 * (kA[0][1] + kA[1][1] * afV[1] + kA[2][1] * afV[2]);
        afW[2] = fT2 * (kA[0][2] + kA[1][2] * afV[1] + kA[2][2] * afV[2]);
        kA[0][0] += afW[0];
        kA[0][1] += afW[1];
        kA[0][2] += afW[2];
        kA[1][1] += afV[1] * afW[1];
        kA[1][2] += afV[1] * afW[2];
        kA[2][1] += afV[2] * afW[1];
        kA[2][2] += afV[2] * afW[2];

        kL[0][0] = 1.0 + fT2;
        kL[0][1] = kL[1][0] = fT2 * afV[1];
        kL[0][2] = kL[2][0] = fT2 * afV[2];
        kL[1][1] = 1.0 + fT2 * afV[1] * afV[1];
        kL[1][2] = kL[2][1] = fT2 * afV[1] * afV[2];
        kL[2][2] = 1.0 + fT2 * afV[2] * afV[2];
        bIdentity = false;
    } else {
        kL = Matrix3::identity();
        bIdentity = true;
    }

    // map first row to (*,*,0)
    fLength = sqrt(kA[0][1] * kA[0][1] + kA[0][2] * kA[0][2]);

    if ( fLength > 0.0 ) {
        fSign = (kA[0][1] > 0.0 ? 1.0 : -1.0);