wmlintrbox3sph3.cpp

3D Game Engine Design Source Code非常棒

    // extents.  The function will still work if it is not, but it needs to be
    // false then, to avoid some checks that assume that y is within the
    // extent.  This function checks the edge that the region is above, as
    // well as does a "face region" check on the face it is heading towards.

    Real fDx = fCx - fEx;
    Real fDz = fCz - fEz;
    Real fRSqr = fRadius*fRadius;

    if ( bAboveEdge )
    {
        Real fDiff = fDx*fDx + fDz*fDz - fRSqr;
        if ( fDiff <= (Real)0.0 )
        {
            // circle is already intersecting the box
            rfTFirst = (Real)0.0;
            return -1;
        }
    }

    Real fDot = fVx*fDx + fVz*fDz;
    if ( fDot >= (Real)0.0 )
    {
        // circle not moving towards box
        return 0;
    }

    // The value fKross splits the region of interest along the edge in the
    // middle of that region.
    Real fKross = fVz*fDx - fVx*fDz; 
    if ( fKross >= (Real)0.0 )
    {
        // sphere moving towards +z face
        if ( fVx >= (Real)0.0 )
        {
            // passed corner, moving away from box
            return 0;
        }

        // Intersection with x-z edge.  If there is trouble with objects that
        // "scrape" the surface (velocity perpendicular to face normal, and
        // point of contact with a radius direction parallel to the face
        // normal), this check may need to be a little more inclusive (small
        // tolerance due to floating point errors) as the edge check needs
        // to get "scraping" objects (as they hit the edge with the point)
        // and the face region check will not catch it because the object is
        // not moving towards the face.
        if ( fKross <= -fRadius*fVx )
        {
            return FindJustEdgeIntersection(fCy,fEz,fEy,fEx,fDz,fDx,fVz,fVy,
                fVx,fRadius,rfTFirst,rfIz,rfIy,rfIx);
        }

        // now, check the face of z for intersections
        return FindFaceRegionIntersection(fEx,fEy,fEz,fCx,fCy,fCz,fVx,fVy,
            fVz,rfTFirst,fRadius,rfIx,rfIy,rfIz,false);
    }
    else
    {
        // sphere moving towards +x face
        if ( fVz >= (Real)0.0 )
        {
            // passed corner, moving away from box
            return 0;
        }

        // Check intersection with x-z edge.  See the note above above
        // "scraping" objects.
        if ( fKross >= fRadius*fVz )
        {
            // possible edge/vertex intersection
            return FindJustEdgeIntersection(fCy,fEx,fEy,fEz,fDx,fDz,fVx,fVy,
                fVz,fRadius,rfTFirst,rfIx,rfIy,rfIz);
        }

        // now, check the face of x for intersections
        return FindFaceRegionIntersection(fEz,fEy,fEx,fCz,fCy,fCx,fVz,fVy,
            fVx,rfTFirst,fRadius,rfIz,rfIy,rfIx,false);
    }
}
//----------------------------------------------------------------------------
template <class Real>
static int FindVertexRegionIntersection (Real fEx, Real fEy,  Real fEz,
    Real fCx, Real fCy, Real fCz, Real fVx, Real fVy, Real fVz,
    Real& rfTFirst, Real fRadius, Real& rfIx, Real& rfIy, Real& rfIz)
{
    // assumes the sphere is above the vertex +Ex, +Ey, +Ez

    Real fDx = fCx - fEx;
    Real fDy = fCy - fEy;
    Real fDz = fCz - fEz;
    Real fRSqr = fRadius*fRadius;
    Real fDiff = fDx*fDx + fDy*fDy + fDz*fDz - fRSqr;
    if ( fDiff <= (Real)0.0 )
    {
        // sphere is already intersecting the box
        rfTFirst = (Real)0.0;
        return -1;
    }

    if ( fVx*fDx + fVy*fDy + fVz*fDz >= (Real)0.0 )
    {
        // sphere not moving towards box
        return 0;
    }

    // The box can be divided up into 3 regions, which simplifies checking to
    // see what the sphere hits.  The regions are divided by which edge
    // (formed by the vertex and some axis) is closest to the velocity
    // vector.
    //
    // To check if it hits the vertex, look at the edge (E) it is going
    // towards.  Create a plane formed by the other two edges (with E as the
    // plane normal) with the vertex at the origin.  The intercept along an
    // axis, in that plane, of the line formed by the sphere center and the
    // velocity as its direction, will be fCrossAxis/fVEdge.  Thus, the
    // distance from the origin to the point in the plane that that line from
    // the sphere in the velocity direction crosses will be the squared sum
    // of those two intercepts.  If that sum is less than the radius squared,
    // then the sphere will hit the vertex otherwise, it will continue on
    // past the vertex.  If it misses, since it is known which edge the box
    // is near, the find edge region test can be used.
    //
    // Also, due to the constrained conditions, only those six cases (two for
    // each region, since fCrossEdge can be + or -) of signs of fCross values
    // can occur.
    //
    // The 3rd case will also pick up cases where D = V, causing a ZERO cross.

    Real fCrossX = fVy*fDz - fVz*fDy;
    Real fCrossY = fVx*fDz - fVz*fDx;
    Real fCrossZ = fVy*fDx - fVx*fDy;
    Real fCrX2 = fCrossX*fCrossX;
    Real fCrY2 = fCrossY*fCrossY;
    Real fCrZ2 = fCrossZ*fCrossZ;
    Real fVx2 = fVx*fVx;
    Real fVy2 = fVy*fVy;
    Real fVz2 = fVz*fVz;

    // Intersection with the vertex?
    if ( fCrossY < (Real)0.0
    &&   fCrossZ >= (Real)0.0
    &&   fCrY2 + fCrZ2 <= fRSqr*fVx2
    
    ||   fCrossZ < (Real)0.0
    &&   fCrossX < (Real)0.0
    &&   fCrX2 + fCrZ2 <= fRSqr*fVy2
    
    ||   fCrossY >= (Real)0.0
    &&   fCrossX >= 0
    &&   fCrX2 + fCrY2 <= fRSqr*fVz2 )
    {
        // standard line-sphere intersection
        rfTFirst = GetVertexIntersection(fDx,fDy,fDz,fVx,fVy,fVz,
            fRadius*fRadius);
        rfIx = rfTFirst*fVx + fCx;
        rfIy = rfTFirst*fVy + fCy;
        rfIz = rfTFirst*fVz + fCz;
        return 1;
    }
    else if ( fCrossY < (Real)0.0 && fCrossZ >= (Real)0.0 )
    {
        // x edge region, check y,z planes
        return FindEdgeRegionIntersection(fEy,fEx,fEz,fCy,fCx,fCz,fVy,fVx,
            fVz,rfTFirst,fRadius,rfIy,rfIx,rfIz,false);
    }
    else if ( fCrossZ < (Real)0.0 && fCrossX < (Real)0.0 )
    {
        // y edge region, check x,z planes
        return FindEdgeRegionIntersection(fEx,fEy,fEz,fCx,fCy,fCz,fVx,fVy,
            fVz,rfTFirst,fRadius,rfIx,rfIy,rfIz,false);
    }
    else // ( fCrossY >= 0 && fCrossX >= 0 )
    {
        // z edge region, check x,y planes
        return FindEdgeRegionIntersection(fEx,fEz,fEy,fCx,fCz,fCy,fVx,fVz,
            fVy,rfTFirst,fRadius,rfIx,rfIz,rfIy,false);        
    }
}
//----------------------------------------------------------------------------
template <class Real>
bool Wml::FindIntersection (const Box3<Real>& rkBox,
    const Vector3<Real>& rkBoxVelocity, const Sphere3<Real>& rkSphere,
    const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
    int& riQuantity, Vector3<Real>& rkP)
{
    // Find intersections relative to the coordinate system of the box.
    // The sphere is transformed to the box coordinates and the velocity of
    // the sphere is relative to the box.
    Vector3<Real> kCDiff = rkSphere.Center() - rkBox.Center();
    Vector3<Real> kVel = rkSphVelocity - rkBoxVelocity;
    Real fAx = kCDiff.Dot(rkBox.Axis(0));
    Real fAy = kCDiff.Dot(rkBox.Axis(1));
    Real fAz = kCDiff.Dot(rkBox.Axis(2));
    Real fVx = kVel.Dot(rkBox.Axis(0));
    Real fVy = kVel.Dot(rkBox.Axis(1));
    Real fVz = kVel.Dot(rkBox.Axis(2));

    // flip coordinate frame into the first octant
    int iSignX = 1;
    if ( fAx < (Real)0.0 )
    {
        fAx = -fAx;
        fVx = -fVx;
        iSignX = -1;
    }

    int iSignY = 1;
    if ( fAy < (Real)0.0 )
    {
        fAy = -fAy;
        fVy = -fVy;
        iSignY = -1;
    }

    int iSignZ = 1;
    if ( fAz < (Real)0.0 )
    {
        fAz = -fAz;
        fVz = -fVz;
        iSignZ = -1;
    }

    // intersection coordinates
    Real fIx, fIy, fIz;
    int iRetVal;

    if ( fAx <= rkBox.Extent(0) )
    {
        if ( fAy <= rkBox.Extent(1) )
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // sphere center inside box
                rfTFirst = (Real)0.0;
                riQuantity = 0;
                return false;
            }
            else
            {
                // sphere above face on axis Z
                iRetVal = FindFaceRegionIntersection(rkBox.Extent(0), 
                    rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
                    rfTFirst,rkSphere.Radius(),fIx,fIy,fIz,true);     
            }
        }
        else
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // sphere above face on axis Y
                iRetVal = FindFaceRegionIntersection(rkBox.Extent(0), 
                    rkBox.Extent(2),rkBox.Extent(1),fAx,fAz,fAy,fVx,fVz,fVy,
                    rfTFirst,rkSphere.Radius(),fIx,fIz,fIy,true);   
            }
            else
            {
                // sphere is above the edge formed by faces y and z
                iRetVal = FindEdgeRegionIntersection(rkBox.Extent(1), 
                    rkBox.Extent(0),rkBox.Extent(2),fAy,fAx,fAz,fVy,fVx,fVz,
                    rfTFirst,rkSphere.Radius(),fIy,fIx,fIz,true);
            }
        }
    }
    else
    {
        if ( fAy <= rkBox.Extent(1) )
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // sphere above face on axis X
                iRetVal = FindFaceRegionIntersection(rkBox.Extent(1),
                    rkBox.Extent(2),rkBox.Extent(0),fAy,fAz,fAx,fVy,fVz,fVx,
                    rfTFirst,rkSphere.Radius(),fIy,fIz,fIx,true);
            }
            else
            {
                // sphere is above the edge formed by faces x and z
                iRetVal = FindEdgeRegionIntersection(rkBox.Extent(0), 
                    rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
                    rfTFirst,rkSphere.Radius(),fIx,fIy,fIz,true);
            }
        }
        else
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // sphere is above the edge formed by faces x and y
                iRetVal = FindEdgeRegionIntersection(rkBox.Extent(0), 
                    rkBox.Extent(2),rkBox.Extent(1),fAx,fAz,fAy,fVx,fVz,fVy,
                    rfTFirst,rkSphere.Radius(),fIx,fIz,fIy,true);
            }
            else
            {
                // sphere is above the corner formed by faces x,y,z
                iRetVal = FindVertexRegionIntersection(rkBox.Extent(0),
                    rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
                    rfTFirst,rkSphere.Radius(),fIx,fIy,fIz);
            }
        }
    }

    if ( iRetVal != 1 || rfTFirst > fTMax )
    {
        riQuantity = 0;
        return false;
    }

    // calculate actual intersection (move point back into world coordinates)
    riQuantity = 1;
    rkP = rkBox.Center() + (iSignX*fIx)*rkBox.Axis(0) +
        (iSignY*fIy)*rkBox.Axis(1) + (iSignZ*fIz)*rkBox.Axis(2);
    return true;
}
//----------------------------------------------------------------------------

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

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