mgcdistlin3box3.cpp

3D Game Engine Design Source Code非常棒

    {
        fDelta = rkPnt[i2] + rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i2] = -rkBox.Extent(i2);
    }
    else if ( rkPnt[i2] > rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] - rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i2] = rkBox.Extent(i2);
    }
}
//----------------------------------------------------------------------------
static void Case00 (int i0, int i1, int i2, MgcVector3& rkPnt,
    const MgcVector3& rkDir, const MgcBox3& rkBox, MgcReal* pfLParam,
    MgcReal& rfSqrDistance)
{
    MgcReal fDelta;

    if ( pfLParam )
        *pfLParam = (rkBox.Extent(0) - rkPnt[i0])/rkDir[i0];

    rkPnt[i0] = rkBox.Extent(i0);

    if ( rkPnt[i1] < -rkBox.Extent(i1) )
    {
        fDelta = rkPnt[i1] + rkBox.Extent(i1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i1] = -rkBox.Extent(i1);
    }
    else if ( rkPnt[i1] > rkBox.Extent(i1) )
    {
        fDelta = rkPnt[i1] - rkBox.Extent(i1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i1] = rkBox.Extent(i1);
    }

    if ( rkPnt[i2] < -rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] + rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i1] = -rkBox.Extent(i2);
    }
    else if ( rkPnt[i2] > rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] - rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i2] = rkBox.Extent(i2);
    }
}
//----------------------------------------------------------------------------
static void Case000 (MgcVector3& rkPnt, const MgcBox3& rkBox,
    MgcReal& rfSqrDistance)
{
    MgcReal fDelta;

    if ( rkPnt.x < -rkBox.Extent(0) )
    {
        fDelta = rkPnt.x + rkBox.Extent(0);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.x = -rkBox.Extent(0);
    }
    else if ( rkPnt.x > rkBox.Extent(0) )
    {
        fDelta = rkPnt.x - rkBox.Extent(0);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.x = rkBox.Extent(0);
    }

    if ( rkPnt.y < -rkBox.Extent(1) )
    {
        fDelta = rkPnt.y + rkBox.Extent(1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.y = -rkBox.Extent(1);
    }
    else if ( rkPnt.y > rkBox.Extent(1) )
    {
        fDelta = rkPnt.y - rkBox.Extent(1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.y = rkBox.Extent(1);
    }

    if ( rkPnt.z < -rkBox.Extent(2) )
    {
        fDelta = rkPnt.z + rkBox.Extent(2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.z = -rkBox.Extent(2);
    }
    else if ( rkPnt.z > rkBox.Extent(2) )
    {
        fDelta = rkPnt.z - rkBox.Extent(2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.z = rkBox.Extent(2);
    }
}
//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcLine3& rkLine, const MgcBox3& rkBox,
    MgcReal* pfLParam, MgcReal* pfBParam0, MgcReal* pfBParam1,
    MgcReal* pfBParam2)
{
#ifdef _DEBUG
    // The four parameters pointers are either all non-null or all null.
    if ( pfLParam )
    {
        assert( pfBParam0 && pfBParam1 && pfBParam2 );
    }
    else
    {
        assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
    }
#endif

    // compute coordinates of line in box coordinate system
    MgcVector3 kDiff = rkLine.Origin() - rkBox.Center();
    MgcVector3 kPnt(kDiff.Dot(rkBox.Axis(0)),kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2)));
    MgcVector3 kDir(rkLine.Direction().Dot(rkBox.Axis(0)),
        rkLine.Direction().Dot(rkBox.Axis(1)),
        rkLine.Direction().Dot(rkBox.Axis(2)));

    // Apply reflections so that direction vector has nonnegative components.
    bool bReflect[3];
    int i;
    for (i = 0; i < 3; i++)
    {
        if ( kDir[i] < 0.0 )
        {
            kPnt[i] = -kPnt[i];
            kDir[i] = -kDir[i];
            bReflect[i] = true;
        }
        else
        {
            bReflect[i] = false;
        }
    }

    float fSqrDistance = 0.0;

    if ( kDir.x > 0.0 )
    {
        if ( kDir.y > 0.0 )
        {
            if ( kDir.z > 0.0 )
            {
                // (+,+,+)
                CaseNoZeros(kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (+,+,0)
                Case0(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
        else
        {
            if ( kDir.z > 0.0 )
            {
                // (+,0,+)
                Case0(0,2,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (+,0,0)
                Case00(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
    }
    else
    {
        if ( kDir.y > 0.0 )
        {
            if ( kDir.z > 0.0 )
            {
                // (0,+,+)
                Case0(1,2,0,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (0,+,0)
                Case00(1,0,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
        else
        {
            if ( kDir.z > 0.0 )
            {
                // (0,0,+)
                Case00(2,0,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (0,0,0)
                Case000(kPnt,rkBox,fSqrDistance);
                if ( pfLParam )
                    *pfLParam = 0.0;
            }
        }
    }

    if ( pfLParam )
    {
        // undo reflections
        for (i = 0; i < 3; i++)
        {
            if ( bReflect[i] )
                kPnt[i] = -kPnt[i];
        }

        *pfBParam0 = kPnt.x;
        *pfBParam1 = kPnt.y;
        *pfBParam2 = kPnt.z;
    }

    return fSqrDistance;
}
//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcRay3& rkRay, const MgcBox3& rkBox,
    MgcReal* pfLParam, MgcReal* pfBParam0, MgcReal* pfBParam1,
    MgcReal* pfBParam2)
{
#ifdef _DEBUG
    // The four parameters pointers are either all non-null or all null.
    if ( pfLParam )
    {
        assert( pfBParam0 && pfBParam1 && pfBParam2 );
    }
    else
    {
        assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
    }
#endif

    MgcLine3 kLine;
    kLine.Origin() = rkRay.Origin();
    kLine.Direction() = rkRay.Direction();

    MgcReal fLP, fBP0, fBP1, fBP2;
    MgcReal fSqrDistance = MgcSqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
    if ( fLP >= 0.0 )
    {
        if ( pfLParam )
        {
            *pfLParam = fLP;
            *pfBParam0 = fBP0;
            *pfBParam1 = fBP1;
            *pfBParam2 = fBP2;
        }

        return fSqrDistance;
    }
    else
    {
        fSqrDistance = MgcSqrDistance(rkRay.Origin(),rkBox,pfBParam0,
            pfBParam1,pfBParam2);

        if ( pfLParam )
            *pfLParam = 0.0;

        return fSqrDistance;
    }
}
//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcSegment3& rkSeg, const MgcBox3& rkBox,
    MgcReal* pfLParam, MgcReal* pfBParam0, MgcReal* pfBParam1,
    MgcReal* pfBParam2)
{
#ifdef _DEBUG
    // The four parameters pointers are either all non-null or all null.
    if ( pfLParam )
    {
        assert( pfBParam0 && pfBParam1 && pfBParam2 );
    }
    else
    {
        assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
    }
#endif

    MgcLine3 kLine;
    kLine.Origin() = rkSeg.Origin();
    kLine.Direction() = rkSeg.Direction();

    MgcReal fLP, fBP0, fBP1, fBP2;
    MgcReal fSqrDistance = MgcSqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
    if ( fLP >= 0.0 )
    {
        if ( fLP <= 1.0 )
        {
            if ( pfLParam )
            {
                *pfLParam = fLP;
                *pfBParam0 = fBP0;
                *pfBParam1 = fBP1;
                *pfBParam2 = fBP2;
            }

            return fSqrDistance;
        }
        else
        {
            fSqrDistance = MgcSqrDistance(rkSeg.Origin()+rkSeg.Direction(),
                rkBox,pfBParam0,pfBParam1,pfBParam2);

            if ( pfLParam )
                *pfLParam = 0.0;

            return fSqrDistance;
        }
    }
    else
    {
        fSqrDistance = MgcSqrDistance(rkSeg.Origin(),rkBox,pfBParam0,
            pfBParam1,pfBParam2);

        if ( pfLParam )
            *pfLParam = 0.0;

        return fSqrDistance;
    }
}
//----------------------------------------------------------------------------
MgcReal MgcDistance (const MgcLine3& rkLine, const MgcBox3& rkBox,
    MgcReal* pfLParam, MgcReal* pfBParam0, MgcReal* pfBParam1,
    MgcReal* pfBParam2)
{
    return MgcMath::Sqrt(MgcSqrDistance(rkLine,rkBox,pfLParam,pfBParam0,
        pfBParam1,pfBParam2));
}
//----------------------------------------------------------------------------
MgcReal MgcDistance (const MgcRay3& rkRay, const MgcBox3& rkBox,
    MgcReal* pfLParam, MgcReal* pfBParam0, MgcReal* pfBParam1,
    MgcReal* pfBParam2)
{
    return MgcMath::Sqrt(MgcSqrDistance(rkRay,rkBox,pfLParam,pfBParam0,
        pfBParam1,pfBParam2));
}
//----------------------------------------------------------------------------
MgcReal MgcDistance (const MgcSegment3& rkSeg, const MgcBox3& rkBox,
    MgcReal* pfLParam, MgcReal* pfBParam0, MgcReal* pfBParam1,
    MgcReal* pfBParam2)
{
    return MgcMath::Sqrt(MgcSqrDistance(rkSeg,rkBox,pfLParam,pfBParam0,
        pfBParam1,pfBParam2));
}
//----------------------------------------------------------------------------