mgcmatrix3.cpp

《3D游戏引擎设计》的源码

    // guarantee that orthogonal matrix has determinant 1 (no reflections)
    MgcReal 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
    MgcMatrix3 kR;
    kR[0][0] = kQ[0][0]*m_aafEntry[0][0] + kQ[1][0]*m_aafEntry[1][0] +
        kQ[2][0]*m_aafEntry[2][0];
    kR[0][1] = kQ[0][0]*m_aafEntry[0][1] + kQ[1][0]*m_aafEntry[1][1] +
        kQ[2][0]*m_aafEntry[2][1];
    kR[1][1] = kQ[0][1]*m_aafEntry[0][1] + kQ[1][1]*m_aafEntry[1][1] +
        kQ[2][1]*m_aafEntry[2][1];
    kR[0][2] = kQ[0][0]*m_aafEntry[0][2] + kQ[1][0]*m_aafEntry[1][2] +
        kQ[2][0]*m_aafEntry[2][2];
    kR[1][2] = kQ[0][1]*m_aafEntry[0][2] + kQ[1][1]*m_aafEntry[1][2] +
        kQ[2][1]*m_aafEntry[2][2];
    kR[2][2] = kQ[0][2]*m_aafEntry[0][2] + kQ[1][2]*m_aafEntry[1][2] +
        kQ[2][2]*m_aafEntry[2][2];

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

    // the shear component
    MgcReal fInvD0 = 1.0/kD[0];
    kU[0] = kR[0][1]*fInvD0;
    kU[1] = kR[0][2]*fInvD0;
    kU[2] = kR[1][2]/kD[1];
}
//----------------------------------------------------------------------------
MgcReal MgcMatrix3::MaxCubicRoot (MgcReal 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 real roots.
    // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].

    // quick out for uniform scale (triple root)
    const MgcReal fOneThird = 1.0/3.0;
    const MgcReal fEpsilon = 1e-06;
    MgcReal fDiscr = afCoeff[2]*afCoeff[2] - 3.0*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.
    MgcReal fX = 1.0;
    MgcReal fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
    if ( fPoly < 0.0 )
    {
        // uses a matrix norm to find an upper bound on maximum root
        fX = MgcMath::Abs(afCoeff[0]);
        MgcReal fTmp = 1.0+MgcMath::Abs(afCoeff[1]);
        if ( fTmp > fX )
            fX = fTmp;
        fTmp = 1.0+MgcMath::Abs(afCoeff[2]);
        if ( fTmp > fX )
            fX = fTmp;
    }

    // Newton's method to find root
    MgcReal fTwoC2 = 2.0*afCoeff[2];
    for (int i = 0; i < 16; i++)
    {
        fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
        if ( MgcMath::Abs(fPoly) <= fEpsilon )
            return fX;

        MgcReal fDeriv = afCoeff[1]+fX*(fTwoC2+3.0*fX);
        fX -= fPoly/fDeriv;
    }

    return fX;
}
//----------------------------------------------------------------------------
MgcReal MgcMatrix3::SpectralNorm () const
{
    MgcMatrix3 kP;
    int iRow, iCol;
    MgcReal 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] +=
                    m_aafEntry[iMid][iRow]*m_aafEntry[iMid][iCol];
            }
            if ( kP[iRow][iCol] > fPmax )
                fPmax = kP[iRow][iCol];
        }
    }

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

    MgcReal 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]);

    MgcReal fRoot = MaxCubicRoot(afCoeff);
    MgcReal fNorm = MgcMath::Sqrt(fPmax*fRoot);
    return fNorm;
}
//----------------------------------------------------------------------------
void MgcMatrix3::ToAxisAngle (MgcVector3& rkAxis, MgcReal& 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.

    MgcReal fTrace = m_aafEntry[0][0] + m_aafEntry[1][1] + m_aafEntry[2][2];
    MgcReal fCos = 0.5*(fTrace-1.0);
    rfRadians = MgcMath::ACos(fCos);  // in [0,PI]

    if ( rfRadians > 0.0 )
    {
        if ( rfRadians < MgcMath::PI )
        {
            rkAxis.x = m_aafEntry[2][1]-m_aafEntry[1][2];
            rkAxis.y = m_aafEntry[0][2]-m_aafEntry[2][0];
            rkAxis.z = m_aafEntry[1][0]-m_aafEntry[0][1];
            rkAxis.Unitize();
        }
        else
        {
            // angle is PI
            float fHalfInverse;
            if ( m_aafEntry[0][0] >= m_aafEntry[1][1] )
            {
                // r00 >= r11
                if ( m_aafEntry[0][0] >= m_aafEntry[2][2] )
                {
                    // r00 is maximum diagonal term
                    rkAxis.x = 0.5*MgcMath::Sqrt(m_aafEntry[0][0] -
                        m_aafEntry[1][1] - m_aafEntry[2][2] + 1.0);
                    fHalfInverse = 0.5/rkAxis.x;
                    rkAxis.y = fHalfInverse*m_aafEntry[0][1];
                    rkAxis.z = fHalfInverse*m_aafEntry[0][2];
                }
                else
                {
                    // r22 is maximum diagonal term
                    rkAxis.z = 0.5*MgcMath::Sqrt(m_aafEntry[2][2] -
                        m_aafEntry[0][0] - m_aafEntry[1][1] + 1.0);
                    fHalfInverse = 0.5/rkAxis.z;
                    rkAxis.x = fHalfInverse*m_aafEntry[0][2];
                    rkAxis.y = fHalfInverse*m_aafEntry[1][2];
                }
            }
            else
            {
                // r11 > r00
                if ( m_aafEntry[1][1] >= m_aafEntry[2][2] )
                {
                    // r11 is maximum diagonal term
                    rkAxis.y = 0.5*MgcMath::Sqrt(m_aafEntry[1][1] -
                        m_aafEntry[0][0] - m_aafEntry[2][2] + 1.0);
                    fHalfInverse  = 0.5/rkAxis.y;
                    rkAxis.x = fHalfInverse*m_aafEntry[0][1];
                    rkAxis.z = fHalfInverse*m_aafEntry[1][2];
                }
                else
                {
                    // r22 is maximum diagonal term
                    rkAxis.z = 0.5*MgcMath::Sqrt(m_aafEntry[2][2] -
                        m_aafEntry[0][0] - m_aafEntry[1][1] + 1.0);
                    fHalfInverse = 0.5/rkAxis.z;
                    rkAxis.x = fHalfInverse*m_aafEntry[0][2];
                    rkAxis.y = fHalfInverse*m_aafEntry[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;
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromAxisAngle (const MgcVector3& rkAxis, MgcReal fRadians)
{
    MgcReal fCos = MgcMath::Cos(fRadians);
    MgcReal fSin = MgcMath::Sin(fRadians);
    MgcReal fOneMinusCos = 1.0-fCos;
    MgcReal fX2 = rkAxis.x*rkAxis.x;
    MgcReal fY2 = rkAxis.y*rkAxis.y;
    MgcReal fZ2 = rkAxis.z*rkAxis.z;
    MgcReal fXYM = rkAxis.x*rkAxis.y*fOneMinusCos;
    MgcReal fXZM = rkAxis.x*rkAxis.z*fOneMinusCos;
    MgcReal fYZM = rkAxis.y*rkAxis.z*fOneMinusCos;
    MgcReal fXSin = rkAxis.x*fSin;
    MgcReal fYSin = rkAxis.y*fSin;
    MgcReal fZSin = rkAxis.z*fSin;
    
    m_aafEntry[0][0] = fX2*fOneMinusCos+fCos;
    m_aafEntry[0][1] = fXYM-fZSin;
    m_aafEntry[0][2] = fXZM+fYSin;
    m_aafEntry[1][0] = fXYM+fZSin;
    m_aafEntry[1][1] = fY2*fOneMinusCos+fCos;
    m_aafEntry[1][2] = fYZM-fXSin;
    m_aafEntry[2][0] = fXZM-fYSin;
    m_aafEntry[2][1] = fYZM+fXSin;
    m_aafEntry[2][2] = fZ2*fOneMinusCos+fCos;
}
//----------------------------------------------------------------------------
MgcReal MgcMatrix3::ExtractAngle (int i0) const
{
    int i1 = (i0 + 1) % 3;
    int i2 = (i0 + 2) % 3;

    if ( MgcMath::Abs(m_aafEntry[i1][i0]) < MgcMath::Abs(m_aafEntry[i2][i0]) )
        return -MgcMath::ATan2(m_aafEntry[i1][i2],m_aafEntry[i1][i1]);
    else
        return MgcMath::ATan2(m_aafEntry[i2][i1],m_aafEntry[i2][i2]);
}
//----------------------------------------------------------------------------
bool MgcMatrix3::ToEulerAnglesXYZ (float& rfYAngle, float& rfPAngle,
    float& rfRAngle)
{
    // 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

    rfPAngle = MgcMath::ASin(m_aafEntry[0][2]);
    if ( rfPAngle < MgcMath::HALF_PI )
    {
        if ( rfPAngle > -MgcMath::HALF_PI )
        {
            rfYAngle = MgcMath::ATan2(-m_aafEntry[1][2],m_aafEntry[2][2]);
            rfRAngle = MgcMath::ATan2(-m_aafEntry[0][1],m_aafEntry[0][0]);
            return true;
        }
        else
        {
            // WARNING.  Not a unique solution.
            float fRmY = MgcMath::ATan2(m_aafEntry[1][0],m_aafEntry[1][1]);
            rfRAngle = 0.0;  // any angle works
            rfYAngle = rfRAngle - fRmY;
            return false;
        }
    }
    else
    {
        // WARNING.  Not a unique solution.
        float fRpY = MgcMath::ATan2(m_aafEntry[1][0],m_aafEntry[1][1]);
        rfRAngle = 0.0;  // any angle works
        rfYAngle = fRpY - rfRAngle;
        return false;
    }
}
//----------------------------------------------------------------------------
bool MgcMatrix3::ToEulerAnglesXZY (float& rfYAngle, float& rfPAngle,
    float& rfRAngle)
{
    // 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

    rfPAngle = MgcMath::ASin(-m_aafEntry[0][1]);
    if ( rfPAngle < MgcMath::HALF_PI )
    {
        if ( rfPAngle > -MgcMath::HALF_PI )
        {
            rfYAngle = MgcMath::ATan2(m_aafEntry[2][1],m_aafEntry[1][1]);
            rfRAngle = MgcMath::ATan2(m_aafEntry[0][2],m_aafEntry[0][0]);
            return true;
        }
        else
        {
            // WARNING.  Not a unique solution.
            float fRmY = MgcMath::ATan2(-m_aafEntry[2][0],m_aafEntry[2][2]);
            rfRAngle = 0.0;  // any angle works
            rfYAngle = rfRAngle - fRmY;
            return false;
        }
    }
    else
    {
        // WARNING.  Not a unique solution.
        float fRpY = MgcMath::ATan2(-m_aafEntry[2][0],m_aafEntry[2][2]);
        rfRAngle = 0.0;  // any angle works
        rfYAngle = fRpY - rfRAngle;
        return false;
    }
}
//----------------------------------------------------------------------------
bool MgcMatrix3::ToEulerAnglesYXZ (float& rfYAngle, float& rfPAngle,
    float& rfRAngle)
{
    // 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

    rfPAngle = MgcMath::ASin(-m_aafEntry[1][2]);
    if ( rfPAngle < MgcMath::HALF_PI )
    {
        if ( rfPAngle > -MgcMath::HALF_PI )
        {
            rfYAngle = MgcMath::ATan2(m_aafEntry[0][2],m_aafEntry[2][2]);
            rfRAngle = MgcMath::ATan2(m_aafEntry[1][0],m_aafEntry[1][1]);
            return true;
        }
        else
        {
            // WARNING.  Not a unique solution.
            float fRmY = MgcMath::ATan2(-m_aafEntry[0][1],m_aafEntry[0][0]);
            rfRAngle = 0.0;  // any angle works
            rfYAngle = rfRAngle - fRmY;
            return false;
        }
    }
    else
    {
        // WARNING.  Not a unique solution.
        float fRpY = MgcMath::ATan2(-m_aafEntry[0][1],m_aafEntry[0][0]);
        rfRAngle = 0.0;  // any angle works
        rfYAngle = fRpY - rfRAngle;
        return false;
    }
}
//----------------------------------------------------------------------------
bool MgcMatrix3::ToEulerAnglesYZX (float& rfYAngle, float& rfPAngle,
    float& rfRAngle)
{
    // 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

    rfPAngle = MgcMath::ASin(m_aafEntry[1][0]);
    if ( rfPAngle < MgcMath::HALF_PI )
    {
        if ( rfPAngle > -MgcMath::HALF_PI )
        {
            rfYAngle = MgcMath::ATan2(-m_aafEntry[2][0],m_aafEntry[0][0]);
            rfRAngle = MgcMath::ATan2(-m_aafEntry[1][2],m_aafEntry[1][1]);
            return true;
        }
        else
        {
            // WARNING.  Not a unique solution.
            float fRmY = MgcMath::ATan2(m_aafEntry[2][1],m_aafEntry[2][2]);