matrix3.cpp

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

    float fDot = kQ[0][0] * elt[0][1] + kQ[1][0] * elt[1][1] +
                kQ[2][0] * elt[2][1];
    kQ[0][1] = elt[0][1] - fDot * kQ[0][0];
    kQ[1][1] = elt[1][1] - fDot * kQ[1][0];
    kQ[2][1] = elt[2][1] - fDot * kQ[2][0];
    fInvLength = 1.0 / sqrt(kQ[0][1] * kQ[0][1] + kQ[1][1] * kQ[1][1] +
                                  kQ[2][1] * kQ[2][1]);
    kQ[0][1] *= fInvLength;
    kQ[1][1] *= fInvLength;
    kQ[2][1] *= fInvLength;

    fDot = kQ[0][0] * elt[0][2] + kQ[1][0] * elt[1][2] +
           kQ[2][0] * elt[2][2];
    kQ[0][2] = elt[0][2] - fDot * kQ[0][0];
    kQ[1][2] = elt[1][2] - fDot * kQ[1][0];
    kQ[2][2] = elt[2][2] - fDot * kQ[2][0];
    fDot = kQ[0][1] * elt[0][2] + kQ[1][1] * elt[1][2] +
           kQ[2][1] * elt[2][2];
    kQ[0][2] -= fDot * kQ[0][1];
    kQ[1][2] -= fDot * kQ[1][1];
    kQ[2][2] -= fDot * kQ[2][1];
    fInvLength = 1.0 / sqrt(kQ[0][2] * kQ[0][2] + kQ[1][2] * kQ[1][2] +
                                  kQ[2][2] * kQ[2][2]);
    kQ[0][2] *= fInvLength;
    kQ[1][2] *= fInvLength;
    kQ[2][2] *= fInvLength;

    // guarantee that orthogonal matrix has determinant 1 (no reflections)
    float fDet = kQ[0][0] * kQ[1][1] * kQ[2][2] + kQ[0][1] * kQ[1][2] * kQ[2][0] +
                kQ[0][2] * kQ[1][0] * kQ[2][1] - kQ[0][2] * kQ[1][1] * kQ[2][0] -
                kQ[0][1] * kQ[1][0] * kQ[2][2] - kQ[0][0] * kQ[1][2] * kQ[2][1];

    if ( fDet < 0.0 ) {
        for (int iRow = 0; iRow < 3; iRow++)
            for (int iCol = 0; iCol < 3; iCol++)
                kQ[iRow][iCol] = -kQ[iRow][iCol];
    }

    // build "right" matrix R
    Matrix3 kR;

    kR[0][0] = kQ[0][0] * elt[0][0] + kQ[1][0] * elt[1][0] +
               kQ[2][0] * elt[2][0];

    kR[0][1] = kQ[0][0] * elt[0][1] + kQ[1][0] * elt[1][1] +
               kQ[2][0] * elt[2][1];

    kR[1][1] = kQ[0][1] * elt[0][1] + kQ[1][1] * elt[1][1] +
               kQ[2][1] * elt[2][1];

    kR[0][2] = kQ[0][0] * elt[0][2] + kQ[1][0] * elt[1][2] +
               kQ[2][0] * elt[2][2];

    kR[1][2] = kQ[0][1] * elt[0][2] + kQ[1][1] * elt[1][2] +
               kQ[2][1] * elt[2][2];

    kR[2][2] = kQ[0][2] * elt[0][2] + kQ[1][2] * elt[1][2] +
               kQ[2][2] * elt[2][2];

    // the scaling component
    kD[0] = kR[0][0];

    kD[1] = kR[1][1];

    kD[2] = kR[2][2];

    // the shear component
    float fInvD0 = 1.0 / kD[0];

    kU[0] = kR[0][1] * fInvD0;

    kU[1] = kR[0][2] * fInvD0;

    kU[2] = kR[1][2] / kD[1];
}

