initial.cpp.svn-base

絲路server源碼 Silk Road server source

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

namespace G3D {

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

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

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

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

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

bool Initil::fuzzyEq(const Initil& 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 Initil::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));
}

//----------------------------------------------------------------------------
Initil::Initil(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);
}

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

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

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

//----------------------------------------------------------------------------
Initil::Initil(
          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 Initil::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 Initil::getColumn (int iCol) const {
    assert((0 <= iCol) && (iCol < 3));
    return Vector3(elt[0][iCol], elt[1][iCol],
                   elt[2][iCol]);
}

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

void Initil::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 Initil::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 Initil::operator== (const Initil& 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 Initil::operator!= (const Initil& rkMatrix) const {
    return !operator==(rkMatrix);
}

//----------------------------------------------------------------------------
Initil Initil::operator+ (const Initil& rkMatrix) const {
    Initil 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;
}

//----------------------------------------------------------------------------
Initil Initil::operator- (const Initil& rkMatrix) const {
    Initil 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;
}

//----------------------------------------------------------------------------
Initil Initil::operator* (const Initil& rkMatrix) const {
    Initil 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;
}

Initil& Initil::operator+= (const Initil& 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;
}

Initil& Initil::operator-= (const Initil& 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;
}

Initil& Initil::operator*= (const Initil& rkMatrix) {
    Initil 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;
}

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

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

    return kNeg;
}

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

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

    return kProd;
}

//----------------------------------------------------------------------------
Initil operator* (double fScalar, const Initil& rkMatrix) {
    Initil 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;
}

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


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

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

    return kTranspose;
}

//----------------------------------------------------------------------------
bool Initil::inverse (Initil& 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;
}

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

//----------------------------------------------------------------------------
float Initil::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 Initil::bidiagonalize (Initil& kA, Initil& kL,
                             Initil& 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 = Initil::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);
        fT1 = kA[0][1] + fSign * fLength;
        afV[2] = kA[0][2] / fT1;

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