matrix3.cpp

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

        return false;
    }
}

//----------------------------------------------------------------------------
bool Matrix3::toEulerAnglesZYX (float& rfZAngle, float& rfYAngle,
                                float& rfXAngle) const {
    // rot =  cy*cz           cz*sx*sy-cx*sz  cx*cz*sy+sx*sz
    //        cy*sz           cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
    //       -sy              cy*sx           cx*cy

    if ( elt[2][0] < 1.0 ) {
        if ( elt[2][0] > -1.0 ) {
            rfZAngle = atan2f(elt[1][0], elt[0][0]);
            rfYAngle = asinf(-(double)elt[2][1]);
            rfXAngle = atan2f(elt[2][1], elt[2][2]);
            return true;
        } else {
            // WARNING.  Not unique.  ZA - XA = -atan2(r01,r02)
            rfZAngle = -G3D::aTan2(elt[0][1], elt[0][2]);
            rfYAngle = (float)G3D_HALF_PI;
            rfXAngle = 0.0f;
            return false;
        }
    } else {
        // WARNING.  Not unique.  ZA + XA = atan2(-r01,-r02)
        rfZAngle = G3D::aTan2( -elt[0][1], -elt[0][2]);
        rfYAngle = -(float)G3D_HALF_PI;
        rfXAngle = 0.0f;
        return false;
    }
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromEulerAnglesXYZ (float fYAngle, float fPAngle,
                                  float fRAngle) {
    float fCos, fSin;

    fCos = cosf(fYAngle);
    fSin = sinf(fYAngle);
    Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0, fSin, fCos);

    fCos = cosf(fPAngle);
    fSin = sinf(fPAngle);
    Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);

    fCos = cosf(fRAngle);
    fSin = sinf(fRAngle);
    Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);

    return kXMat * (kYMat * kZMat);
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromEulerAnglesXZY (float fYAngle, float fPAngle,
                                  float fRAngle) {

    float fCos, fSin;

    fCos = cosf(fYAngle);
    fSin = sinf(fYAngle);
    Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);

    fCos = cosf(fPAngle);
    fSin = sinf(fPAngle);
    Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);

    fCos = cosf(fRAngle);
    fSin = sinf(fRAngle);
    Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);

    return kXMat * (kZMat * kYMat);
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromEulerAnglesYXZ(
    float fYAngle, 
    float fPAngle,
    float fRAngle) {
    
    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);

    return kYMat * (kXMat * kZMat);
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromEulerAnglesYZX(
    float fYAngle, 
    float fPAngle,
    float fRAngle) {

    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);

    return kYMat * (kZMat * kXMat);
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromEulerAnglesZXY (float fYAngle, float fPAngle,
                                  float fRAngle) {
    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);

    return kZMat * (kXMat * kYMat);
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromEulerAnglesZYX (float fYAngle, float fPAngle,
                                  float fRAngle) {
    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);

    return kZMat * (kYMat * kXMat);
}

//----------------------------------------------------------------------------
void Matrix3::tridiagonal (float afDiag[3], float afSubDiag[3]) {
    // Householder reduction T = Q^t M Q
    //   Input:
    //     mat, symmetric 3x3 matrix M
    //   Output:
    //     mat, orthogonal matrix Q
    //     diag, diagonal entries of T
    //     subd, subdiagonal entries of T (T is symmetric)

    float fA = elt[0][0];
    float fB = elt[0][1];
    float fC = elt[0][2];
    float fD = elt[1][1];
    float fE = elt[1][2];
    float fF = elt[2][2];

    afDiag[0] = fA;
    afSubDiag[2] = 0.0;

    if ( G3D::abs(fC) >= EPSILON ) {
        float fLength = sqrt(fB * fB + fC * fC);
        float fInvLength = 1.0 / fLength;
        fB *= fInvLength;
        fC *= fInvLength;
        float fQ = 2.0 * fB * fE + fC * (fF - fD);
        afDiag[1] = fD + fC * fQ;
        afDiag[2] = fF - fC * fQ;
        afSubDiag[0] = fLength;
        afSubDiag[1] = fE - fB * fQ;
        elt[0][0] = 1.0;
        elt[0][1] = 0.0;
        elt[0][2] = 0.0;
        elt[1][0] = 0.0;
        elt[1][1] = fB;
        elt[1][2] = fC;
        elt[2][0] = 0.0;
        elt[2][1] = fC;
        elt[2][2] = -fB;
    } else {
        afDiag[1] = fD;
        afDiag[2] = fF;
        afSubDiag[0] = fB;
        afSubDiag[1] = fE;
        elt[0][0] = 1.0;
        elt[0][1] = 0.0;
        elt[0][2] = 0.0;
        elt[1][0] = 0.0;
        elt[1][1] = 1.0;
        elt[1][2] = 0.0;
        elt[2][0] = 0.0;
        elt[2][1] = 0.0;
        elt[2][2] = 1.0;
    }
}