//----------------------------------------------------------------------------
float Matrix3::maxCubicRoot (float afCoeff[3]) {
    // Spectral norm is for A^T*A, so characteristic polynomial
    // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive float roots.
    // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].

    // quick out for uniform scale (triple root)
    const float fOneThird = 1.0f / 3.0f;
    const float fEpsilon = 1e-06f;
    float fDiscr = afCoeff[2] * afCoeff[2] - 3.0f * afCoeff[1];

    if ( fDiscr <= fEpsilon )
        return -fOneThird*afCoeff[2];

    // Compute an upper bound on roots of P(x).  This assumes that A^T*A
    // has been scaled by its largest entry.
    float fX = 1.0f;

    float fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX));

    if ( fPoly < 0.0f ) {
        // uses a matrix norm to find an upper bound on maximum root
        fX = G3D::abs(afCoeff[0]);
        float fTmp = 1.0 + G3D::abs(afCoeff[1]);

        if ( fTmp > fX )
            fX = fTmp;

        fTmp = 1.0 + G3D::abs(afCoeff[2]);

        if ( fTmp > fX )
            fX = fTmp;
    }

    // Newton's method to find root
    float fTwoC2 = 2.0f * afCoeff[2];

    for (int i = 0; i < 16; i++) {
        fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX));

        if ( G3D::abs(fPoly) <= fEpsilon )
            return fX;

        float fDeriv = afCoeff[1] + fX * (fTwoC2 + 3.0f * fX);

        fX -= fPoly / fDeriv;
    }

    return fX;
}

//----------------------------------------------------------------------------
float Matrix3::spectralNorm () const {
    Matrix3 kP;
    int iRow, iCol;
    float fPmax = 0.0;

    for (iRow = 0; iRow < 3; iRow++) {
        for (iCol = 0; iCol < 3; iCol++) {
            kP[iRow][iCol] = 0.0;

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

            if ( kP[iRow][iCol] > fPmax )
                fPmax = kP[iRow][iCol];
        }
    }

    float fInvPmax = 1.0 / fPmax;

    for (iRow = 0; iRow < 3; iRow++) {
        for (iCol = 0; iCol < 3; iCol++)
            kP[iRow][iCol] *= fInvPmax;
    }

    float afCoeff[3];
    afCoeff[0] = -(kP[0][0] * (kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1]) +
                   kP[0][1] * (kP[2][0] * kP[1][2] - kP[1][0] * kP[2][2]) +
                   kP[0][2] * (kP[1][0] * kP[2][1] - kP[2][0] * kP[1][1]));
    afCoeff[1] = kP[0][0] * kP[1][1] - kP[0][1] * kP[1][0] +
                 kP[0][0] * kP[2][2] - kP[0][2] * kP[2][0] +
                 kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1];
    afCoeff[2] = -(kP[0][0] + kP[1][1] + kP[2][2]);

    float fRoot = maxCubicRoot(afCoeff);
    float fNorm = sqrt(fPmax * fRoot);
    return fNorm;
}

