mgcpolynomial.cpp

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

        for (i = 0; i < m_iDegree; i++)
        {
            fTmp = MgcMath::Abs(m_afCoeff[i]) + fAbsD;
            if ( fBound < fTmp )
                fBound = fTmp;
        }
        fBound /= fAbsD;

        GetRootsOnInterval(-fBound,fBound,riCount,afRoot);
    }
    else
    {
        fBound = fAbsD;
        for (i = 0; i < m_iDegree; i++)
        {
            fTmp = MgcMath::Abs(m_afCoeff[i]) + fAbs0;
            if ( fBound < fTmp )
                fBound = fTmp;
        }
        fBound /= fAbs0;

        // use inverted polynomial to find inverse roots
        MgcReal* afInvRoot = new MgcReal[m_iDegree];
        MgcPolynomial kInvPoly = GetInversion();
        kInvPoly.GetRootsOnInterval(-fBound,fBound,riCount,afInvRoot);
        for (i = 0; i < riCount; i++)
            afRoot[i] = 1.0/afInvRoot[riCount-1-i];
        delete[] afInvRoot;
    }
}
//----------------------------------------------------------------------------
MgcReal MgcPolynomial::GetRootBound () const
{
    MgcReal fBound, fTmp;
    int i;

    MgcReal fAbs0 = MgcMath::Abs(m_afCoeff[0]);
    MgcReal fAbsD = MgcMath::Abs(m_afCoeff[m_iDegree]);

    if ( fAbsD >= fAbs0 )
    {
        fBound = fAbs0;
        for (i = 0; i < m_iDegree; i++)
        {
            fTmp = MgcMath::Abs(m_afCoeff[i]) + fAbsD;
            if ( fBound < fTmp )
                fBound = fTmp;
        }
        fBound /= fAbsD;
    }
    else
    {
        fBound = fAbsD;
        for (i = 0; i < m_iDegree; i++)
        {
            fTmp = MgcMath::Abs(m_afCoeff[i]) + fAbs0;
            if ( fBound < fTmp )
                fBound = fTmp;
        }
        fBound /= fAbs0;
    }

    return fBound;
}
//----------------------------------------------------------------------------
bool MgcPolynomial::AllRealPartsNegative (int iDegree, MgcReal* afCoeff)
{
    // assert:  afCoeff[iDegree] = 1

    if ( afCoeff[iDegree-1] <= 0.0 )
        return false;
    if ( iDegree == 1 )
        return true;

    MgcReal* afTmpCoeff = new MgcReal[iDegree];
    afTmpCoeff[0] = 2.0*afCoeff[0]*afCoeff[iDegree-1];
    int i;
    for (i = 1; i <= iDegree-2; i++) 
    {
        afTmpCoeff[i] = afCoeff[iDegree-1]*afCoeff[i];
        if ( ((iDegree-i) % 2) == 0 )
            afTmpCoeff[i] -= afCoeff[i-1];
        afTmpCoeff[i] *= 2.0;
    }
    afTmpCoeff[iDegree-1] = 2.0*afCoeff[iDegree-1]*afCoeff[iDegree-1];

    int iNextDegree;
    for (iNextDegree = iDegree-1; iNextDegree >= 0; iNextDegree--)
    {
        if ( afTmpCoeff[iNextDegree] != 0.0 )
            break;
    }
    for (i = 0; i <= iNextDegree-1; i++)
        afCoeff[i] = afTmpCoeff[i]/afTmpCoeff[iNextDegree];
    delete[] afTmpCoeff;

    return AllRealPartsNegative(iNextDegree,afCoeff);
}
//----------------------------------------------------------------------------
bool MgcPolynomial::AllRealPartsNegative () const
{
    // make a copy of coefficients, later calls will change the copy
    MgcReal* afCoeff = new MgcReal[m_iDegree+1];
    memcpy(afCoeff,m_afCoeff,(m_iDegree+1)*sizeof(MgcReal));

    // make polynomial monic
    if ( afCoeff[m_iDegree] != 1.0 )
    {
        MgcReal fInv = 1.0/afCoeff[m_iDegree];
        for (int i = 0; i < m_iDegree; i++)
            afCoeff[i] *= fInv;
        afCoeff[m_iDegree] = 1.0;
    }

    return AllRealPartsNegative(m_iDegree,afCoeff);
}
//----------------------------------------------------------------------------
bool MgcPolynomial::AllRealPartsPositive () const
{
    // make a copy of coefficients, later calls will change the copy
    MgcReal* afCoeff = new MgcReal[m_iDegree+1];
    memcpy(afCoeff,m_afCoeff,(m_iDegree+1)*sizeof(MgcReal));

    // make polynomial monic
    int i;
    if ( afCoeff[m_iDegree] != 1.0 )
    {
        MgcReal fInv = 1.0/afCoeff[m_iDegree];
        for (i = 0; i < m_iDegree; i++)
            afCoeff[i] *= fInv;
        afCoeff[m_iDegree] = 1.0;
    }

    // reflect z -> -z
    int iSign = -1;
    for (i = m_iDegree-1; i >= 0; i--, iSign = -iSign)
        afCoeff[i] *= iSign;

    return AllRealPartsNegative(m_iDegree,afCoeff);
}
//----------------------------------------------------------------------------
bool MgcPolynomial::RootsDegree2 (int& riCount, MgcReal afRoot[2]) const
{
    // compute real roots to x^2+c[1]*x+c[0] = 0
    if ( m_iDegree != 2 )
        return false;

    // make polynomial monic
    MgcReal afCoeff[2] = { m_afCoeff[0], m_afCoeff[1] };
    if ( m_afCoeff[2] != 1.0 )
    {
        MgcReal fInv = 1.0/m_afCoeff[2];
        afCoeff[0] *= fInv;
        afCoeff[1] *= fInv;
    }

    MgcReal fDiscr = afCoeff[1]*afCoeff[1]-4.0*afCoeff[0];
    if ( MgcMath::Abs(fDiscr) <= ZERO_TOLERANCE )
        fDiscr = 0.0;

    if ( fDiscr >= 0.0 )
    {
        fDiscr = MgcMath::Sqrt(fDiscr);
        afRoot[0] = 0.5*(-afCoeff[1]-fDiscr);
        afRoot[1] = 0.5*(-afCoeff[1]+fDiscr);
        riCount = 2;
    }
    else
    {
        riCount = 0;
    }

    return true;
}
//----------------------------------------------------------------------------
bool MgcPolynomial::RootsDegree3 (int& riCount, MgcReal afRoot[3]) const
{
    // compute real roots to x^3+c[2]*x^2+c[1]*x+c[0] = 0
    if ( m_iDegree != 3 )
        return false;

    // make polynomial monic
    MgcReal afCoeff[3] = { m_afCoeff[0], m_afCoeff[1], m_afCoeff[2] };
    if ( m_afCoeff[3] != 1.0 )
    {
        MgcReal fInv = 1.0/m_afCoeff[3];
        afCoeff[0] *= fInv;
        afCoeff[1] *= fInv;
        afCoeff[2] *= fInv;
    }

    // convert to y^3+a*y+b = 0 by x = y-c[2]/3 and
    MgcReal fA = ms_fThird*(3.0*afCoeff[1]-afCoeff[2]*afCoeff[2]);
    MgcReal fB = ms_fTwentySeventh*(2.0*afCoeff[2]*afCoeff[2]*afCoeff[2] -
        9.0*afCoeff[1]*afCoeff[2]+27.0*afCoeff[0]);
    MgcReal fOffset = ms_fThird*afCoeff[2];

    MgcReal fDiscr = 0.25*fB*fB + ms_fTwentySeventh*fA*fA*fA;
    if ( MgcMath::Abs(fDiscr) <= ZERO_TOLERANCE )
        fDiscr = 0.0;

    MgcReal fHalfB = 0.5*fB;
    if ( fDiscr > 0.0 )  // 1 real, 2 complex roots
    {
        fDiscr = MgcMath::Sqrt(fDiscr);
        MgcReal fTemp = -fHalfB + fDiscr;
        if ( fTemp >= 0.0 )
            afRoot[0] = MgcMath::Pow(fTemp,ms_fThird);
        else
            afRoot[0] = -MgcMath::Pow(-fTemp,ms_fThird);
        fTemp = -fHalfB - fDiscr;
        if ( fTemp >= 0.0 )
            afRoot[0] += MgcMath::Pow(fTemp,ms_fThird);
        else
            afRoot[0] -= MgcMath::Pow(-fTemp,ms_fThird);
        afRoot[0] -= fOffset;
        riCount = 1;
    }
    else if ( fDiscr < 0.0 ) 
    {
        MgcReal fDist = MgcMath::Sqrt(-ms_fThird*fA);
        MgcReal fAngle = ms_fThird*MgcMath::ATan2(MgcMath::Sqrt(-fDiscr),
            -fHalfB);
        MgcReal fCos = MgcMath::Cos(fAngle);
        MgcReal fSin = MgcMath::Sin(fAngle);
        afRoot[0] = 2.0*fDist*fCos-fOffset;
        afRoot[1] = -fDist*(fCos+ms_fSqrt3*fSin)-fOffset;
        afRoot[2] = -fDist*(fCos-ms_fSqrt3*fSin)-fOffset;
        riCount = 3;
    }
    else 
    {
        MgcReal fTemp;
        if ( fHalfB >= 0.0 )
            fTemp = -MgcMath::Pow(fHalfB,ms_fThird);
        else
            fTemp = MgcMath::Pow(-fHalfB,ms_fThird);
        afRoot[0] = 2.0*fTemp-fOffset;
        afRoot[1] = -fTemp-fOffset;
        afRoot[2] = afRoot[1];
        riCount = 3;
    }

    return true;
}
//----------------------------------------------------------------------------
bool MgcPolynomial::RootsDegree4 (int& riCount, MgcReal afRoot[4]) const
{
    // compute real roots to x^4+c[3]*x^3+c[2]*x^2+c[1]*x+c[0] = 0
    if ( m_iDegree != 4 )
        return false;

    // make polynomial monic
    MgcReal afCoeff[4] = { m_afCoeff[0], m_afCoeff[1], m_afCoeff[2],
        m_afCoeff[3] };
    if ( m_afCoeff[4] != 1.0 )
    {
        MgcReal fInv = 1.0/m_afCoeff[4];
        afCoeff[0] *= fInv;
        afCoeff[1] *= fInv;
        afCoeff[2] *= fInv;
        afCoeff[3] *= fInv;
    }

    // reduction to resolvent cubic polynomial
    MgcPolynomial kResolve(3);
    kResolve[3] = 1.0;
    kResolve[2] = -afCoeff[2];
    kResolve[1] = afCoeff[3]*afCoeff[1]-4.0*afCoeff[0];
    kResolve[0] = -afCoeff[3]*afCoeff[3]*afCoeff[0] +
        4.0*afCoeff[2]*afCoeff[0]-afCoeff[1]*afCoeff[1];
    int iResolveCount;
    MgcReal afResolveRoot[3];
    kResolve.RootsDegree3(iResolveCount,afResolveRoot);
    MgcReal fY = afResolveRoot[0];

    riCount = 0;
    MgcReal fDiscr = 0.25*afCoeff[3]*afCoeff[3]-afCoeff[2]+fY;
    if ( MgcMath::Abs(fDiscr) <= ZERO_TOLERANCE )
        fDiscr = 0.0;

    if ( fDiscr > 0.0 ) 
    {
        MgcReal fR = MgcMath::Sqrt(fDiscr);
        MgcReal fT1 = 0.75*afCoeff[3]*afCoeff[3]-fR*fR-2.0*afCoeff[2];
        MgcReal fT2 = (4.0*afCoeff[3]*afCoeff[2]-8.0*afCoeff[1]-
            afCoeff[3]*afCoeff[3]*afCoeff[3])/(4.0*fR);

        MgcReal fTplus = fT1+fT2;
        MgcReal fTminus = fT1-fT2;
        if ( MgcMath::Abs(fTplus) <= ZERO_TOLERANCE ) 
            fTplus = 0.0;
        if ( MgcMath::Abs(fTminus) <= ZERO_TOLERANCE ) 
            fTminus = 0.0;

        if ( fTplus >= 0.0 )
        {
            MgcReal fD = MgcMath::Sqrt(fTplus);
            afRoot[0] = -0.25*afCoeff[3]+0.5f*(fR+fD);
            afRoot[1] = -0.25*afCoeff[3]+0.5f*(fR-fD);
            riCount += 2;
        }
        if ( fTminus >= 0.0 )
        {
            MgcReal fE = MgcMath::Sqrt(fTminus);
            afRoot[riCount++] = -0.25*afCoeff[3]+0.5f*(fE-fR);
            afRoot[riCount++] = -0.25*afCoeff[3]-0.5f*(fE+fR);
        }
    }
    else if ( fDiscr < 0.0 )
    {
        riCount = 0;
    }
    else
    {
        MgcReal fT2 = fY*fY-4.0*afCoeff[0];
        if ( fT2 >= -ZERO_TOLERANCE ) 
        {
            if ( fT2 < 0.0 ) // round to zero
                fT2 = 0.0;
            fT2 = 2.0*MgcMath::Sqrt(fT2);
            MgcReal fT1 = 0.75*afCoeff[3]*afCoeff[3]-2.0*afCoeff[2];
            if ( fT1+fT2 >= ZERO_TOLERANCE ) 
            {
                MgcReal fD = MgcMath::Sqrt(fT1+fT2);
                afRoot[0] = -0.25*afCoeff[3]+0.5*fD;
                afRoot[1] = -0.25*afCoeff[3]-0.5*fD;
                riCount += 2;
            }
            if ( fT1-fT2 >= ZERO_TOLERANCE ) 
            {
                MgcReal fE = MgcMath::Sqrt(fT1-fT2);
                afRoot[riCount++] = -0.25*afCoeff[3]+0.5*fE;
                afRoot[riCount++] = -0.25*afCoeff[3]-0.5*fE;
            }
        }
    }

    return true;
}
//----------------------------------------------------------------------------
MgcReal MgcPolynomial::SpecialCubeRoot (MgcReal fA, MgcReal fB, MgcReal fC)
{
    // Solve A*r^3 + B*r = C where A > 0 and B > 0.
    //
    // Let r = D*sinh(u) where D = sqrt(4*B/(3*A)).  Then
    // sinh(3*u) = 4*[sinh(u)]^3+3*sinh(u) = E where E = 4*C/(A*D^3).
    // sinh(3*u) = E has solution u = (1/3)*log(E+sqrt(E^2+1)).  This
    // leads to sinh(u) = ((E+sqrt(E^2+1))^{1/3}-(E+sqrt(E^2+1))^{-1/3})/2.
    // Therefore,
    //
    //     r = D*((E+sqrt(E^2+1))^{1/3}-(E+sqrt(E^2+1))^{-1/3})/2.

    MgcReal fD = MgcMath::Sqrt(4.0*ms_fThird*fB/fA);
    MgcReal fE = 4.0*fC/(fA*fD*fD*fD);
    MgcReal fF = MgcMath::Pow(fE+MgcMath::Sqrt(fE*fE+1.0),ms_fThird);
    MgcReal fRoot = 0.5*fD*(fF-1.0/fF);

    return fRoot;
}
//----------------------------------------------------------------------------