mgcnaturalspline3.cpp

《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 "MgcLinearSystem.h"
#include "MgcNaturalSpline3.h"
#include "MgcPolynomial.h"
#include "MgcRTLib.h"

//---------------------------------------------------------------------------
MgcNaturalSpline3::MgcNaturalSpline3 (BoundaryType eType, int iSegments,
    MgcReal* afTime, MgcVector3* akPoint)
    :
    MgcMultipleCurve3(iSegments,afTime)
{
    m_akA = akPoint;

    switch ( eType )
    {
        case BT_FREE:
        {
            CreateFreeSpline();
            break;
        }
        case BT_CLAMPED:
        {
            CreateClampedSpline();
            break;
        }
        case BT_CLOSED:
        {
            CreateClosedSpline();
            break;
        }
    }
}
//---------------------------------------------------------------------------
MgcNaturalSpline3::~MgcNaturalSpline3 ()
{
    delete[] m_akA;
    delete[] m_akB;
    delete[] m_akC;
    delete[] m_akD;
}
//---------------------------------------------------------------------------
void MgcNaturalSpline3::CreateFreeSpline ()
{
    MgcReal* afDt = new MgcReal[m_iSegments];
    int i;
    for (i = 0; i < m_iSegments; i++)
        afDt[i] = m_afTime[i+1] - m_afTime[i];

    MgcReal* afD2t = new MgcReal[m_iSegments];
    for (i = 1; i < m_iSegments; i++)
        afD2t[i] = m_afTime[i+1] - m_afTime[i-1];

    MgcVector3* akAlpha = new MgcVector3[m_iSegments];
    for (i = 1; i < m_iSegments; i++)
    {
        MgcVector3 kNumer = 3.0*(afDt[i-1]*m_akA[i+1] - afD2t[i]*m_akA[i] +
            afDt[i]*m_akA[i-1]);
        MgcReal fInvDenom = 1.0/(afDt[i-1]*afDt[i]);
        akAlpha[i] = fInvDenom*kNumer;
    }

    MgcReal* afEll = new MgcReal[m_iSegments+1];
    MgcReal* afMu = new MgcReal[m_iSegments];
    MgcVector3* akZ = new MgcVector3[m_iSegments+1];
    MgcReal fInv;

    afEll[0] = 1.0;
    afMu[0] = 0.0;
    akZ[0] = MgcVector3::ZERO;
    for (i = 1; i < m_iSegments; i++)
    {
        afEll[i] = 2.0*afD2t[i] - afDt[i-1]*afMu[i-1];
        fInv = 1.0/afEll[i];
        afMu[i] = fInv*afDt[i];
        akZ[i] = fInv*(akAlpha[i] - afDt[i-1]*akZ[i-1]);
    }
    afEll[m_iSegments] = 1.0;
    akZ[m_iSegments] = MgcVector3::ZERO;

    m_akB = new MgcVector3[m_iSegments];
    m_akC = new MgcVector3[m_iSegments+1];
    m_akD = new MgcVector3[m_iSegments];

    m_akC[m_iSegments] = MgcVector3::ZERO;

    for (i = m_iSegments-1; i >= 0; i--)
    {
        const MgcReal fOneThird = 1.0/3.0;
        m_akC[i] = akZ[i] - afMu[i]*m_akC[i+1];
        fInv = 1.0/afDt[i];
        m_akB[i] = fInv*(m_akA[i+1] - m_akA[i]) - fOneThird*afDt[i]*(
            m_akC[i+1] + 2.0*m_akC[i]);
        m_akD[i] = fOneThird*fInv*(m_akC[i+1] - m_akC[i]);
    }

    delete[] afDt;
    delete[] afD2t;
    delete[] akAlpha;
    delete[] afEll;
    delete[] afMu;
    delete[] akZ;
}
//-----------------------------------------------------------------------------
void MgcNaturalSpline3::CreateClampedSpline ()
{
    MgcReal* afDt = new MgcReal[m_iSegments];
    int i;
    for (i = 0; i < m_iSegments; i++)
        afDt[i] = m_afTime[i+1] - m_afTime[i];

    MgcReal* afD2t = new MgcReal[m_iSegments];
    for (i = 1; i < m_iSegments; i++)
        afD2t[i] = m_afTime[i+1] - m_afTime[i-1];

    MgcVector3* akAlpha = new MgcVector3[m_iSegments+1];
    MgcReal fInv = 1.0/afDt[0];
    MgcVector3 kDiff = m_akA[1] - m_akA[0];
    akAlpha[0] = 3.0*(fInv - 1.0)*(m_akA[1] - m_akA[0]);
    fInv = 1.0/afDt[m_iSegments-1];
    akAlpha[m_iSegments] = 3.0*(1.0 - fInv)*(m_akA[m_iSegments] -
        m_akA[m_iSegments-1]);
    for (i = 1; i < m_iSegments; i++)
    {
        MgcVector3 kNumer = 3.0*(afDt[i-1]*m_akA[i+1] - afD2t[i]*m_akA[i] +
            afDt[i]*m_akA[i-1]);
        MgcReal fInvDenom = 1.0/(afDt[i-1]*afDt[i]);
        akAlpha[i] = fInvDenom*kNumer;
    }

    MgcReal* afEll = new MgcReal[m_iSegments+1];
    MgcReal* afMu = new MgcReal[m_iSegments];
    MgcVector3* akZ = new MgcVector3[m_iSegments+1];

    afEll[0] = 2.0*afDt[0];
    afMu[0] = 0.5;
    fInv = 1.0/afEll[0];
    akZ[0] = fInv*akAlpha[0];

    for (i = 1; i < m_iSegments; i++)
    {
        afEll[i] = 2.0*afD2t[i] - afDt[i-1]*afMu[i-1];
        fInv = 1.0/afEll[i];
        afMu[i] = fInv*afDt[i];
        akZ[i] = fInv*(akAlpha[i] - afDt[i-1]*akZ[i-1]);
    }
    afEll[m_iSegments] = afDt[m_iSegments-1]*(2.0 - afMu[m_iSegments-1]);
    fInv = 1.0/afEll[m_iSegments];
    akZ[m_iSegments] = fInv*(akAlpha[m_iSegments] - afDt[m_iSegments-1]*
        akZ[m_iSegments-1]);

    m_akB = new MgcVector3[m_iSegments];
    m_akC = new MgcVector3[m_iSegments+1];
    m_akD = new MgcVector3[m_iSegments];

    m_akC[m_iSegments] = akZ[m_iSegments];

    for (i = m_iSegments-1; i >= 0; i--)
    {
        MgcReal fOneThird = 1.0/3.0;
        m_akC[i] = akZ[i] - afMu[i]*m_akC[i+1];
        fInv = 1.0/afDt[i];
        m_akB[i] = fInv*(m_akA[i+1] - m_akA[i]) - fOneThird*afDt[i]*(
            m_akC[i+1] + 2.0*m_akC[i]);
        m_akD[i] = fOneThird*fInv*(m_akC[i+1] - m_akC[i]);
    }

    delete[] afDt;
    delete[] afD2t;
    delete[] akAlpha;
    delete[] afEll;
    delete[] afMu;
    delete[] akZ;
}
//-----------------------------------------------------------------------------
void MgcNaturalSpline3::CreateClosedSpline ()
{
    MgcReal* afDt = new MgcReal[m_iSegments];
    int i;
    for (i = 0; i < m_iSegments; i++)
        afDt[i] = m_afTime[i+1] - m_afTime[i];

    // TO DO.  Add ability to solve AX = B for B nxm (not just m=1) and
    // remove resetting of aafMat that occurs for each component of m_afC.
    MgcLinearSystem kSys;

    // construct matrix of system
    MgcReal** aafMat = kSys.NewMatrix(m_iSegments+1);
    aafMat[0][0] = 1.0;
    aafMat[0][m_iSegments] = -1.0;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        aafMat[i][i-1] = afDt[i-1];
        aafMat[i][i  ] = 2.0*(afDt[i-1] + afDt[i]);
        aafMat[i][i+1] = afDt[i];
    }
    aafMat[m_iSegments][m_iSegments-1] = afDt[m_iSegments-1];
    aafMat[m_iSegments][0] = 2.0*(afDt[m_iSegments-1] + afDt[0]);
    aafMat[m_iSegments][1] = afDt[0];

    // construct right-hand side of system
    m_akC = new MgcVector3[m_iSegments+1];
    m_akC[0] = MgcVector3::ZERO;
    MgcReal fInv0, fInv1;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        fInv0 = 1.0/afDt[i];
        fInv1 = 1.0/afDt[i-1];
        m_akC[i] = 3.0*(fInv0*(m_akA[i+1] - m_akA[i]) - fInv1*(m_akA[i] -
            m_akA[i-1]));
    }
    fInv0 = 1.0/afDt[0];
    fInv1 = 1.0/afDt[m_iSegments-1];
    m_akC[m_iSegments] = 3.0*(fInv0*(m_akA[1] - m_akA[0]) - fInv1*(m_akA[0] -
        m_akA[m_iSegments-1]));

    MgcReal* afCx = kSys.NewVector(m_iSegments+1);
    MgcReal* afCy = kSys.NewVector(m_iSegments+1);
    MgcReal* afCz = kSys.NewVector(m_iSegments+1);
    for (i = 0; i <= m_iSegments; i++)
    {
        afCx[i] = m_akC[i].x;
        afCy[i] = m_akC[i].y;
        afCz[i] = m_akC[i].z;
    }
    kSys.Solve(m_iSegments+1,aafMat,afCx);

    // reset matrix for next system
    aafMat[0][0] = 1.0;
    aafMat[0][m_iSegments] = -1.0;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        aafMat[i][i-1] = afDt[i-1];
        aafMat[i][i  ] = 2.0*(afDt[i-1] + afDt[i]);
        aafMat[i][i+1] = afDt[i];
    }
    aafMat[m_iSegments][m_iSegments-1] = afDt[m_iSegments-1];
    aafMat[m_iSegments][0] = 2.0*(afDt[m_iSegments-1] + afDt[0]);
    aafMat[m_iSegments][1] = afDt[0];

    kSys.Solve(m_iSegments+1,aafMat,afCy);

    // reset matrix for next system
    aafMat[0][0] = 1.0;
    aafMat[0][m_iSegments] = -1.0;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        aafMat[i][i-1] = afDt[i-1];
        aafMat[i][i  ] = 2.0*(afDt[i-1] + afDt[i]);
        aafMat[i][i+1] = afDt[i];
    }
    aafMat[m_iSegments][m_iSegments-1] = afDt[m_iSegments-1];
    aafMat[m_iSegments][0] = 2.0*(afDt[m_iSegments-1] + afDt[0]);
    aafMat[m_iSegments][1] = afDt[0];

    kSys.Solve(m_iSegments+1,aafMat,afCz);

    for (i = 0; i <= m_iSegments; i++)
    {
        m_akC[i].x = afCx[i];
        m_akC[i].y = afCy[i];
        m_akC[i].z = afCz[i];
    }

    const MgcReal fOneThird = 1.0/3.0;
    m_akB = new MgcVector3[m_iSegments];
    m_akD = new MgcVector3[m_iSegments];
    for (i = 0; i < m_iSegments; i++)
    {
        fInv0 = 1.0/afDt[i];
        m_akB[i] = fInv0*(m_akA[i+1] - m_akA[i]) - fOneThird*(m_akC[i+1] +
            2.0*m_akC[i])*afDt[i];
        m_akD[i] = fOneThird*fInv0*(m_akC[i+1] - m_akC[i]);
    }

    kSys.DeleteMatrix(m_iSegments+1,aafMat);
    delete[] afDt;
    delete[] afCx;
    delete[] afCy;
    delete[] afCz;
}
//-----------------------------------------------------------------------------
MgcVector3 MgcNaturalSpline3::GetPosition (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    MgcVector3 kResult = m_akA[iKey] + fDt*(m_akB[iKey] + fDt*(
        m_akC[iKey] + fDt*m_akD[iKey]));

    return kResult;
}
//---------------------------------------------------------------------------
MgcVector3 MgcNaturalSpline3::GetFirstDerivative (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    MgcVector3 kResult = m_akB[iKey] + fDt*(2.0*m_akC[iKey] + 3.0*fDt*
        m_akD[iKey]);

    return kResult;
}
//---------------------------------------------------------------------------
MgcVector3 MgcNaturalSpline3::GetSecondDerivative (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    MgcVector3 kResult = 2.0*m_akC[iKey] + 6.0*fDt*m_akD[iKey];

    return kResult;
}
//---------------------------------------------------------------------------
MgcVector3 MgcNaturalSpline3::GetThirdDerivative (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    MgcVector3 kResult = 6.0*m_akD[iKey];

    return kResult;
}
//---------------------------------------------------------------------------
MgcReal MgcNaturalSpline3::GetSpeed (int iKey, MgcReal fTime) const
{
    MgcVector3 kVelocity = m_akB[iKey] + fTime*(2.0*m_akC[iKey] +
        3.0*fTime*m_akD[iKey]);
    return kVelocity.Length();
}
//-----------------------------------------------------------------------------
MgcReal MgcNaturalSpline3::GetLength (int iKey, MgcReal fT0, MgcReal fT1) const
{
    class ThisPlusKey
    {
    public:
        ThisPlusKey (const MgcNaturalSpline3* pkThis, int iKey)
            : m_pkThis(pkThis), m_iKey(iKey) { /**/ }

        const MgcNaturalSpline3* m_pkThis;
        int m_iKey;
    };

    ThisPlusKey kData(this,iKey);

    return MgcIntegrate::RombergIntegral(fT0,fT1,GetSpeedWithData,
        (void*)&kData);
}
//-----------------------------------------------------------------------------
MgcReal MgcNaturalSpline3::GetVariation (int iKey, MgcReal fT0,
    MgcReal fT1, const MgcVector3& rkA, const MgcVector3& rkB) const
{
    MgcPolynomial kXPoly(3);
    kXPoly[0] = m_akA[iKey].x;
    kXPoly[1] = m_akB[iKey].x;
    kXPoly[2] = m_akC[iKey].x;
    kXPoly[3] = m_akD[iKey].x;

    MgcPolynomial kYPoly(3);
    kYPoly[0] = m_akA[iKey].y;
    kYPoly[1] = m_akB[iKey].y;
    kYPoly[2] = m_akC[iKey].y;
    kYPoly[3] = m_akD[iKey].y;

    MgcPolynomial kZPoly(3);
    kZPoly[0] = m_akA[iKey].z;
    kZPoly[1] = m_akB[iKey].z;
    kZPoly[2] = m_akC[iKey].z;
    kZPoly[3] = m_akD[iKey].z;

    // construct line segment A + t*B
    MgcPolynomial kLx(1), kLy(1), kLz(1);
    kLx[0] = rkA.x;
    kLx[1] = rkB.x;
    kLy[0] = rkA.y;
    kLy[1] = rkB.y;
    kLz[0] = rkA.z;
    kLz[1] = rkB.z;

    // compute |X(t) - L(t)|^2
    MgcPolynomial kDx = kXPoly - kLx;
    MgcPolynomial kDy = kYPoly - kLy;
    MgcPolynomial kDz = kZPoly - kLz;
    MgcPolynomial kNormSqr = kDx*kDx + kDy*kDy + kDz*kDz;

    // compute indefinite integral of |X(t)-L(t)|^2
    MgcPolynomial kIntegral(kNormSqr.GetDegree()+1);
    kIntegral[0] = 0.0;
    for (int i = 1; i <= kIntegral.GetDegree(); i++)
        kIntegral[i] = kNormSqr[i-1]/i;

    // compute definite Integral(t0,t1,|X(t)-L(t)|^2)
    MgcReal fResult = kIntegral(fT1) - kIntegral(fT0);
    return fResult;
}
//---------------------------------------------------------------------------