wmlintrtri3tri3.cpp

3D Game Engine Design Source Code非常棒

}
//----------------------------------------------------------------------------
template <class Real>
static void FindContactSetTriTri (Vector3<Real> akU[3], Vector3<Real> akV[3],
    int& riQuantity, Vector3<Real> akP[6])
{
    // This function is ignorant of whether the triangles are coplanar and
    // assumes they are intersecting.

    Vector3<Real> kEdge1 = akU[1] - akU[0];
    Vector3<Real> kEdge2 = akU[2] - akU[0];
    Vector3<Real> kN = kEdge1.Cross(kEdge2);

    kEdge1 = akV[1] - akV[0];
    kEdge2 = akV[2] - akV[0];
    Vector3<Real> kM = kEdge1.Cross(kEdge2);

    Vector3<Real> kNxM = kN.Cross(kM);

    if ( kNxM.Dot(kNxM) < Math<Real>::EPSILON*(kN.Dot(kN)*kM.Dot(kM)) )
        FindContactSetCoplanarTri(akU,akV,riQuantity,akP);
    else
        FindContactSetNonCoplanarTri(akU,akV,riQuantity,akP);
}
//----------------------------------------------------------------------------
template <class Real>
static void FindContactSet (Vector3<Real> akU[3], Vector3<Real> akV[3], 
    ContactSide& reSide, ContactConfig<Real>& rkUC,
    ContactConfig<Real>& rkVC, const Vector3<Real>& /* rkVel */,
    int& riQuantity, Vector3<Real> akP[6])
{
    // TO DO.  The parameter rkVel is not used here.  Is this an error?  Or
    // did the caller already make the adjustments to the positions to
    // incorporate the motion?

    if ( reSide == RIGHT ) // V right of U
    {
        if ( rkUC.m_kMap == m21 || rkUC.m_kMap == m111 )
        {
            // triangle U touching triangle V at a single point
            riQuantity = 1;
            akP[0] = akU[2];
        }
        else if ( rkVC.m_kMap == m12 || rkVC.m_kMap == m111 )
        {
            // triangle V touching triangle U at a single point
            riQuantity = 1;
            akP[0] = akV[0];
        }
        else if ( rkUC.m_kMap == m12 )
        {
            if ( rkVC.m_kMap == m21 )
            {
                // uedge-vedge intersection
                Vector3<Real> akUEdge[2] = { akU[1], akU[2] };
                Vector3<Real> akVEdge[2] = { akV[0], akV[1] };
                FindContactSetLinLin(akUEdge,akVEdge,riQuantity,akP);
            }
            else // ( rkVC.m_kMap == m3 )
            {
                // uedge-vface intersection
                Vector3<Real> akUEdge[2] = { akU[1], akU[2] };
                FindContactSetColinearLineTri(akUEdge,akV,riQuantity,akP);
            }
        }
        else // ( rkUC.m_kMap == m3 )
        {
            if ( rkVC.m_kMap == m21 )
            {

                // uface-vedge intersection
                Vector3<Real> akVEdge[2] = { akV[0], akV[1] };
                FindContactSetColinearLineTri(akVEdge,akU,riQuantity,akP);
            } 
            else // ( rkVC.m_kMap == m3 )
            {
                // uface-vface intersection
                FindContactSetCoplanarTri(akU,akV,riQuantity,akP);
            }
        }
    }
    else if ( reSide == LEFT ) // V left of U 
    {
        if ( rkVC.m_kMap == m21 || rkVC.m_kMap == m111 )
        {
            // triangle V touching triangle U at a single point
            riQuantity = 1;
            akP[0] = akV[2];
        }
        else if ( rkUC.m_kMap == m12 || rkUC.m_kMap == m111 )
        {
            // triangle U touching triangle V at a single point
            riQuantity = 1;
            akP[0] = akU[0];
        }
        else if ( rkVC.m_kMap == m12 )
        {
            if ( rkUC.m_kMap == m21 )
            {
                // uedge-vedge intersection
                Vector3<Real> akUEdge[2] = { akU[0], akU[1] };
                Vector3<Real> akVEdge[2] = { akV[1], akV[2] };
                FindContactSetLinLin(akUEdge,akVEdge,riQuantity,akP);
            }
            else // ( rkUC.m_kMap == m3 )
            {
                // vedge-uface intersection
                Vector3<Real> akVEdge[2] = { akV[1], akV[2] };
                FindContactSetColinearLineTri(akVEdge,akU,riQuantity,akP);
            }
        }
        else // ( rkVC.m_kMap == m3 )
        {
            if ( rkUC.m_kMap == m21 )
            {
                // uedge-vface intersection
                Vector3<Real> akUEdge[2] = { akU[0], akU[1] };
                FindContactSetColinearLineTri(akUEdge,akV,riQuantity,akP);
            } 
            else // ( rkUC.m_kMap == m3 )
            {
                // uface-vface intersection
                FindContactSetCoplanarTri(akU,akV,riQuantity,akP);
            }
        }
    }
    else // ( reSide == NONE )
    {
        FindContactSetTriTri(akU,akV,riQuantity,akP);
    }
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::TestIntersection (const Triangle3<Real>& rkUTri, 
    const Vector3<Real>& kUVelocity, const Triangle3<Real>& rkVTri,
    const Vector3<Real>& kVVelocity, Real& rfTFirst, Real fTMax)
{
    Vector3<Real> akU[3] =
    {
        rkUTri.Origin(),
        rkUTri.Origin() + rkUTri.Edge0(),
        rkUTri.Origin() + rkUTri.Edge1()
    };

    Vector3<Real> akV[3] =
    {
        rkVTri.Origin(),
        rkVTri.Origin() + rkVTri.Edge0(),
        rkVTri.Origin() + rkVTri.Edge1()
    };

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

    // velocity relative to the U triangle
    Vector3<Real> kVel = kVVelocity - kUVelocity;

    // direction N
    Vector3<Real> akE[3] = { akU[1]-akU[0], akU[2]-akU[1], akU[0]-akU[2] };
    Vector3<Real> kN = akE[0].Cross(akE[1]);
    if ( !TriTriAxisTest(kVel,kN,akU,akV,rfTFirst,fTLast,fTMax) )
        return false;

    // direction M
    Vector3<Real> akF[3] = { akV[1]-akV[0], akV[2]-akV[1], akV[0]-akV[2] };
    Vector3<Real> kM = akF[0].Cross(akF[1]);

    Vector3<Real> kNxM = kN.Cross(kM);
    Vector3<Real> kDir;
    int i0, i1;

    if ( kNxM.Dot(kNxM) >= Math<Real>::EPSILON*(kN.Dot(kN)*kM.Dot(kM)) )
    {
        // triangles are not parallel

        // direction M
        if ( !TriTriAxisTest(kVel,kM,akU,akV,rfTFirst,fTLast,fTMax) )
            return false;

        // directions E[i0]xF[i1]
        for (i1 = 0; i1 < 3; i1++)
        {
            for (i0 = 0; i0 < 3; i0++)
            {
                kDir = akE[i0].Cross(akF[i1]);
                if ( !TriTriAxisTest(kVel,kDir,akU,akV,rfTFirst,fTLast,
                     fTMax) )
                {
                    return false;
                }
            }
        }
    }
    else  // triangles are parallel (and, in fact, coplanar)
    {
        // directions NxE[i0]
        for (i0 = 0; i0 < 3; i0++)
        {
            kDir = kN.Cross(akE[i0]);
            if ( !TriTriAxisTest(kVel,kDir,akU,akV,rfTFirst,fTLast,fTMax) )
                return false;
        }

        // directions NxF[i1]
        for (i1 = 0; i1 < 3; i1++)
        {
            kDir = kM.Cross(akF[i1]);
            if ( !TriTriAxisTest(kVel,kDir,akU,akV,rfTFirst,fTLast,fTMax) )
                return false;
        }
    }

    return true;
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::FindIntersection (const Triangle3<Real>& rkUTri,
    const Vector3<Real>& rkUVelocity, const Triangle3<Real>& rkVTri,
    const Vector3<Real>& rkVVelocity, Real& rfTFirst, Real fTMax,
    int& riQuantity, Vector3<Real> akP[6])
{
    Vector3<Real> akU[3] =
    {
        rkUTri.Origin(),
        rkUTri.Origin() + rkUTri.Edge0(),
        rkUTri.Origin() + rkUTri.Edge1()
    };

    Vector3<Real> akV[3] =
    {
        rkVTri.Origin(),
        rkVTri.Origin() + rkVTri.Edge0(),
        rkVTri.Origin() + rkVTri.Edge1()
    };

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

    // velocity relative to the U triangle
    Vector3<Real> kVel = rkVVelocity - rkUVelocity;

    ContactSide eSide = NONE;
    ContactConfig<Real> kTUC, kTVC;

    // direction N
    Vector3<Real> akE[3] = { akU[1]-akU[0], akU[2]-akU[1], akU[0]-akU[2] };
    Vector3<Real> kN = akE[0].Cross(akE[1]);
    if ( !TriTriAxisFind(kVel,kN,akU,akV,eSide,kTUC,kTVC,rfTFirst,fTLast,
         fTMax) )
    {
        return false;
    }

    // direction M
    Vector3<Real> akF[3] = { akV[1]-akV[0], akV[2]-akV[1], akV[0]-akV[2] };
    Vector3<Real> kM = akF[0].Cross(akF[1]);

    Vector3<Real> kNxM = kN.Cross(kM);
    Vector3<Real> kDir;
    int i0, i1;

    if ( kNxM.Dot(kNxM) >= Math<Real>::EPSILON*(kN.Dot(kN)*kM.Dot(kM)) )
    {
        // triangles are not parallel

        // direction M
        if ( !TriTriAxisFind(kVel,kM,akU,akV,eSide,kTUC,kTVC,rfTFirst,fTLast,
             fTMax) )
        {
            return false;
        }

        // directions E[i0]xF[i1]
        for (i1 = 0; i1 < 3; i1++)
        {
            for (i0 = 0; i0 < 3; i0++)
            {
                kDir = akE[i0].Cross(akF[i1]);
                if ( !TriTriAxisFind(kVel,kDir,akU,akV,eSide,kTUC,kTVC,
                     rfTFirst,fTLast,fTMax) )
                {
                    return false;
                }
            }
        }
    }
    else  // triangles are parallel (and, in fact, coplanar)
    {
        // directions NxE[i0]
        for (i0 = 0; i0 < 3; i0++)
        {
            kDir = kN.Cross(akE[i0]);
            if ( !TriTriAxisFind(kVel,kDir,akU,akV,eSide,kTUC,kTVC,rfTFirst,
                 fTLast,fTMax) )
            {
                return 0;
            }
        }

        // directions NxF[i1]
        for (i1 = 0; i1 < 3; i1++)
        {
            kDir = kM.Cross(akF[i1]);
            if ( !TriTriAxisFind(kVel,kDir,akU,akV,eSide,kTUC,kTVC,rfTFirst,
                 fTLast,fTMax) )
            {
                return 0;
            }
        }
    }
    
    if ( rfTFirst <= (Real)0.0 )
        return false;

    // adjust U and V for first time of contact before finding contact set
    Vector3<Real> akNewU[3], akNewV[3];
    for (i0 = 0; i0 < 3; i0++)
    {
        akNewU[i0] = akU[i0] + rfTFirst*rkUVelocity;
        akNewV[i0] = akV[i0] + rfTFirst*rkVVelocity;
    }

    FindContactSet(akNewU,akNewV,eSide,kTUC,kTVC,kVel,riQuantity,akP);
    return true;
}
//----------------------------------------------------------------------------

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

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