mgcintrbox3box3.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcIntrBox3Box3.h"

//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcBox3& rkBox0, const MgcBox3& rkBox1)
{
    // convenience variables
    const MgcVector3* akA = rkBox0.Axes();
    const MgcVector3* akB = rkBox1.Axes();
    const MgcReal* afEA = rkBox0.Extents();
    const MgcReal* afEB = rkBox1.Extents();

    // compute difference of box centers, D = C1-C0
    MgcVector3 kD = rkBox1.Center() - rkBox0.Center();

    MgcReal aafC[3][3];     // matrix C = A^T B, c_{ij} = Dot(A_i,B_j)
    MgcReal aafAbsC[3][3];  // |c_{ij}|
    MgcReal afAD[3];        // Dot(A_i,D)
    MgcReal fR0, fR1, fR;   // interval radii and distance between centers
    MgcReal fR01;           // = R0 + R1
    
    // axis C0+t*A0
    aafC[0][0] = akA[0].Dot(akB[0]);
    aafC[0][1] = akA[0].Dot(akB[1]);
    aafC[0][2] = akA[0].Dot(akB[2]);
    afAD[0] = akA[0].Dot(kD);
    aafAbsC[0][0] = MgcMath::Abs(aafC[0][0]);
    aafAbsC[0][1] = MgcMath::Abs(aafC[0][1]);
    aafAbsC[0][2] = MgcMath::Abs(aafC[0][2]);
    fR = MgcMath::Abs(afAD[0]);
    fR1 = afEB[0]*aafAbsC[0][0]+afEB[1]*aafAbsC[0][1]+afEB[2]*aafAbsC[0][2];
    fR01 = afEA[0] + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A1
    aafC[1][0] = akA[1].Dot(akB[0]);
    aafC[1][1] = akA[1].Dot(akB[1]);
    aafC[1][2] = akA[1].Dot(akB[2]);
    afAD[1] = akA[1].Dot(kD);
    aafAbsC[1][0] = MgcMath::Abs(aafC[1][0]);
    aafAbsC[1][1] = MgcMath::Abs(aafC[1][1]);
    aafAbsC[1][2] = MgcMath::Abs(aafC[1][2]);
    fR = MgcMath::Abs(afAD[1]);
    fR1 = afEB[0]*aafAbsC[1][0]+afEB[1]*aafAbsC[1][1]+afEB[2]*aafAbsC[1][2];
    fR01 = afEA[1] + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A2
    aafC[2][0] = akA[2].Dot(akB[0]);
    aafC[2][1] = akA[2].Dot(akB[1]);
    aafC[2][2] = akA[2].Dot(akB[2]);
    afAD[2] = akA[2].Dot(kD);
    aafAbsC[2][0] = MgcMath::Abs(aafC[2][0]);
    aafAbsC[2][1] = MgcMath::Abs(aafC[2][1]);
    aafAbsC[2][2] = MgcMath::Abs(aafC[2][2]);
    fR = MgcMath::Abs(afAD[2]);
    fR1 = afEB[0]*aafAbsC[2][0]+afEB[1]*aafAbsC[2][1]+afEB[2]*aafAbsC[2][2];
    fR01 = afEA[2] + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*B0
    fR = MgcMath::Abs(akB[0].Dot(kD));
    fR0 = afEA[0]*aafAbsC[0][0]+afEA[1]*aafAbsC[1][0]+afEA[2]*aafAbsC[2][0];
    fR01 = fR0 + afEB[0];
    if ( fR > fR01 )
        return false;

    // axis C0+t*B1
    fR = MgcMath::Abs(akB[1].Dot(kD));
    fR0 = afEA[0]*aafAbsC[0][1]+afEA[1]*aafAbsC[1][1]+afEA[2]*aafAbsC[2][1];
    fR01 = fR0 + afEB[1];
    if ( fR > fR01 )
        return false;

    // axis C0+t*B2
    fR = MgcMath::Abs(akB[2].Dot(kD));
    fR0 = afEA[0]*aafAbsC[0][2]+afEA[1]*aafAbsC[1][2]+afEA[2]*aafAbsC[2][2];
    fR01 = fR0 + afEB[2];
    if ( fR > fR01 )
        return false;

    // axis C0+t*A0xB0
    fR = MgcMath::Abs(afAD[2]*aafC[1][0]-afAD[1]*aafC[2][0]);
    fR0 = afEA[1]*aafAbsC[2][0] + afEA[2]*aafAbsC[1][0];
    fR1 = afEB[1]*aafAbsC[0][2] + afEB[2]*aafAbsC[0][1];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A0xB1
    fR = MgcMath::Abs(afAD[2]*aafC[1][1]-afAD[1]*aafC[2][1]);
    fR0 = afEA[1]*aafAbsC[2][1] + afEA[2]*aafAbsC[1][1];
    fR1 = afEB[0]*aafAbsC[0][2] + afEB[2]*aafAbsC[0][0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A0xB2
    fR = MgcMath::Abs(afAD[2]*aafC[1][2]-afAD[1]*aafC[2][2]);
    fR0 = afEA[1]*aafAbsC[2][2] + afEA[2]*aafAbsC[1][2];
    fR1 = afEB[0]*aafAbsC[0][1] + afEB[1]*aafAbsC[0][0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A1xB0
    fR = MgcMath::Abs(afAD[0]*aafC[2][0]-afAD[2]*aafC[0][0]);
    fR0 = afEA[0]*aafAbsC[2][0] + afEA[2]*aafAbsC[0][0];
    fR1 = afEB[1]*aafAbsC[1][2] + afEB[2]*aafAbsC[1][1];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A1xB1
    fR = MgcMath::Abs(afAD[0]*aafC[2][1]-afAD[2]*aafC[0][1]);
    fR0 = afEA[0]*aafAbsC[2][1] + afEA[2]*aafAbsC[0][1];
    fR1 = afEB[0]*aafAbsC[1][2] + afEB[2]*aafAbsC[1][0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A1xB2
    fR = MgcMath::Abs(afAD[0]*aafC[2][2]-afAD[2]*aafC[0][2]);
    fR0 = afEA[0]*aafAbsC[2][2] + afEA[2]*aafAbsC[0][2];
    fR1 = afEB[0]*aafAbsC[1][1] + afEB[1]*aafAbsC[1][0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A2xB0
    fR = MgcMath::Abs(afAD[1]*aafC[0][0]-afAD[0]*aafC[1][0]);
    fR0 = afEA[0]*aafAbsC[1][0] + afEA[1]*aafAbsC[0][0];
    fR1 = afEB[1]*aafAbsC[2][2] + afEB[2]*aafAbsC[2][1];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A2xB1
    fR = MgcMath::Abs(afAD[1]*aafC[0][1]-afAD[0]*aafC[1][1]);
    fR0 = afEA[0]*aafAbsC[1][1] + afEA[1]*aafAbsC[0][1];
    fR1 = afEB[0]*aafAbsC[2][2] + afEB[2]*aafAbsC[2][0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0+t*A2xB2
    fR = MgcMath::Abs(afAD[1]*aafC[0][2]-afAD[0]*aafC[1][2]);
    fR0 = afEA[0]*aafAbsC[1][2] + afEA[1]*aafAbsC[0][2];
    fR1 = afEB[0]*aafAbsC[2][1] + afEB[1]*aafAbsC[2][0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    return true;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (MgcReal fTime, const MgcBox3& rkBox0,
    const MgcVector3& rkVel0, const MgcBox3& rkBox1,
    const MgcVector3& rkVel1)
{
    // convenience variables
    const MgcVector3* akA = rkBox0.Axes();
    const MgcVector3* akB = rkBox1.Axes();
    const MgcReal* afEA = rkBox0.Extents();
    const MgcReal* afEB = rkBox1.Extents();

    // Compute relative velocity of box1 with respect to box0 so that box0
    // may as well be stationary.
    MgcVector3 kW = rkVel1 - rkVel0;

    // Compute difference of box centers at time 0 and time 'fTime'.
    MgcVector3 kD0 = rkBox1.Center() - rkBox0.Center();
    MgcVector3 kD1 = kD0 + fTime*kW;

    MgcReal aafC[3][3];     // matrix C = A^T B, c_{ij} = Dot(A_i,B_j)
    MgcReal aafAbsC[3][3];  // |c_{ij}|
    MgcReal afAD0[3];       // Dot(A_i,D0)
    MgcReal afAD1[3];       // Dot(A_i,D1)
    MgcReal fR0, fR1, fR;   // interval radii and distance between centers
    MgcReal fR01;           // = R0 + R1
    
    // axis C0+t*A0
    aafC[0][0] = akA[0].Dot(akB[0]);
    aafC[0][1] = akA[0].Dot(akB[1]);
    aafC[0][2] = akA[0].Dot(akB[2]);
    afAD0[0] = akA[0].Dot(kD0);
    afAD1[0] = akA[0].Dot(kD1);
    aafAbsC[0][0] = MgcMath::Abs(aafC[0][0]);
    aafAbsC[0][1] = MgcMath::Abs(aafC[0][1]);
    aafAbsC[0][2] = MgcMath::Abs(aafC[0][2]);
    fR1 = afEB[0]*aafAbsC[0][0]+afEB[1]*aafAbsC[0][1]+afEB[2]*aafAbsC[0][2];
    fR01 = afEA[0] + fR1;
    if ( afAD0[0] > fR01 )
    {
        if ( afAD1[0] > fR01 )
            return false;
    }
    else if ( afAD0[0] < -fR01 )
    {
        if ( afAD1[0] < -fR01 )
            return false;
    }

    // axis C0+t*A1
    aafC[1][0] = akA[1].Dot(akB[0]);
    aafC[1][1] = akA[1].Dot(akB[1]);
    aafC[1][2] = akA[1].Dot(akB[2]);
    afAD0[1] = akA[1].Dot(kD0);
    afAD1[1] = akA[1].Dot(kD1);
    aafAbsC[1][0] = MgcMath::Abs(aafC[1][0]);
    aafAbsC[1][1] = MgcMath::Abs(aafC[1][1]);
    aafAbsC[1][2] = MgcMath::Abs(aafC[1][2]);
    fR1 = afEB[0]*aafAbsC[1][0]+afEB[1]*aafAbsC[1][1]+afEB[2]*aafAbsC[1][2];
    fR01 = afEA[1] + fR1;
    if ( afAD0[1] > fR01 )
    {
        if ( afAD1[1] > fR01 )
            return false;
    }
    else if ( afAD0[1] < -fR01 )
    {
        if ( afAD1[1] < -fR01 )
            return false;
    }

    // axis C0+t*A2
    aafC[2][0] = akA[2].Dot(akB[0]);
    aafC[2][1] = akA[2].Dot(akB[1]);
    aafC[2][2] = akA[2].Dot(akB[2]);
    afAD0[2] = akA[2].Dot(kD0);
    afAD1[2] = akA[2].Dot(kD1);
    aafAbsC[2][0] = MgcMath::Abs(aafC[2][0]);
    aafAbsC[2][1] = MgcMath::Abs(aafC[2][1]);
    aafAbsC[2][2] = MgcMath::Abs(aafC[2][2]);
    fR1 = afEB[0]*aafAbsC[2][0]+afEB[1]*aafAbsC[2][1]+afEB[2]*aafAbsC[2][2];
    fR01 = afEA[2] + fR1;
    if ( afAD0[2] > fR01 )
    {
        if ( afAD1[2] > fR01 )
            return false;
    }
    else if ( afAD0[2] < -fR01 )
    {
        if ( afAD1[2] < -fR01 )
            return false;
    }

    // axis C0+t*B0
    fR = akB[0].Dot(kD0);
    fR0 = afEA[0]*aafAbsC[0][0]+afEA[1]*aafAbsC[1][0]+afEA[2]*aafAbsC[2][0];
    fR01 = fR0 + afEB[0];
    if ( fR > fR01 )
    {
        fR = akB[0].Dot(kD1);
        if ( fR > fR01)
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = akB[0].Dot(kD1);
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*B1
    fR = akB[1].Dot(kD0);
    fR0 = afEA[0]*aafAbsC[0][1]+afEA[1]*aafAbsC[1][1]+afEA[2]*aafAbsC[2][1];
    fR01 = fR0 + afEB[1];
    if ( fR > fR01 )
    {
        fR = akB[1].Dot(kD1);
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = akB[1].Dot(kD1);
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*B2
    fR = akB[2].Dot(kD0);
    fR0 = afEA[0]*aafAbsC[0][2]+afEA[1]*aafAbsC[1][2]+afEA[2]*aafAbsC[2][2];
    fR01 = fR0 + afEB[2];
    if ( fR > fR01 )
    {
        fR = akB[2].Dot(kD1);
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = akB[2].Dot(kD1);
        if ( fR < -fR01 )
            return false;
    }