mgcpolynomial.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcRTLib.h"
#include "MgcPolynomial.h"

int MgcPolynomial::DIGITS_ACCURACY = 6;
MgcReal MgcPolynomial::ZERO_TOLERANCE = 1e-06;
const MgcReal MgcPolynomial::ms_fInvLog2 = 1.0/MgcMath::Log(2.0);
const MgcReal MgcPolynomial::ms_fLog10 = MgcMath::Log(10.0);
const MgcReal MgcPolynomial::ms_fThird = 1.0/3.0;
const MgcReal MgcPolynomial::ms_fSqrt3 = MgcMath::Sqrt(3.0);
const MgcReal MgcPolynomial::ms_fTwentySeventh = 1.0/27.0;

//----------------------------------------------------------------------------
MgcPolynomial::MgcPolynomial (int iDegree)
{
    if ( iDegree >= 0 )
    {
        m_iDegree = iDegree;
        m_afCoeff = new MgcReal[m_iDegree+1];
    }
    else
    {
        // default creation
        m_iDegree = -1;
        m_afCoeff = 0;
    }
}
//----------------------------------------------------------------------------
MgcPolynomial::MgcPolynomial (const MgcPolynomial& rkPoly)
{
    m_iDegree = rkPoly.m_iDegree;
    m_afCoeff = new MgcReal[m_iDegree+1];
    for (int i = 0; i <= m_iDegree; i++)
        m_afCoeff[i] = rkPoly.m_afCoeff[i];
}
//----------------------------------------------------------------------------
MgcPolynomial::~MgcPolynomial ()
{
    delete[] m_afCoeff;
}
//----------------------------------------------------------------------------
MgcReal& MgcPolynomial::operator[] (int i) const
{
    assert( 0 <= i && i <= m_iDegree );
    return ((MgcReal*)m_afCoeff)[i];
}
//----------------------------------------------------------------------------
MgcPolynomial& MgcPolynomial::operator= (const MgcPolynomial& rkPoly)
{
    delete[] m_afCoeff;
    m_iDegree = rkPoly.m_iDegree;

    if ( m_iDegree >= 0 )
    {
        m_afCoeff = new MgcReal[m_iDegree+1];
        for (int i = 0; i <= m_iDegree; i++)
            m_afCoeff[i] = rkPoly.m_afCoeff[i];
    }

    return *this;
}
//----------------------------------------------------------------------------
void MgcPolynomial::SetDegree (int iDegree)
{
    m_iDegree = iDegree;
    delete[] m_afCoeff;

    if ( m_iDegree >= 0 )
        m_afCoeff = new MgcReal[m_iDegree+1];
    else
        m_afCoeff = 0;
}
//----------------------------------------------------------------------------
int MgcPolynomial::GetDegree () const
{
    return m_iDegree;
}
//----------------------------------------------------------------------------
MgcReal MgcPolynomial::operator() (MgcReal fT) const
{
    assert( m_iDegree >= 0 );

    MgcReal fResult = m_afCoeff[m_iDegree];
    for (int i = m_iDegree-1; i >= 0; i--)
    {
        fResult *= fT;
        fResult += m_afCoeff[i];
    }
    return fResult;
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::GetDerivative () const
{
    if ( m_iDegree > 0 )
    {
        MgcPolynomial kDeriv(m_iDegree-1);
        for (int i0 = 0, i1 = 1; i0 < m_iDegree; i0++, i1++)
            kDeriv[i0] = i1*m_afCoeff[i1];
        return kDeriv;
    }
    else
    {
        return MgcPolynomial(-1);
    }
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::GetInversion () const
{
    MgcPolynomial kInvPoly(m_iDegree);
    for (int i = 0; i <= m_iDegree; i++)
        kInvPoly[i] = m_afCoeff[m_iDegree-i];
    return kInvPoly;
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::operator+ (const MgcPolynomial& rkPoly) const
{
    assert( m_iDegree >= 0 && rkPoly.m_iDegree >= 0 );

    MgcPolynomial kSum(-1);
    int i;

    if ( m_iDegree > rkPoly.m_iDegree )
    {
        kSum.SetDegree(m_iDegree);
        for (i = 0; i <= rkPoly.m_iDegree; i++)
            kSum[i] = m_afCoeff[i] + rkPoly.m_afCoeff[i];
        for (i = rkPoly.m_iDegree+1; i <= m_iDegree; i++)
            kSum[i] = m_afCoeff[i];

    }
    else
    {
        kSum.SetDegree(rkPoly.m_iDegree);
        for (i = 0; i <= m_iDegree; i++)
            kSum[i] = m_afCoeff[i] + rkPoly.m_afCoeff[i];
        for (i = m_iDegree+1; i <= m_iDegree; i++)
            kSum[i] = rkPoly.m_afCoeff[i];
    }

    return kSum;
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::operator- (const MgcPolynomial& rkPoly) const
{
    assert( m_iDegree >= 0 && rkPoly.m_iDegree >= 0 );

    MgcPolynomial kDiff(-1);
    int i;

    if ( m_iDegree > rkPoly.m_iDegree )
    {
        kDiff.SetDegree(m_iDegree);
        for (i = 0; i <= rkPoly.m_iDegree; i++)
            kDiff[i] = m_afCoeff[i] - rkPoly.m_afCoeff[i];
        for (i = rkPoly.m_iDegree+1; i <= m_iDegree; i++)
            kDiff[i] = m_afCoeff[i];

    }
    else
    {
        kDiff.SetDegree(rkPoly.m_iDegree);
        for (i = 0; i <= m_iDegree; i++)
            kDiff[i] = m_afCoeff[i] - rkPoly.m_afCoeff[i];
        for (i = m_iDegree+1; i <= m_iDegree; i++)
            kDiff[i] = -rkPoly.m_afCoeff[i];
    }

    return kDiff;
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::operator* (const MgcPolynomial& rkPoly) const
{
    assert( m_iDegree >= 0 && rkPoly.m_iDegree >= 0 );

    MgcPolynomial kProd(m_iDegree + rkPoly.m_iDegree);

    memset(kProd.m_afCoeff,0,(kProd.m_iDegree+1)*sizeof(MgcReal));
    for (int i0 = 0; i0 <= m_iDegree; i0++)
    {
        for (int i1 = 0; i1 <= rkPoly.m_iDegree; i1++)
        {
            int i2 = i0 + i1;
            kProd.m_afCoeff[i2] += m_afCoeff[i0]*rkPoly.m_afCoeff[i1];
        }
    }

    return kProd;
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::operator* (MgcReal fScalar) const
{
    assert( m_iDegree >= 0 );

    MgcPolynomial kProd(m_iDegree);
    for (int i = 0; i <= m_iDegree; i++)
        kProd[i] = fScalar*m_afCoeff[i];

    return kProd;
}
//----------------------------------------------------------------------------
MgcPolynomial MgcPolynomial::operator- () const
{
    assert( m_iDegree >= 0 );

    MgcPolynomial kNeg(m_iDegree);
    for (int i = 0; i <= m_iDegree; i++)
        kNeg[i] = -m_afCoeff[i];

    return kNeg;
}
//----------------------------------------------------------------------------
MgcPolynomial operator* (MgcReal fScalar, const MgcPolynomial& rkPoly)
{
    assert( rkPoly.m_iDegree >= 0 );

    MgcPolynomial kProd(rkPoly.m_iDegree);
    for (int i = 0; i <= rkPoly.m_iDegree; i++)
        kProd[i] = fScalar*rkPoly.m_afCoeff[i];

    return kProd;
}
//----------------------------------------------------------------------------
bool MgcPolynomial::Bisection (MgcReal fXMin, MgcReal fXMax,
    MgcReal& rfRoot) const
{
    MgcReal fP0 = (*this)(fXMin);
    MgcReal fP1 = (*this)(fXMax);

    // check for endpoint zeros
    if ( MgcMath::Abs(fP0) <= ZERO_TOLERANCE )
    {
        rfRoot = fXMin;
        return true;
    }
    if ( MgcMath::Abs(fP1) <= ZERO_TOLERANCE )
    {
        rfRoot = fXMax;
        return true;
    }

    if ( fP0*fP1 > 0.0 )
        return false;

    // determine number of iterations to get 'digits' accuracy.
    MgcReal fTmp0 = MgcMath::Log(fXMax-fXMin);
    MgcReal fTmp1 = DIGITS_ACCURACY*ms_fLog10;
    MgcReal fArg = (fTmp0 + fTmp1)*ms_fInvLog2;
    int iMaxIter = MgcMath::ICeil(fArg);
    
    for (int i = 0; i < iMaxIter; i++)
    {
        rfRoot = 0.5*(fXMin + fXMax);
        MgcReal fP = (*this)(rfRoot);
        if ( MgcMath::Abs(fP) <= ZERO_TOLERANCE )
            return true;
        
        if ( fP*fP0 < 0.0 )
        {
            fXMax = rfRoot;
            fP1 = fP;
        }
        else
        {
            fXMin = rfRoot;
            fP0 = fP;
        }
    }

    return true;
}
//----------------------------------------------------------------------------
void MgcPolynomial::GetRootsOnInterval (MgcReal fXMin, MgcReal fXMax,
    int& riCount, MgcReal* afRoot) const
{
    MgcReal fRoot;

    if ( m_iDegree == 1 )
    {
        if ( Bisection(fXMin,fXMax,fRoot) )
        {
            riCount = 1;
            afRoot[0] = fRoot;
        }
        else
        {
            riCount = 0;
        }
        return;
    }

    // get roots of derivative polynomial
    MgcPolynomial kDeriv = GetDerivative();
    kDeriv.GetRootsOnInterval(fXMin,fXMax,riCount,afRoot);

    int i, iNewCount = 0;
    MgcReal* afNewRoot = new MgcReal[riCount+1];

    if ( riCount > 0 )
    {
        // find root on [xmin,root[0]]
        if ( Bisection(fXMin,afRoot[0],fRoot) )
            afNewRoot[iNewCount++] = fRoot;

        // find root on [root[i],root[i+1]] for 0 <= i <= count-2
        for (i = 0; i <= riCount-2; i++)
        {
            if ( Bisection(afRoot[i],afRoot[i+1],fRoot) )
                afNewRoot[iNewCount++] = fRoot;
        }

        // find root on [root[count-1],xmax]
        if ( Bisection(afRoot[riCount-1],fXMax,fRoot) )
            afNewRoot[iNewCount++] = fRoot;
    }
    else
    {
        // polynomial is monotone on [xmin,xmax], has at most one root
        if ( Bisection(fXMin,fXMax,fRoot) )
            afNewRoot[iNewCount++] = fRoot;
    }

    // copy to old buffer
    if ( iNewCount > 0 )
    {
        riCount = 1;
        afRoot[0] = afNewRoot[0];
        for (i = 1; i < iNewCount; i++)
        {
            if ( MgcMath::Abs(afNewRoot[i]-afNewRoot[i-1]) > ZERO_TOLERANCE )
                afRoot[riCount++] = afNewRoot[i];
        }
    }
    else
    {
        riCount = 0;
    }

    delete[] afNewRoot;
}
//----------------------------------------------------------------------------
void MgcPolynomial::GetAllRoots (int& riCount, MgcReal* afRoot) 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;