mgcmultiplecurve.inl

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

// 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 "MgcIntegrate.h"
#include "MgcRTLib.h"

//----------------------------------------------------------------------------
template <class Vector>
MgcMultipleCurve<Vector>::MgcMultipleCurve (int iSegments, MgcReal* afTime)
    :
    MgcCurve<Vector>(afTime[0],afTime[iSegments])
{
    m_iSegments = iSegments;
    m_afTime = afTime;
    m_afLength = 0;
    m_afAccumLength = 0;
}
//----------------------------------------------------------------------------
template <class Vector>
MgcMultipleCurve<Vector>::~MgcMultipleCurve ()
{
    delete[] m_afTime;
    delete[] m_afLength;
    delete[] m_afAccumLength;
}
//---------------------------------------------------------------------------
template <class Vector>
int MgcMultipleCurve<Vector>::GetSegments () const
{
    return m_iSegments;
}
//----------------------------------------------------------------------------
template <class Vector>
const MgcReal* MgcMultipleCurve<Vector>::GetTimes () const
{
    return m_afTime;
}
//----------------------------------------------------------------------------
template <class Vector>
void MgcMultipleCurve<Vector>::GetKeyInfo (MgcReal fTime, int& riKey,
    MgcReal& rfDt) const
{
    if ( fTime <= m_afTime[0] )
    {
        riKey = 0;
        rfDt = 0.0;
    }
    else if ( fTime >= m_afTime[m_iSegments] )
    {
        riKey = m_iSegments-1;
        rfDt = m_afTime[m_iSegments] - m_afTime[m_iSegments-1];
    }
    else
    {
        for (int i = 0; i < m_iSegments; i++)
        {
            if ( fTime < m_afTime[i+1] )
            {
                riKey = i;
                rfDt = fTime - m_afTime[i];
                break;
            }
        }
    }
}
//---------------------------------------------------------------------------
template <class Vector>
MgcReal MgcMultipleCurve<Vector>::GetSpeedWithData (MgcReal fTime,
    void* pvData)
{
    MgcMultipleCurve<Vector>* pvThis = *(MgcMultipleCurve<Vector>**) pvData;
    int iKey = *(int*)((char*)pvThis + sizeof(pvThis));
    return pvThis->GetSpeed(iKey,fTime);
}
//---------------------------------------------------------------------------
template <class Vector>
void MgcMultipleCurve<Vector>::InitializeLength () const
{
    m_afLength = new MgcReal[m_iSegments];
    m_afAccumLength = new MgcReal[m_iSegments];

    // arc lengths of the segments
    int iKey;
    for (iKey = 0; iKey < m_iSegments; iKey++)
    {
        m_afLength[iKey] = GetLength(iKey,0.0,
            m_afTime[iKey+1]-m_afTime[iKey]);
    }

    // accumulative arc length
    m_afAccumLength[0] = m_afLength[0];
    for (iKey = 1; iKey < m_iSegments; iKey++)
        m_afAccumLength[iKey] = m_afAccumLength[iKey-1] + m_afLength[iKey];
}
//---------------------------------------------------------------------------
template <class Vector>
MgcReal MgcMultipleCurve<Vector>::GetLength (MgcReal fT0, MgcReal fT1) const
{
    assert( m_fTMin <= fT0 && fT0 <= m_fTMax );
    assert( m_fTMin <= fT1 && fT1 <= m_fTMax );
    assert( fT0 <= fT1 );

    if ( !m_afLength )
        InitializeLength();

    int iKey0, iKey1;
    MgcReal fDt0, fDt1;
    GetKeyInfo(fT0,iKey0,fDt0);
    GetKeyInfo(fT1,iKey1,fDt1);

    MgcReal fLength;
    if ( iKey0 < iKey1 )
    {
        // accumulate full-segment lengths
        fLength = 0.0;
        for (int i = iKey0+1; i < iKey1; i++)
            fLength += m_afLength[i];
        
        // add on partial first segment
        fLength += GetLength(iKey0,fDt0,m_afTime[iKey0+1]-m_afTime[iKey0]);
        
        // add on partial last segment
        fLength += GetLength(iKey1,0.0,fDt1);
    }
    else
    {
        fLength = GetLength(iKey0,fDt0,fDt1);
    }

    return fLength;
}
//---------------------------------------------------------------------------
template <class Vector>
MgcReal MgcMultipleCurve<Vector>::GetTime (MgcReal fLength, int iIterations,
    MgcReal fTolerance) const
{
    if ( !m_afLength )
        InitializeLength();

    if ( fLength <= 0.0 )
        return m_fTMin;

    if ( fLength >= m_afAccumLength[m_iSegments-1] )
        return m_fTMax;

    int iKey;
    for (iKey = 0; iKey < m_iSegments; iKey++)
    {
        if ( fLength < m_afAccumLength[iKey] )
            break;
    }
    if ( iKey >= m_iSegments )
        return m_afTime[m_iSegments];

    // try Newton's method first for rapid convergence
    MgcReal fL0, fL1;
    if ( iKey == 0 )
    {
        fL0 = fLength;
        fL1 = m_afAccumLength[0];
    }
    else
    {
        fL0 = fLength - m_afAccumLength[iKey-1];
        fL1 = m_afAccumLength[iKey] - m_afAccumLength[iKey-1];
    }

    // use Newton's method to invert the arc length integral
    MgcReal fDt1 = m_afTime[iKey+1] - m_afTime[iKey];
    MgcReal fDt0 = fDt1*fL0/fL1;
    for (int i = 0; i < iIterations; i++)
    {
        MgcReal fDifference = GetLength(iKey,0.0,fDt0) - fL0;
        if ( MgcMath::Abs(fDifference) <= fTolerance )
            return m_afTime[iKey] + fDt0;

        fDt0 -= fDifference/GetSpeed(iKey,fDt0);
    }

    // Newton's method failed.  If this happens, increase iterations or
    // tolerance or integration accuracy.
    return MgcMath::INFINITY;
}
//---------------------------------------------------------------------------
template <class Vector>
MgcReal MgcMultipleCurve<Vector>::GetVariation (MgcReal fT0, MgcReal fT1,
    const Vector* pkP0, const Vector* pkP1) const
{
    assert( m_fTMin <= fT0 && fT0 <= m_fTMax );
    assert( m_fTMin <= fT1 && fT1 <= m_fTMax );
    assert( fT0 <= fT1 );

    // construct line segment, A + (t-t0)*B
    Vector kP0, kP1;
    if ( !pkP0 )
    {
        kP0 = GetPosition(fT0);
        pkP0 = &kP0;
    }
    if ( !pkP1 )
    {
        kP1 = GetPosition(fT1);
        pkP1 = &kP1;
    }
    MgcReal fInvDT = 1.0/(fT1 - fT0);
    Vector kA, kB = fInvDT*(*pkP1 - *pkP0);

    int iKey0, iKey1;
    MgcReal fDt0, fDt1;
    GetKeyInfo(fT0,iKey0,fDt0);
    GetKeyInfo(fT1,iKey1,fDt1);

    MgcReal fVariation;
    if ( iKey0 < iKey1 )
    {
        // accumulate full-segment variations
        fVariation = 0.0;
        for (int i = iKey0+1; i < iKey1; i++)
        {
            kA = kP0 + (m_afTime[i] - fT0)*kB;
            fVariation += GetVariation(i,0.0,m_afTime[i+1]-m_afTime[i],kA,kB);
        }
        
        // add on partial first segment
        kA = kP0 + (m_afTime[iKey0] - fT0)*kB;
        fVariation += GetVariation(iKey0,fDt0,
            m_afTime[iKey0+1]-m_afTime[iKey0],kA,kB);
        
        // add on partial last segment
        kA = kP0 + (m_afTime[iKey1] - fT0)*kB;
        fVariation += GetVariation(iKey1,0.0,fDt1,kA,kB);
    }
    else
    {
        kA = kP0 + (m_afTime[iKey0] - fT0)*kB;
        fVariation = GetVariation(iKey0,fDt0,fDt1,kA,kB);
    }

    return fVariation;
}
//---------------------------------------------------------------------------