//----------------------------------------------------------------------------
bool Matrix3::qLAlgorithm (float afDiag[3], float afSubDiag[3]) {
    // QL iteration with implicit shifting to reduce matrix from tridiagonal
    // to diagonal

    for (int i0 = 0; i0 < 3; i0++) {
        const int iMaxIter = 32;
        int iIter;

        for (iIter = 0; iIter < iMaxIter; iIter++) {
            int i1;

            for (i1 = i0; i1 <= 1; i1++) {
                float fSum = G3D::abs(afDiag[i1]) +
                            G3D::abs(afDiag[i1 + 1]);

                if ( G3D::abs(afSubDiag[i1]) + fSum == fSum )
                    break;
            }

            if ( i1 == i0 )
                break;

            float fTmp0 = (afDiag[i0 + 1] - afDiag[i0]) / (2.0 * afSubDiag[i0]);

            float fTmp1 = sqrt(fTmp0 * fTmp0 + 1.0);

            if ( fTmp0 < 0.0 )
                fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 - fTmp1);
            else
                fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 + fTmp1);

            float fSin = 1.0;

            float fCos = 1.0;

            float fTmp2 = 0.0;

            for (int i2 = i1 - 1; i2 >= i0; i2--) {
                float fTmp3 = fSin * afSubDiag[i2];
                float fTmp4 = fCos * afSubDiag[i2];

                if (G3D::abs(fTmp3) >= G3D::abs(fTmp0)) {
                    fCos = fTmp0 / fTmp3;
                    fTmp1 = sqrt(fCos * fCos + 1.0);
                    afSubDiag[i2 + 1] = fTmp3 * fTmp1;
                    fSin = 1.0 / fTmp1;
                    fCos *= fSin;
                } else {
                    fSin = fTmp3 / fTmp0;
                    fTmp1 = sqrt(fSin * fSin + 1.0);
                    afSubDiag[i2 + 1] = fTmp0 * fTmp1;
                    fCos = 1.0 / fTmp1;
                    fSin *= fCos;
                }

                fTmp0 = afDiag[i2 + 1] - fTmp2;
                fTmp1 = (afDiag[i2] - fTmp0) * fSin + 2.0 * fTmp4 * fCos;
                fTmp2 = fSin * fTmp1;
                afDiag[i2 + 1] = fTmp0 + fTmp2;
                fTmp0 = fCos * fTmp1 - fTmp4;

                for (int iRow = 0; iRow < 3; iRow++) {
                    fTmp3 = elt[iRow][i2 + 1];
                    elt[iRow][i2 + 1] = fSin * elt[iRow][i2] +
                                               fCos * fTmp3;
                    elt[iRow][i2] = fCos * elt[iRow][i2] -
                                           fSin * fTmp3;
                }
            }

            afDiag[i0] -= fTmp2;
            afSubDiag[i0] = fTmp0;
            afSubDiag[i1] = 0.0;
        }

        if ( iIter == iMaxIter ) {
            // should not get here under normal circumstances
            return false;
        }
    }

    return true;
}