//----------------------------------------------------------------------------
void Matrix3::toAxisAngle (Vector3& rkAxis, float& rfRadians) const {
    // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
    // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
    // I is the identity and
    //
    //       +-        -+
    //   P = |  0 -z +y |
    //       | +z  0 -x |
    //       | -y +x  0 |
    //       +-        -+
    //
    // If A > 0, R represents a counterclockwise rotation about the axis in
    // the sense of looking from the tip of the axis vector towards the
    // origin.  Some algebra will show that
    //
    //   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
    //
    // In the event that A = pi, R-R^t = 0 which prevents us from extracting
    // the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
    // P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and
    // z^2-1.  We can solve these for axis (x,y,z).  Because the angle is pi,
    // it does not matter which sign you choose on the square roots.

    float fTrace = elt[0][0] + elt[1][1] + elt[2][2];
    float fCos = 0.5 * (fTrace - 1.0);
    rfRadians = G3D::aCos(fCos);  // in [0,PI]

    if ( rfRadians > 0.0 ) {
        if ( rfRadians < G3D_PI ) {
            rkAxis.x = elt[2][1] - elt[1][2];
            rkAxis.y = elt[0][2] - elt[2][0];
            rkAxis.z = elt[1][0] - elt[0][1];
            rkAxis.unitize();
        } else {
            // angle is PI
            float fHalfInverse;

            if ( elt[0][0] >= elt[1][1] ) {
                // r00 >= r11
                if ( elt[0][0] >= elt[2][2] ) {
                    // r00 is maximum diagonal term
                    rkAxis.x = 0.5 * sqrt(elt[0][0] -
                                                elt[1][1] - elt[2][2] + 1.0);
                    fHalfInverse = 0.5 / rkAxis.x;
                    rkAxis.y = fHalfInverse * elt[0][1];
                    rkAxis.z = fHalfInverse * elt[0][2];
                } else {
                    // r22 is maximum diagonal term
                    rkAxis.z = 0.5 * sqrt(elt[2][2] -
                                                elt[0][0] - elt[1][1] + 1.0);
                    fHalfInverse = 0.5 / rkAxis.z;
                    rkAxis.x = fHalfInverse * elt[0][2];
                    rkAxis.y = fHalfInverse * elt[1][2];
                }
            } else {
                // r11 > r00
                if ( elt[1][1] >= elt[2][2] ) {
                    // r11 is maximum diagonal term
                    rkAxis.y = 0.5 * sqrt(elt[1][1] -
                                                elt[0][0] - elt[2][2] + 1.0);
                    fHalfInverse = 0.5 / rkAxis.y;
                    rkAxis.x = fHalfInverse * elt[0][1];
                    rkAxis.z = fHalfInverse * elt[1][2];
                } else {
                    // r22 is maximum diagonal term
                    rkAxis.z = 0.5 * sqrt(elt[2][2] -
                                                elt[0][0] - elt[1][1] + 1.0);
                    fHalfInverse = 0.5 / rkAxis.z;
                    rkAxis.x = fHalfInverse * elt[0][2];
                    rkAxis.y = fHalfInverse * elt[1][2];
                }
            }
        }
    } else {
        // The angle is 0 and the matrix is the identity.  Any axis will
        // work, so just use the x-axis.
        rkAxis.x = 1.0;
        rkAxis.y = 0.0;
        rkAxis.z = 0.0;
    }
}

//----------------------------------------------------------------------------
Matrix3 Matrix3::fromAxisAngle (const Vector3& rkAxis, float fRadians) {
    Matrix3 m;

    float fCos = cos(fRadians);
    float fSin = sin(fRadians);
    float fOneMinusCos = 1.0 - fCos;
    float fX2 = rkAxis.x * rkAxis.x;
    float fY2 = rkAxis.y * rkAxis.y;
    float fZ2 = rkAxis.z * rkAxis.z;
    float fXYM = rkAxis.x * rkAxis.y * fOneMinusCos;
    float fXZM = rkAxis.x * rkAxis.z * fOneMinusCos;
    float fYZM = rkAxis.y * rkAxis.z * fOneMinusCos;
    float fXSin = rkAxis.x * fSin;
    float fYSin = rkAxis.y * fSin;
    float fZSin = rkAxis.z * fSin;

    m.elt[0][0] = fX2 * fOneMinusCos + fCos;
    m.elt[0][1] = fXYM - fZSin;
    m.elt[0][2] = fXZM + fYSin;
    m.elt[1][0] = fXYM + fZSin;
    m.elt[1][1] = fY2 * fOneMinusCos + fCos;
    m.elt[1][2] = fYZM - fXSin;
    m.elt[2][0] = fXZM - fYSin;
    m.elt[2][1] = fYZM + fXSin;
    m.elt[2][2] = fZ2 * fOneMinusCos + fCos;

    return m;
}

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

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

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

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

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

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

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

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

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

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