wmlintrtri3box3.cpp

3D Game Engine Design Source Code非常棒

                FindContactSetCoplanarLineRect(akTriSeg,akBoxFace,riQuantity,
                    akP);
            }
        }
        else // rkTriContact.m_kMap == m3
        {
            if ( rkBoxContact.m_kMap == m2_2 )
            {
                // boxseg-triface intersection
                Vector3<Real> akBoxSeg[2];
                akBoxSeg[0] = GetPoint(rkBoxContact.m_aiIndex[6],kNewBox);
                akBoxSeg[1] = GetPoint(rkBoxContact.m_aiIndex[7],kNewBox);
                FindContactSetColinearLineTri(akBoxSeg,akNewTri,riQuantity,
                    akP);
            }
            else
            {
                // triface-boxface intersection
                Vector3<Real> akBoxFace[4];
                akBoxFace[0] = GetPoint(rkBoxContact.m_aiIndex[4],kNewBox);
                akBoxFace[1] = GetPoint(rkBoxContact.m_aiIndex[5],kNewBox);
                akBoxFace[2] = GetPoint(rkBoxContact.m_aiIndex[6],kNewBox);
                akBoxFace[3] = GetPoint(rkBoxContact.m_aiIndex[7],kNewBox);
                FindContactSetCoplanarTriRect(akNewTri,akBoxFace,riQuantity,
                    akP);
            }
        }
    }
    else // eSide == RIGHT 
    {
        // box on right of tri
        if ( rkTriContact.m_kMap == m111 || rkTriContact.m_kMap == m21 )
        {
            riQuantity = 1;
            akP[0] = akNewTri[rkTriContact.m_aiIndex[2]];
        }
        else if ( rkBoxContact.m_kMap == m1_1 )
        {
            riQuantity = 1;
            akP[0] = GetPoint(rkBoxContact.m_aiIndex[0],kNewBox);
        }
        else if ( rkTriContact.m_kMap == m12 )
        {
            if ( rkBoxContact.m_kMap == m2_2 )
            {
                // segment-segment intersection
                Vector3<Real> akTriSeg[2], akBoxSeg[2];
                akTriSeg[0] = akNewTri[rkTriContact.m_aiIndex[1]];
                akTriSeg[1] = akNewTri[rkTriContact.m_aiIndex[2]];
                akBoxSeg[0] = GetPoint(rkBoxContact.m_aiIndex[0],kNewBox);
                akBoxSeg[1] = GetPoint(rkBoxContact.m_aiIndex[1],kNewBox);
                FindContactSetLinLin(akTriSeg,akBoxSeg,riQuantity,akP);
            }
            else // rkBoxContact.m_kMap == m44
            {
                // triseg-boxface intersection
                Vector3<Real> akTriSeg[2], akBoxFace[4];
                akTriSeg[0] = akNewTri[rkTriContact.m_aiIndex[1]];
                akTriSeg[1] = akNewTri[rkTriContact.m_aiIndex[2]];
                akBoxFace[0] = GetPoint(rkBoxContact.m_aiIndex[0],kNewBox);
                akBoxFace[1] = GetPoint(rkBoxContact.m_aiIndex[1],kNewBox);
                akBoxFace[2] = GetPoint(rkBoxContact.m_aiIndex[2],kNewBox);
                FindContactSetCoplanarLineRect(akTriSeg,akBoxFace,riQuantity,
                    akP);
            }
        }
        else // rkTriContact.m_kMap == m3
        {
            if ( rkBoxContact.m_kMap == m2_2 )
            {
                // boxseg-triface intersection
                Vector3<Real> akBoxSeg[2];
                akBoxSeg[0] = GetPoint(rkBoxContact.m_aiIndex[0],kNewBox);
                akBoxSeg[1] = GetPoint(rkBoxContact.m_aiIndex[1],kNewBox);
                FindContactSetColinearLineTri(akBoxSeg,akNewTri,riQuantity,
                    akP);
            }
            else
            {
                // triface-boxface intersection
                Vector3<Real> akBoxFace[4];
                akBoxFace[0] = GetPoint(rkBoxContact.m_aiIndex[0],kNewBox);
                akBoxFace[1] = GetPoint(rkBoxContact.m_aiIndex[1],kNewBox);
                akBoxFace[2] = GetPoint(rkBoxContact.m_aiIndex[2],kNewBox);
                akBoxFace[3] = GetPoint(rkBoxContact.m_aiIndex[3],kNewBox);
                FindContactSetCoplanarTriRect(akNewTri,akBoxFace,riQuantity,
                    akP);
            }
        }
    }
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::TestIntersection (const Triangle3<Real>& rkTri,
    const Vector3<Real>& rkTriVelocity, const Box3<Real>& rkBox,
    const Vector3<Real>& rkBoxVelocity, Real fTMax, Real& rfTFirst,
    Real& rfTLast)
{
    // get triangle vertices
    Vector3<Real> akV[3] =
    {
        rkTri.Origin(),
        rkTri.Origin() + rkTri.Edge0(),
        rkTri.Origin() + rkTri.Edge1()
    };

    Real fMin0, fMax0, fMin1, fMax1, fSpeed;
    Vector3<Real> kD, akE[3];

    // process as if triangle is stationary, box is moving
    Vector3<Real> kW = rkBoxVelocity - rkTriVelocity;
    rfTFirst = (Real)0.0;
    rfTLast = Math<Real>::MAX_REAL;

    // test direction of triangle normal
    akE[0] = akV[1] - akV[0];
    akE[1] = akV[2] - akV[0];
    kD = akE[0].Cross(akE[1]);
    fMin0 = kD.Dot(akV[0]);
    fMax0 = fMin0;
    ProjectBox(kD,rkBox,fMin1,fMax1);
    fSpeed = kD.Dot(kW);
    if ( NoIntersect(fTMax,fSpeed,fMin0,fMax0,fMin1,fMax1,rfTFirst,rfTLast) )
        return false;

    // test direction of box faces
    for (int i = 0; i < 3; i++)
    {
        kD = rkBox.Axis(i);
        ProjectTriangle(kD,akV,fMin0,fMax0);
        Real fDdC = kD.Dot(rkBox.Center());
        fMin1 = fDdC - rkBox.Extent(i);
        fMax1 = fDdC + rkBox.Extent(i);
        fSpeed = kD.Dot(kW);
        if ( NoIntersect(fTMax,fSpeed,fMin0,fMax0,fMin1,fMax1,rfTFirst,
             rfTLast) )
        {
            return false;
        }
    }

    // test direction of triangle-box edge cross products
    akE[2] = akE[1] - akE[0];
    for (int i0 = 0; i0 < 3; i0++)
    {
        for (int i1 = 0; i1 < 3; i1++)
        {
            kD = akE[i0].Cross(rkBox.Axis(i1));
            ProjectTriangle(kD,akV,fMin0,fMax0);
            ProjectBox(kD,rkBox,fMin1,fMax1);
            fSpeed = kD.Dot(kW);
            if ( NoIntersect(fTMax,fSpeed,fMin0,fMax0,fMin1,fMax1,rfTFirst,
                 rfTLast) )
            {
                return false;
            }
        }
    }

    return true;
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::FindIntersection (const Triangle3<Real>& rkTri,
    const Vector3<Real>& rkTriVelocity, const Box3<Real>& rkBox,
    const Vector3<Real>& rkBoxVelocity, Real& rfTFirst, Real fTMax,
    int& riQuantity, Vector3<Real> akP[6])
{
    // get triangle vertices
    Vector3<Real> akV[3] =
    {
        rkTri.Origin(),
        rkTri.Origin() + rkTri.Edge0(),
        rkTri.Origin() + rkTri.Edge1()
    };

    // process as if triangle is stationary, box is moving
    Vector3<Real> kVel = rkBoxVelocity - rkTriVelocity;

    rfTFirst = (Real)0.0;
    Real fTLast = Math<Real>::MAX_REAL;

    ContactSide eSide = NONE;
    ContactConfig<Real> kTriContact, kBoxContact;

    // test tri-normal
    Vector3<Real> akE[3] = { akV[1]-akV[0], akV[2]-akV[1],  akV[0]-akV[2] };
    Vector3<Real> kTriNorm = akE[0].Cross(akE[1]);
    if ( !TriBoxAxisFind(kTriNorm,kVel,akV,rkBox,rfTFirst,fTLast,fTMax,eSide,
         kTriContact,kBoxContact) )
    {
        return false;
    }

    Vector3<Real> kAxis;
    int iCoplanar = -1; // triangle coplanar to no box normals
    int i0;
    for (i0 = 0; i0 < 3; i0++)
    {
        kAxis = rkBox.Axis(i0);
        if ( !TriBoxAxisFind(kAxis,kVel,akV,rkBox,rfTFirst,fTLast,fTMax,
             eSide,kTriContact,kBoxContact) )
        {
            return false;
        }

        // Test if axis is parallel to triangle normal.  The test is:
        // sin(Angle(normal,axis)) < epsilon
        Real fNdA = kTriNorm.Dot(kAxis);
        Real fNdN = kTriNorm.SquaredLength();
        Real fAdA = kAxis.SquaredLength();
        Real fSin = Math<Real>::Sqrt(Math<Real>::FAbs((Real)1.0 -
            fNdA*fNdA/(fNdN*fAdA)));
        if ( fSin < Math<Real>::EPSILON )
            iCoplanar = i0;
    }

    if ( iCoplanar == -1 )
    {
        // test triedges cross boxfaces
        for (i0 = 0; i0 < 3; i0++ )
        {
            for (int i1 = 0; i1 < 3; i1++ )
            {
                kAxis = akE[i0].Cross(rkBox.Axis(i1));
                if ( !TriBoxAxisFind(kAxis,kVel,akV,rkBox,rfTFirst,fTLast,
                     fTMax,eSide,kTriContact,kBoxContact) )
                {
                    return false;
                }
            }
        }
    }
    else
    {
        // test triedges cross coplanar box axis
        for (i0 = 0; i0 < 3; i0++)
        {
            kAxis = akE[i0].Cross(kTriNorm);
            if ( !TriBoxAxisFind(kAxis,kVel,akV,rkBox,rfTFirst,fTLast,fTMax,
                 eSide,kTriContact,kBoxContact) )
            {
                return false;
            }
        }
    }

    // test velocity cross box faces
    for (i0 = 0; i0 < 3; i0++)
    {
        kAxis = kVel.Cross(rkBox.Axis(i0));
        if ( !TriBoxAxisFind(kAxis,kVel,akV,rkBox,rfTFirst,fTLast,fTMax,
             eSide,kTriContact,kBoxContact) )
        {
            return false;
        }
    }

    if ( rfTFirst < (Real)0.0 || eSide == NONE )
        return false;

    FindContactSet(akV,rkBox,eSide,kTriContact,kBoxContact,rkTriVelocity,
        rkBoxVelocity,rfTFirst,riQuantity,akP);
    return true;
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// explicit instantiation
//----------------------------------------------------------------------------
namespace Wml
{
template WML_ITEM bool TestIntersection<float> (
    const Triangle3<float>&, const Box3<float>&);
template WML_ITEM bool FindIntersection<float> (
    const Triangle3<float>&, const Box3<float>&, int&,
    Vector3<float>[6]);
template WML_ITEM bool TestIntersection<float> (
    const Triangle3<float>&, const Vector3<float>&, const Box3<float>&,
    const Vector3<float>&, float, float&, float&);
template WML_ITEM bool FindIntersection<float> (
    const Triangle3<float>&, const Vector3<float>&, const Box3<float>&,
    const Vector3<float>&, float&, float, int&, Vector3<float>[6]);

template WML_ITEM bool TestIntersection<double> (
    const Triangle3<double>&, const Box3<double>&);
template WML_ITEM bool FindIntersection<double> (
    const Triangle3<double>&, const Box3<double>&, int&,
    Vector3<double>[6]);
template WML_ITEM bool TestIntersection<double> (
    const Triangle3<double>&, const Vector3<double>&, const Box3<double>&,
    const Vector3<double>&, double, double&, double&);
template WML_ITEM bool FindIntersection<double> (
    const Triangle3<double>&, const Vector3<double>&, const Box3<double>&,
    const Vector3<double>&, double&, double, int&, Vector3<double>[6]);
}
//----------------------------------------------------------------------------