//----------------------------------------------------------------------------
void Matrix3::eigenSolveSymmetric (float afEigenvalue[3],
                                   Vector3 akEigenvector[3]) const {
    Matrix3 kMatrix = *this;
    float afSubDiag[3];
    kMatrix.tridiagonal(afEigenvalue, afSubDiag);
    kMatrix.qLAlgorithm(afEigenvalue, afSubDiag);

    for (int i = 0; i < 3; i++) {
        akEigenvector[i][0] = kMatrix[0][i];
        akEigenvector[i][1] = kMatrix[1][i];
        akEigenvector[i][2] = kMatrix[2][i];
    }

    // make eigenvectors form a right--handed system
    Vector3 kCross = akEigenvector[1].cross(akEigenvector[2]);

    float fDet = akEigenvector[0].dot(kCross);

    if ( fDet < 0.0 ) {
        akEigenvector[2][0] = - akEigenvector[2][0];
        akEigenvector[2][1] = - akEigenvector[2][1];
        akEigenvector[2][2] = - akEigenvector[2][2];
    }
}

//----------------------------------------------------------------------------
void Matrix3::tensorProduct (const Vector3& rkU, const Vector3& rkV,
                             Matrix3& rkProduct) {
    for (int iRow = 0; iRow < 3; iRow++) {
        for (int iCol = 0; iCol < 3; iCol++) {
            rkProduct[iRow][iCol] = rkU[iRow] * rkV[iCol];
        }
    }
}

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

// Runs in 52 cycles on AMD, 76 cycles on Intel Centrino
//
// The loop unrolling is necessary for performance. 
// I was unable to improve performance further by flattening the matrices
// into float*'s instead of 2D arrays.  
//
// -morgan
void Matrix3::_mul(const Matrix3& A, const Matrix3& B, Matrix3& out) {
    const float* ARowPtr = A.elt[0];
    float* outRowPtr     = out.elt[0];
        outRowPtr[0] =
            ARowPtr[0] * B.elt[0][0] +
            ARowPtr[1] * B.elt[1][0] +
            ARowPtr[2] * B.elt[2][0];
        outRowPtr[1] =
            ARowPtr[0] * B.elt[0][1] +
            ARowPtr[1] * B.elt[1][1] +
            ARowPtr[2] * B.elt[2][1];
        outRowPtr[2] =
            ARowPtr[0] * B.elt[0][2] +
            ARowPtr[1] * B.elt[1][2] +
            ARowPtr[2] * B.elt[2][2];

    ARowPtr       = A.elt[1];
    outRowPtr     = out.elt[1];

        outRowPtr[0] =
            ARowPtr[0] * B.elt[0][0] +
            ARowPtr[1] * B.elt[1][0] +
            ARowPtr[2] * B.elt[2][0];
        outRowPtr[1] =
            ARowPtr[0] * B.elt[0][1] +
            ARowPtr[1] * B.elt[1][1] +
            ARowPtr[2] * B.elt[2][1];
        outRowPtr[2] =
            ARowPtr[0] * B.elt[0][2] +
            ARowPtr[1] * B.elt[1][2] +
            ARowPtr[2] * B.elt[2][2];

    ARowPtr       = A.elt[2];
    outRowPtr     = out.elt[2];

        outRowPtr[0] =
            ARowPtr[0] * B.elt[0][0] +
            ARowPtr[1] * B.elt[1][0] +
            ARowPtr[2] * B.elt[2][0];
        outRowPtr[1] =
            ARowPtr[0] * B.elt[0][1] +
            ARowPtr[1] * B.elt[1][1] +
            ARowPtr[2] * B.elt[2][1];
        outRowPtr[2] =
            ARowPtr[0] * B.elt[0][2] +
            ARowPtr[1] * B.elt[1][2] +
            ARowPtr[2] * B.elt[2][2];
}

//----------------------------------------------------------------------------
void Matrix3::_transpose(const Matrix3& A, Matrix3& out) {
    out[0][0] = A.elt[0][0];
    out[0][1] = A.elt[1][0];
    out[0][2] = A.elt[2][0];
    out[1][0] = A.elt[0][1];
    out[1][1] = A.elt[1][1];
    out[1][2] = A.elt[2][1];
    out[2][0] = A.elt[0][2];
    out[2][1] = A.elt[1][2];
    out[2][2] = A.elt[2][2];
}

//-----------------------------------------------------------------------------
std::string Matrix3::toString() const {
    return G3D::format("[%g, %g, %g; %g, %g, %g; %g, %g, %g]", 
			elt[0][0], elt[0][1], elt[0][2],
			elt[1][0], elt[1][1], elt[1][2],
			elt[2][0], elt[2][1], elt[2][2]);
}



} // namespace