wmlintrbox3sph3.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// http://www.wild-magic.com
// Copyright (c) 2003.  All Rights Reserved
//
// The Wild Magic Library (WML) source code is supplied under the terms of
// the license agreement http://www.magic-software.com/License/WildMagic.pdf
// and may not be copied or disclosed except in accordance with the terms of
// that agreement.

#include "WmlIntrBox3Sph3.h"
#include "WmlIntrUtility3.h"
using namespace Wml;

//----------------------------------------------------------------------------
template <class Real>
bool Wml::TestIntersection (const Box3<Real>& rkBox,
    const Sphere3<Real>& rkSphere)
{
    // Test for intersection in the coordinate system of the box by
    // transforming the sphere into that coordinate system.
    Vector3<Real> kCDiff = rkSphere.Center() - rkBox.Center();

    Real fAx = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(0)));
    Real fAy = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(1)));
    Real fAz = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(2)));
    Real fDx = fAx - rkBox.Extent(0);
    Real fDy = fAy - rkBox.Extent(1);
    Real fDz = fAz - rkBox.Extent(2);

    if ( fAx <= rkBox.Extent(0) )
    {
        if ( fAy <= rkBox.Extent(1) )
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // sphere center inside box
                return true;
            }
            else
            {
                // potential sphere-face intersection with face z
                return fDz <= rkSphere.Radius();
            }
        }
        else
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // potential sphere-face intersection with face y
                return fDy <= rkSphere.Radius();
            }
            else
            {
                // potential sphere-edge intersection with edge formed
                // by faces y and z
                Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
                return fDy*fDy + fDz*fDz <= fRSqr;
            }
        }
    }
    else
    {
        if ( fAy <= rkBox.Extent(1) )
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // potential sphere-face intersection with face x
                return fDx <= rkSphere.Radius();
            }
            else
            {
                // potential sphere-edge intersection with edge formed
                // by faces x and z
                Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
                return fDx*fDx + fDz*fDz <= fRSqr;
            }
        }
        else
        {
            if ( fAz <= rkBox.Extent(2) )
            {
                // potential sphere-edge intersection with edge formed
                // by faces x and y
                Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
                return fDx*fDx + fDy*fDy <= fRSqr;
            }
            else
            {
                // potential sphere-vertex intersection at corner formed
                // by faces x,y,z
                Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
                return fDx*fDx + fDy*fDy + fDz*fDz <= fRSqr;
            }
        }
    }
}
//----------------------------------------------------------------------------
template <class Real>
static Real GetVertexIntersection (Real fDx, Real fDy, Real fDz, Real fVx,
    Real fVy, Real fVz, Real fRSqr)
{
    // Finds the time of a 3D line-sphere intersection between a line
    // P = Dt, where P = (fDx, fDy, fDz) and D = (fVx, fVy, fVz) and
    // a sphere of radius^2 fRSqr.  Note: only valid if there is, in fact,
    // an intersection.

    Real fVSqr = fVx*fVx + fVy*fVy + fVz*fVz;
    Real fDot = fDx*fVx + fDy*fVy + fDz*fVz;
    Real fDiff = fDx*fDx + fDy*fDy + fDz*fDz - fRSqr;
    Real fInv = Math<Real>::InvSqrt(Math<Real>::FAbs(fDot*fDot-fVSqr*fDiff));
    return fDiff*fInv/(1.0f-fDot*fInv);
}
//----------------------------------------------------------------------------
template <class Real>
static Real GetEdgeIntersection (Real fDx, Real fDz, Real fVx, Real fVz,
    Real fVSqr, Real fRSqr)
{
    // Finds the time of a 2D line-circle intersection between a line
    // P = Dt where P = (fDx,fDz) and D = (fVx, fVz) and a circle of radius^2
    // fRSqr.  Note: only valid if there is, in fact, an intersection.

    Real fDot = fVx*fDx + fVz*fDz;
    Real fDiff = fDx*fDx + fDz*fDz - fRSqr;
    Real fInv = Math<Real>::InvSqrt(Math<Real>::FAbs(fDot*fDot-fVSqr*fDiff));
    return fDiff*fInv/(1.0f-fDot*fInv);
}
//----------------------------------------------------------------------------
template <class Real>
static int FindFaceRegionIntersection (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, bool bAboveFace)
{
    // Returns when and whether a sphere in the region above face +Z
    // intersects face +Z or any of its vertices or edges.  The input
    // bAboveFace is true when the x and y coordinates are within the x and y
    // extents.  The function will still work if they are not, but it needs
    // to be false then, to avoid some checks that assume that x and y are
    // within the extents.  This function checks face z, and the vertex and
    // two edges that the velocity is headed towards on the face.

    // check for already intersecting if above face
    if ( fCz <= fEz + fRadius && bAboveFace )
    {
        rfTFirst = (Real)0.0;
        return -1;
    }

    // check for easy out (moving away on Z axis)
    if ( fVz >= (Real)0.0 )
        return 0;

    Real fRSqr = fRadius*fRadius;

    Real fVSqrX = fVz*fVz + fVx*fVx;
    Real fVSqrY = fVz*fVz + fVy*fVy;
    Real fDx, fDy, fDz = fCz - fEz;
    Real fCrossX, fCrossY;
    int iXSign, iYSign;

    // This determines which way the box is heading and finds the values of
    // CrossX and CrossY which are positive if the sphere center will not
    // pass through the box.  Then it is only necessary to check two edges,
    // the face and the vertex for intersection.

    if ( fVx >= (Real)0.0 )
    {
        iXSign = 1;
        fDx = fCx - fEx;
        fCrossX = fVx*fDz - fVz*fDx;
    }
    else
    {
        iXSign = -1;
        fDx = fCx + fEx;
        fCrossX = fVz*fDx - fVx*fDz;
    }

    if ( fVy >= (Real)0.0 )
    {
        iYSign = 1;
        fDy = fCy - fEy;
        fCrossY = fVy*fDz - fVz*fDy;
    }
    else
    {
        iYSign = -1;
        fDy = fCy + fEy;
        fCrossY = fVz*fDy - fVy*fDz;
    }

    // does the circle intersect along the x edge?
    if ( fCrossX > fRadius*fVx*iXSign )
    {
        if ( fCrossX*fCrossX > fRSqr*fVSqrX )
        {
            // sphere overshoots box on the x-axis (either side)
            return 0;
        }

        // does the circle hit the y edge?
        if ( fCrossY > fRadius*fVy*iYSign )
        {
            // potential vertex intersection
            if ( fCrossY*fCrossY > fRSqr*fVSqrY )
            {
                // sphere overshoots box on the y-axis (either side)
                return 0;
            }

            Vector3<Real> kVel(fVx,fVy,fVz);
            Vector3<Real> kD(fDx,fDy,fDz);
            Vector3<Real> kCross = kD.Cross(kVel);
            if ( kCross.SquaredLength() > fRSqr*kVel.SquaredLength() )
            {
                // sphere overshoots the box on the corner
                return 0;
            }

            rfTFirst = GetVertexIntersection(fDx,fDy,fDz,fVx,fVy,fVz,fRSqr);
            rfIx = fEx*iXSign;
            rfIy = fEy*iYSign;
        }
        else
        {
            // x-edge intersection
            rfTFirst = GetEdgeIntersection(fDx,fDz,fVx,fVz,fVSqrX,fRSqr);
            rfIx = fEx*iXSign;
            rfIy = fCy + fVy*rfTFirst;
        }
    }
    else
    {
        // does the circle hit the y edge?
        if ( fCrossY > fRadius*fVy*iYSign )
        {
            // potential y-edge intersection

            if ( fCrossY*fCrossY > fRSqr*fVSqrY )
            {
                // sphere overshoots box on the y-axis (either side)
                return 0;
            }

            rfTFirst = GetEdgeIntersection(fDy,fDz,fVy,fVz,fVSqrY,fRSqr);
            rfIx = fCx + fVx*rfTFirst;
            rfIy = fEy*iYSign;
        }
        else
        {
            // face intersection (easy)
            rfTFirst = (-fDz + fRadius)/fVz;
            rfIx = rfTFirst*fVx + fCx;
            rfIy = rfTFirst*fVy + fCy;
        }
    }

    // z coordinate of any intersection must be the face of z
    rfIz = fEz;
    return 1;
}
//----------------------------------------------------------------------------
template <class Real>
static int FindJustEdgeIntersection (Real fCy, Real fEx, Real fEy, Real fEz,
    Real fDx, Real fDz, Real fVx, Real fVy, Real fVz, Real fRadius,
    Real& rfTFirst, Real& rfIx, Real& rfIy, Real& rfIz)
{
    // Finds the intersection of a point fDx and fDz away from an edge with
    // direction y.  The sphere is at a point fCy, and the edge is at the
    // point fEx.  Checks the edge and the vertex the velocity is heading
    // towards.

    Real fRSqr = fRadius*fRadius;
    Real fDy, fCrossZ, fCrossX;  // possible edge/vertex intersection
    int iSignY;

    // Depending on the sign of Vy, pick the vertex that the velocity is
    // heading towards on the edge, as well as creating fCrossX and fCrossZ
    // such that their sign will always be positive if the sphere center goes
    // over that edge.

    if ( fVy >= (Real)0.0 )
    {
        iSignY = 1;
        fDy = fCy - fEy;
        fCrossZ = fDx*fVy - fDy*fVx;
        fCrossX = fDz*fVy - fDy*fVz;
    }
    else
    {
        iSignY = -1;
        fDy = fCy + fEy;
        fCrossZ = fDy*fVx - fDx*fVy;
        fCrossX = fDy*fVz - fDz*fVy;
    }

    // Check where on edge this intersection will occur
    if ( fCrossZ >= (Real)0.0 && fCrossX >= (Real)0.0 && 
         fCrossX*fCrossX + fCrossZ*fCrossZ > fVy*fVy*fRadius*fRadius )
    {
        // sphere potentially intersects with vertex
        Vector3<Real> kVel(fVx,fVy,fVz);
        Vector3<Real> kD(fDx,fDy,fDz);
        Vector3<Real> kCross = kD.Cross(kVel);
        if ( kCross.SquaredLength() > fRSqr*kVel.SquaredLength() )
        {
            // sphere overshoots the box on the vertex
            return 0;
        }

        // sphere actually does intersect the vertex
        rfTFirst = GetVertexIntersection(fDx,fDy,fDz,fVx,fVy,fVz,fRSqr);
        rfIx = fEx;
        rfIy = iSignY*fEy;
        rfIz = fEz;
    }
    else
    {
        // sphere intersects with edge
        Real fVSqrX = fVz*fVz + fVx*fVx;
        rfTFirst = GetEdgeIntersection(fDx,fDz,fVx,fVz,fVSqrX,fRSqr);
        rfIx = fEx;
        rfIy = fCy + rfTFirst*fVy;
        rfIz = fEz;
    }
    return 1;
}
//----------------------------------------------------------------------------
template <class Real>
static int FindEdgeRegionIntersection (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,
    bool bAboveEdge)
{
    // Assumes the sphere center is in the region above the x and z planes.
    // The input bAboveEdge is true when the y coordinate is within the y