mgcintrbox3box3.cpp

3D Game Engine Design Source Code非常棒

    // axis C0+t*A0xB0
    fR = afAD0[2]*aafC[1][0]-afAD0[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 )
    {
        fR = afAD1[2]*aafC[1][0]-afAD1[1]*aafC[2][0];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[2]*aafC[1][0]-afAD1[1]*aafC[2][0];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A0xB1
    fR = afAD0[2]*aafC[1][1]-afAD0[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 )
    {
        fR = afAD1[2]*aafC[1][1]-afAD1[1]*aafC[2][1];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[2]*aafC[1][1]-afAD1[1]*aafC[2][1];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A0xB2
    fR = afAD0[2]*aafC[1][2]-afAD0[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 )
    {
        fR = afAD1[2]*aafC[1][2]-afAD1[1]*aafC[2][2];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[2]*aafC[1][2]-afAD1[1]*aafC[2][2];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A1xB0
    fR = afAD0[0]*aafC[2][0]-afAD0[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 )
    {
        fR = afAD1[0]*aafC[2][0]-afAD1[2]*aafC[0][0];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[0]*aafC[2][0]-afAD1[2]*aafC[0][0];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A1xB1
    fR = afAD0[0]*aafC[2][1]-afAD0[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 )
    {
        fR = afAD1[0]*aafC[2][1]-afAD1[2]*aafC[0][1];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[0]*aafC[2][1]-afAD1[2]*aafC[0][1];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A1xB2
    fR = afAD0[0]*aafC[2][2]-afAD0[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 )
    {
        fR = afAD1[0]*aafC[2][2]-afAD1[2]*aafC[0][2];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[0]*aafC[2][2]-afAD1[2]*aafC[0][2];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A2xB0
    fR = afAD0[1]*aafC[0][0]-afAD0[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 )
    {
        fR = afAD1[1]*aafC[0][0]-afAD1[0]*aafC[1][0];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[1]*aafC[0][0]-afAD1[0]*aafC[1][0];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A2xB1
    fR = afAD0[1]*aafC[0][1]-afAD0[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 )
    {
        fR = afAD1[1]*aafC[0][1]-afAD1[0]*aafC[1][1];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[1]*aafC[0][1]-afAD1[0]*aafC[1][1];
        if ( fR < -fR01 )
            return false;
    }

    // axis C0+t*A2xB2
    fR = afAD0[1]*aafC[0][2]-afAD0[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 )
    {
        fR = afAD1[1]*aafC[0][2]-afAD1[0]*aafC[1][2];
        if ( fR > fR01 )
            return false;
    }
    else if ( fR < -fR01 )
    {
        fR = afAD1[1]*aafC[0][2]-afAD1[0]*aafC[1][2];
        if ( fR < -fR01 )
            return false;
    }

    // At this point none of the 15 axes separate the boxes.  It is still
    // possible that they are separated as viewed in any plane orthogonal
    // to the relative direction of motion W.  In the worst case, the two
    // projected boxes are hexagons.  This requires three separating axis
    // tests per box.
    MgcVector3 kWxD0 = kW.Cross(kD0);
    MgcReal afWA[3], afWB[3];

    // axis C0 + t*WxA0
    afWA[1] = kW.Dot(akA[1]);
    afWA[2] = kW.Dot(akA[2]);
    fR = MgcMath::Abs(akA[0].Dot(kWxD0));
    fR0 = afEA[1]*afWA[2] + afEA[2]*afWA[1];
    fR1 =
        afEB[0]*MgcMath::Abs(aafC[1][0]*afWA[2] - aafC[2][0]*afWA[1]) +
        afEB[1]*MgcMath::Abs(aafC[1][1]*afWA[2] - aafC[2][1]*afWA[1]) +
        afEB[2]*MgcMath::Abs(aafC[1][2]*afWA[2] - aafC[2][2]*afWA[1]);
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0 + t*WxA1
    afWA[0] = kW.Dot(akA[0]);
    fR = MgcMath::Abs(akA[1].Dot(kWxD0));
    fR0 = afEA[2]*afWA[0] + afEA[0]*afWA[2];
    fR1 =
        afEB[0]*MgcMath::Abs(aafC[2][0]*afWA[0] - aafC[0][0]*afWA[2]) +
        afEB[1]*MgcMath::Abs(aafC[2][1]*afWA[0] - aafC[0][1]*afWA[2]) +
        afEB[2]*MgcMath::Abs(aafC[2][2]*afWA[0] - aafC[0][2]*afWA[2]);
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0 + t*WxA2
    fR = MgcMath::Abs(akA[2].Dot(kWxD0));
    fR0 = afEA[0]*afWA[1] + afEA[1]*afWA[0];
    fR1 =
        afEB[0]*MgcMath::Abs(aafC[0][0]*afWA[1] - aafC[1][0]*afWA[0]) +
        afEB[1]*MgcMath::Abs(aafC[0][1]*afWA[1] - aafC[1][1]*afWA[0]) +
        afEB[2]*MgcMath::Abs(aafC[0][2]*afWA[1] - aafC[1][2]*afWA[0]);
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0 + t*WxB0
    afWB[1] = kW.Dot(akB[1]);
    afWB[2] = kW.Dot(akB[2]);
    fR = MgcMath::Abs(akB[0].Dot(kWxD0));
    fR0 =
        afEA[0]*MgcMath::Abs(aafC[0][1]*afWB[2] - aafC[0][2]*afWB[1]) +
        afEA[1]*MgcMath::Abs(aafC[1][1]*afWB[2] - aafC[1][2]*afWB[1]) +
        afEA[2]*MgcMath::Abs(aafC[2][1]*afWB[2] - aafC[2][2]*afWB[1]);
    fR1 = afEB[1]*afWB[2] + afEB[2]*afWB[1];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0 + t*WxB1
    afWB[0] = kW.Dot(akB[0]);
    fR = MgcMath::Abs(akB[1].Dot(kWxD0));
    fR0 =
        afEA[0]*MgcMath::Abs(aafC[0][2]*afWB[0] - aafC[0][0]*afWB[2]) +
        afEA[1]*MgcMath::Abs(aafC[1][2]*afWB[0] - aafC[1][0]*afWB[2]) +
        afEA[2]*MgcMath::Abs(aafC[2][2]*afWB[0] - aafC[2][0]*afWB[2]);
    fR1 = afEB[2]*afWB[0] + afEB[0]*afWB[2];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    // axis C0 + t*WxB2
    fR = MgcMath::Abs(akB[2].Dot(kWxD0));
    fR0 =
        afEA[0]*MgcMath::Abs(aafC[0][0]*afWB[1] - aafC[0][1]*afWB[0]) +
        afEA[1]*MgcMath::Abs(aafC[1][0]*afWB[1] - aafC[1][1]*afWB[0]) +
        afEA[2]*MgcMath::Abs(aafC[2][0]*afWB[1] - aafC[2][1]*afWB[0]);
    fR1 = afEB[0]*afWB[1] + afEB[1]*afWB[0];
    fR01 = fR0 + fR1;
    if ( fR > fR01 )
        return false;

    return true;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (MgcReal fTime, int iNumSteps, const MgcBox3& rkBox0,
    const MgcVector3& rkVel0, const MgcVector3& rkRotCen0,
    const MgcVector3& rkRotAxis0, const MgcBox3& rkBox1,
    const MgcVector3& rkVel1, const MgcVector3& rkRotCen1,
    const MgcVector3& rkRotAxis1)
{
    // time step for the integration
    MgcReal fStep = fTime/MgcReal(iNumSteps);

    // initialize subinterval boxes
    MgcBox3 kSubBox0, kSubBox1;
    kSubBox0.Center() = rkBox0.Center();
    kSubBox1.Center() = rkBox1.Center();
    int i;
    for (i = 0; i < 3; i++)
    {
        kSubBox0.Axis(i) = rkBox0.Axis(i);
        kSubBox1.Axis(i) = rkBox1.Axis(i);
    }

    // integrate the differential equations using Euler's method
    for (int iStep = 1; iStep <= iNumSteps; iStep++)
    {
        // compute box velocities and test boxes for intersection
        MgcReal fSubTime = fStep*MgcReal(iStep);
        MgcVector3 kNewRotCen0 = rkRotCen0 + fSubTime*rkVel0;
        MgcVector3 kNewRotCen1 = rkRotCen1 + fSubTime*rkVel1;
        MgcVector3 kDiff0 = kSubBox0.Center() - kNewRotCen0;
        MgcVector3 kDiff1 = kSubBox1.Center() - kNewRotCen1;
        MgcVector3 kSubVel0 = fStep*(rkVel0 + rkRotAxis0.Cross(kDiff0));
        MgcVector3 kSubVel1 = fStep*(rkVel1 + rkRotAxis1.Cross(kDiff1));
        if ( MgcTestIntersection(fStep,kSubBox0,kSubVel0,kSubBox1,kSubVel1) )
            return true;

        // update the box centers
        kSubBox0.Center() = kSubBox0.Center() + kSubVel0;
        kSubBox1.Center() = kSubBox1.Center() + kSubVel1;

        // update the box axes
        for (i = 0; i < 3; i++)
        {
            kSubBox0.Axis(i) = kSubBox0.Axis(i) +
                fStep*rkRotAxis0.Cross(kSubBox0.Axis(i));

            kSubBox1.Axis(i) = kSubBox1.Axis(i) +
                fStep*rkRotAxis1.Cross(kSubBox1.Axis(i));
        }

        // Use Gram-Schmidt to orthonormalize the updated axes.  NOTE:  If
        // T/N is small and N is small, you can remove this expensive step
        // with the assumption that the updated axes are nearly orthonormal.
        MgcVector3::Orthonormalize(kSubBox0.Axes());
        MgcVector3::Orthonormalize(kSubBox1.Axes());
    }

    // NOTE:  If the boxes do not intersect, then the application might
    // want to move/rotate the boxes to their new locations.  In this case
    // you want to return the final values of kSubBox0 and kSubBox1 so that
    // the application can set rkBox0 <- kSubBox0 and rkBox1 <- kSubBox1.
    // Otherwise, the application would have to solve the differential
    // equation again or compute the new box locations using the closed form
    // solution for the rigid motion.

    return false;
}
//----------------------------------------------------------------------------