wmlintrtri2tri2.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000-2003.  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 "WmlIntrTri2Tri2.h"
#include "WmlIntrInterval.h"
using namespace Wml;

//----------------------------------------------------------------------------
// stationary triangles
//----------------------------------------------------------------------------
template <class Real>
static int WhichSide (const Vector2<Real> akV[3], const Vector2<Real>& rkP,
    const Vector2<Real>& rkD)
{
    // Vertices are projected to the form P+t*D.  Return value is +1 if all
    // t > 0, -1 if all t < 0, 0 otherwise, in which case the line splits the
    // triangle.

    int iPositive = 0, iNegative = 0, iZero = 0;

    for (int i = 0; i < 3; i++)
    {
        Real fT = rkD.Dot(akV[i] - rkP);
        if ( fT > 0.0f )
            iPositive++;
        else if ( fT < 0.0f )
            iNegative++;
        else
            iZero++;

        if ( iPositive > 0 && iNegative > 0 )
            return 0;
    }

    return ( iZero == 0 ? ( iPositive > 0 ? 1 : -1 ) : 0 );
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::TestIntersection (const Triangle2<Real>& rkTri0,
    const Triangle2<Real>& rkTri1)
{
    Vector2<Real> akV0[3] =
    {
        rkTri0.Origin(),
        rkTri0.Origin() + rkTri0.Edge0(),
        rkTri0.Origin() + rkTri0.Edge1()
    };

    Vector2<Real> akV1[3] =
    {
        rkTri1.Origin(),
        rkTri1.Origin() + rkTri1.Edge0(),
        rkTri1.Origin() + rkTri1.Edge1()
    };

    int iI0, iI1;
    Vector2<Real> kD;

    // test edges of V0-triangle for separation
    for (iI0 = 0, iI1 = 2; iI0 < 3; iI1 = iI0, iI0++)
    {
        // test axis V0[i1] + t*perp(V0[i0]-V0[i1]), perp(x,y) = (y,-x)
        kD.X() = akV0[iI0].Y() - akV0[iI1].Y();
        kD.Y() = akV0[iI1].X() - akV0[iI0].X();
        if ( WhichSide(akV1,akV0[iI1],kD) > 0 )
        {
            // V1-triangle is entirely on positive side of V0-triangle
            return false;
        }
    }

    // test edges of V1-triangle for separation
    for (iI0 = 0, iI1 = 2; iI0 < 3; iI1 = iI0, iI0++)
    {
        // test axis V1[i1] + t*perp(V1[i0]-V1[i1]), perp(x,y) = (y,-x)
        kD.X() = akV1[iI0].Y() - akV1[iI1].Y();
        kD.Y() = akV1[iI1].X() - akV1[iI0].X();
        if ( WhichSide(akV0,akV1[iI1],kD) > 0 )
        {
            // V0-triangle is entirely on positive side of V1-triangle
            return false;
        }
    }

    return true;
}
//----------------------------------------------------------------------------
template <class Real>
static void ClipConvexPolygonAgainstLine (const Vector2<Real>& rkN, Real fC,
    int& riQuantity, Vector2<Real> akV[6])
{
    // The input vertices are assumed to be in counterclockwise order.  The
    // ordering is an invariant of this function.

    // test on which side of line are the vertices
    int iPositive = 0, iNegative = 0, iPIndex = -1;
    Real afTest[6];
    int i;
    for (i = 0; i < riQuantity; i++)
    {
        afTest[i] = rkN.Dot(akV[i]) - fC;
        if ( afTest[i] > (Real)0.0 )
        {
            iPositive++;
            if ( iPIndex < 0 )
                iPIndex = i;
        }
        else if ( afTest[i] < (Real)0.0 )
        {
            iNegative++;
        }
    }

    if ( iPositive > 0 )
    {
        if ( iNegative > 0 )
        {
            // line transversely intersects polygon
            Vector2<Real> akCV[6];
            int iCQuantity = 0, iCur, iPrv;
            Real fT;

            if ( iPIndex > 0 )
            {
                // first clip vertex on line
                iCur = iPIndex;
                iPrv = iCur-1;
                fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
                akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);

                // vertices on positive side of line
                while ( iCur < riQuantity && afTest[iCur] > (Real)0.0 )
                    akCV[iCQuantity++] = akV[iCur++];

                // last clip vertex on line
                if ( iCur < riQuantity )
                {
                    iPrv = iCur-1;
                }
                else
                {
                    iCur = 0;
                    iPrv = riQuantity - 1;
                }
                fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
                akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);
            }
            else  // iPIndex is 0
            {
                // vertices on positive side of line
                iCur = 0;
                while ( iCur < riQuantity && afTest[iCur] > (Real)0.0 )
                    akCV[iCQuantity++] = akV[iCur++];

                // last clip vertex on line
                iPrv = iCur-1;
                fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
                akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);

                // skip vertices on negative side
                while ( iCur < riQuantity && afTest[iCur] <= (Real)0.0 )
                    iCur++;

                // first clip vertex on line
                if ( iCur < riQuantity )
                {
                    iPrv = iCur-1;
                    fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
                    akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);

                    // vertices on positive side of line
                    while ( iCur < riQuantity && afTest[iCur] > (Real)0.0 )
                        akCV[iCQuantity++] = akV[iCur++];
                }
                else
                {
                    // iCur = 0
                    iPrv = riQuantity - 1;
                    fT = afTest[0]/(afTest[0] - afTest[iPrv]);
                    akCV[iCQuantity++] = akV[0]+fT*(akV[iPrv]-akV[0]);
                }
            }

            riQuantity = iCQuantity;
            memcpy(akV,akCV,iCQuantity*sizeof(Vector2<Real>));
        }
        // else polygon fully on positive side of line, nothing to do
    }
    else
    {
        // polygon does not intersect positive side of line, clip all
        riQuantity = 0;
    }
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::FindIntersection (const Triangle2<Real>& rkTri0,
    const Triangle2<Real>& rkTri1, int& riQuantity, Vector2<Real> akVertex[6])
{
    Vector2<Real> akV0[3] =
    {
        rkTri0.Origin(),
        rkTri0.Origin() + rkTri0.Edge0(),
        rkTri0.Origin() + rkTri0.Edge1()
    };

    // The potential intersection is initialized to the V1-triangle.  The
    // set of vertices is refined based on clipping against each edge of the
    // V0-triangle.
    riQuantity = 3;
    akVertex[0] = rkTri1.Origin();
    akVertex[1] = rkTri1.Origin() + rkTri1.Edge0();
    akVertex[2] = rkTri1.Origin() + rkTri1.Edge1();

    for (int iI1 = 2, iI2 = 0; iI2 < 3; iI1 = iI2, iI2++)
    {
        // clip against edge <V0[iI1],V0[iI2]>
        Vector2<Real> kN(akV0[iI1].Y()-akV0[iI2].Y(),
            akV0[iI2].X()-akV0[iI1].X());
        Real fC = kN.Dot(akV0[iI1]);
        ClipConvexPolygonAgainstLine(kN,fC,riQuantity,akVertex);
        if ( riQuantity == 0 )
        {
            // triangle completely clipped, no intersection occurs
            return false;
        }
    }

    return true;
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// moving triangles
//----------------------------------------------------------------------------
template <class Real>
class _Configuration
{
public:
    void ComputeTwo (const Vector2<Real> akV[3], const Vector2<Real>& rkD,
        int iI0, int iI1, int iI2);

    void ComputeThree (const Vector2<Real> akV[3], const Vector2<Real>& rkD,
        const Vector2<Real>& rkP);

    static bool NoIntersect (const _Configuration& rkCfg0,
        const _Configuration& rkCfg1, Real fTMax, Real fSpeed,
        int& riSide,  _Configuration& rkTCfg0, _Configuration& rkTCfg1,
        Real& rfTFirst, Real& rfTLast);

    static void GetIntersection (const _Configuration& rkCfg0,
        const _Configuration& rkCfg1, int iSide, const Vector2<Real> akV0[3],
        const Vector2<Real> akV1[3], int& riQuantity,
        Vector2<Real> akVertex[6]);

private:
    enum ProjectionMap
    {
        M21,  // 2 vertices map to min, 1 vertex maps to max
        M12,  // 1 vertex maps to min, 2 vertices map to max
        M11   // 1 vertex maps to min, 1 vertex maps to max
    };

    ProjectionMap m_eMap;    // how vertices map to the projection interval
    int m_aiIndex[3];        // the sorted indices of the vertices
    Real m_fMin, m_fMax;  // the interval is [min,max]
};
//----------------------------------------------------------------------------
template <class Real>
void _Configuration<Real>::ComputeTwo (const Vector2<Real> akV[3],
    const Vector2<Real>& rkD, int iI0, int iI1, int iI2)
{
    m_eMap = M12;
    m_aiIndex[0] = iI0;
    m_aiIndex[1] = iI1;
    m_aiIndex[2] = iI2;
    m_fMin = rkD.Dot(akV[iI0] - akV[iI1]);
    m_fMax = (Real)0.0;
}
//----------------------------------------------------------------------------
template <class Real>
void _Configuration<Real>::ComputeThree (const Vector2<Real> akV[3],
    const Vector2<Real>& rkD, const Vector2<Real>& rkP)
{
    Real fD0 = rkD.Dot(akV[0] - rkP);
    Real fD1 = rkD.Dot(akV[1] - rkP);
    Real fD2 = rkD.Dot(akV[2] - rkP);

    // Make sure that m_aiIndex[...] is an even permutation of (0,1,2)
    // whenever the map value is M12 or M21.  This is needed to guarantee
    // the intersection of overlapping edges is properly computed.

    if ( fD0 <= fD1 )
    {
        if ( fD1 <= fD2 )  // d0 <= d1 <= d2
        {
            if ( fD0 != fD1 )
                m_eMap = ( fD1 != fD2 ? M11 : M12 );
            else
                m_eMap = M21;

            m_aiIndex[0] = 0;
            m_aiIndex[1] = 1;
            m_aiIndex[2] = 2;
            m_fMin = fD0;
            m_fMax = fD2;
        }
        else if ( fD0 <= fD2 )  // d0 <= d2 < d1
        {
            if ( fD0 != fD2 )
            {
                m_eMap = M11;
                m_aiIndex[0] = 0;
                m_aiIndex[1] = 2;
                m_aiIndex[2] = 1;
            }
            else
            {
                m_eMap = M21;
                m_aiIndex[0] = 2;
                m_aiIndex[1] = 0;
                m_aiIndex[2] = 1;
            }

            m_fMin = fD0;
            m_fMax = fD1;
        }
        else  // d2 < d0 <= d1
        {
            m_eMap = ( fD0 != fD1 ? M12 : M11 );
            m_aiIndex[0] = 2;
            m_aiIndex[1] = 0;
            m_aiIndex[2] = 1;
            m_fMin = fD2;
            m_fMax = fD1;
        }
    }
    else
    {
        if ( fD2 <= fD1 )  // d2 <= d1 < d0
        {
            if ( fD2 != fD1 )
            {
                m_eMap = M11;
                m_aiIndex[0] = 2;
                m_aiIndex[1] = 1;
                m_aiIndex[2] = 0;
            }
            else
            {
                m_eMap = M21;
                m_aiIndex[0] = 1;
                m_aiIndex[1] = 2;
                m_aiIndex[2] = 0;
            }

            m_fMin = fD2;
            m_fMax = fD0;
        }
        else if ( fD2 <= fD0 )  // d1 < d2 <= d0
        {
            m_eMap = ( fD2 != fD0 ? M11 : M12 );
            m_aiIndex[0] = 1;
            m_aiIndex[1] = 2;
            m_aiIndex[2] = 0;
            m_fMin = fD1;
            m_fMax = fD0;
        }