Motion Planning Kernel: basic/geometry/geo_rangesensor/MgcIntrBox3Frustum.cpp Source File

00001 // Magic Software, Inc.
00002 // http://www.magic-software.com
00003 // Copyright (c) 2000, All Rights Reserved
00004 //
00005 // Source code from Magic Software is supplied under the terms of a license
00006 // agreement and may not be copied or disclosed except in accordance with the
00007 // terms of that agreement.  The various license agreements may be found at
00008 // the Magic Software web site.  This file is subject to the license
00009 //
00010 // FREE SOURCE CODE
00011 // http://www.magic-software.com/License.html/free.pdf
00012 
00013 #include "MgcIntrBox3Frustum.h"
00014 
00015 //----------------------------------------------------------------------------
00016 bool MgcTestIntersection (const MgcBox3& rkBox, const MgcFrustum& rkFrustum)
00017 {
00018     // for convenience
00019     const MgcVector3* akA = rkBox.Axes();
00020     const MgcReal* afE = rkBox.Extents();
00021 
00022     MgcVector3 kDiff = rkBox.Center() - rkFrustum.Origin();
00023 
00024     MgcReal afA[3], afB[3], afC[3], afD[3];
00025     MgcReal afNA[3], afNB[3], afNC[3], afND[3];
00026     MgcReal afNApLC[3], afNAmLC[3], afNBpUC[3], afNBmUC[3];
00027     MgcReal afLC[3], afLD[3], afUC[3], afUD[3], afLBpUA[3], afLBmUA[3];
00028     MgcReal fDdD, fR, fP, fMin, fMax, fMTwoLF, fMTwoUF, fLB, fUA, fTmp;
00029     int i, j;
00030 
00031     // M = D
00032     afD[2] = kDiff.Dot(rkFrustum.DVector());
00033     for (i = 0; i < 3; i++)
00034         afC[i] = akA[i].Dot(rkFrustum.DVector());
00035     fR = afE[0]*MgcMath::Abs(afC[0]) +
00036          afE[1]*MgcMath::Abs(afC[1]) +
00037          afE[2]*MgcMath::Abs(afC[2]);
00038     if ( afD[2] + fR < rkFrustum.DMin() || afD[2] - fR > rkFrustum.DMax() )
00039         return false;
00040 
00041     // M = n*L - l*D
00042     for (i = 0; i < 3; i++)
00043     {
00044         afA[i] = akA[i].Dot(rkFrustum.LVector());
00045         afLC[i] = rkFrustum.LBound()*afC[i];
00046         afNA[i] = rkFrustum.DMin()*afA[i];
00047         afNAmLC[i] = afNA[i] - afLC[i];
00048     }
00049     afD[0] = kDiff.Dot(rkFrustum.LVector());
00050     fR = afE[0]*MgcMath::Abs(afNAmLC[0]) +
00051          afE[1]*MgcMath::Abs(afNAmLC[1]) +
00052          afE[2]*MgcMath::Abs(afNAmLC[2]);
00053     afND[0] = rkFrustum.DMin()*afD[0];
00054     afLD[2] = rkFrustum.LBound()*afD[2];
00055     fDdD = afND[0] - afLD[2];
00056     fMTwoLF = rkFrustum.GetMTwoLF();
00057     if ( fDdD + fR < fMTwoLF || fDdD > fR )
00058         return false;
00059 
00060     // M = -n*L - l*D
00061     for (i = 0; i < 3; i++)
00062         afNApLC[i] = afNA[i] + afLC[i];
00063     fR = afE[0]*MgcMath::Abs(afNApLC[0]) +
00064          afE[1]*MgcMath::Abs(afNApLC[1]) +
00065          afE[2]*MgcMath::Abs(afNApLC[2]);
00066     fDdD = -(afND[0] + afLD[2]);
00067     if ( fDdD + fR < fMTwoLF || fDdD > fR )
00068         return false;
00069 
00070     // M = n*U - u*D
00071     for (i = 0; i < 3; i++)
00072     {
00073         afB[i] = akA[i].Dot(rkFrustum.UVector());
00074         afUC[i] = rkFrustum.UBound()*afC[i];
00075         afNB[i] = rkFrustum.DMin()*afB[i];
00076         afNBmUC[i] = afNB[i] - afUC[i];
00077     }
00078     afD[1] = kDiff.Dot(rkFrustum.UVector());
00079     fR = afE[0]*MgcMath::Abs(afNBmUC[0]) +
00080          afE[1]*MgcMath::Abs(afNBmUC[1]) +
00081          afE[2]*MgcMath::Abs(afNBmUC[2]);
00082     afND[1] = rkFrustum.DMin()*afD[1];
00083     afUD[2] = rkFrustum.UBound()*afD[2];
00084     fDdD = afND[1] - afUD[2];
00085     fMTwoUF = rkFrustum.GetMTwoUF();
00086     if ( fDdD + fR < fMTwoUF || fDdD > fR )
00087         return false;
00088 
00089     // M = -n*U - u*D
00090     for (i = 0; i < 3; i++)
00091         afNBpUC[i] = afNB[i] + afUC[i];
00092     fR = afE[0]*MgcMath::Abs(afNBpUC[0]) +
00093          afE[1]*MgcMath::Abs(afNBpUC[1]) +
00094          afE[2]*MgcMath::Abs(afNBpUC[2]);
00095     fDdD = -(afND[1] + afUD[2]);
00096     if ( fDdD + fR < fMTwoUF || fDdD > fR )
00097         return false;
00098 
00099     // M = A[i]
00100     for (i = 0; i < 3; i++)
00101     {
00102         fP = rkFrustum.LBound()*MgcMath::Abs(afA[i]) +
00103              rkFrustum.UBound()*MgcMath::Abs(afB[i]);
00104         afNC[i] = rkFrustum.DMin()*afC[i];
00105         fMin = afNC[i] - fP;
00106         if ( fMin < 0.0 )
00107             fMin *= rkFrustum.GetDRatio();
00108         fMax = afNC[i] + fP;
00109         if ( fMax > 0.0 )
00110             fMax *= rkFrustum.GetDRatio();
00111         fDdD = afA[i]*afD[0] + afB[i]*afD[1] + afC[i]*afD[2];
00112         if ( fDdD + afE[i] < fMin || fDdD - afE[i] > fMax )
00113             return false;
00114     }
00115 
00116     // M = Cross(L,A[i])
00117     for (i = 0; i < 3; i++)
00118     {
00119         fP = rkFrustum.UBound()*MgcMath::Abs(afC[i]);
00120         fMin = afNB[i] - fP;
00121         if ( fMin < 0.0 )
00122             fMin *= rkFrustum.GetDRatio();
00123         fMax = afNB[i] + fP;
00124         if ( fMax > 0.0 )
00125             fMax *= rkFrustum.GetDRatio();
00126         fDdD = -afC[i]*afD[1] + afB[i]*afD[2];
00127         fR = afE[0]*MgcMath::Abs(afB[i]*afC[0]-afB[0]*afC[i]) +
00128              afE[1]*MgcMath::Abs(afB[i]*afC[1]-afB[1]*afC[i]) +
00129              afE[2]*MgcMath::Abs(afB[i]*afC[2]-afB[2]*afC[i]);
00130         if ( fDdD + fR < fMin || fDdD - fR > fMax )
00131             return false;
00132     }
00133 
00134     // M = Cross(U,A[i])
00135     for (i = 0; i < 3; i++)
00136     {
00137         fP = rkFrustum.LBound()*MgcMath::Abs(afC[i]);
00138         fMin = -afNA[i] - fP;
00139         if ( fMin < 0.0 )
00140             fMin *= rkFrustum.GetDRatio();
00141         fMax = -afNA[i] + fP;
00142         if ( fMax > 0.0 )
00143             fMax *= rkFrustum.GetDRatio();
00144         fDdD = afC[i]*afD[0] - afA[i]*afD[2];
00145         fR = afE[0]*MgcMath::Abs(afA[i]*afC[0]-afA[0]*afC[i]) +
00146              afE[1]*MgcMath::Abs(afA[i]*afC[1]-afA[1]*afC[i]) +
00147              afE[2]*MgcMath::Abs(afA[i]*afC[2]-afA[2]*afC[i]);
00148         if ( fDdD + fR < fMin || fDdD - fR > fMax )
00149             return false;
00150     }
00151 
00152     // M = Cross(n*D+l*L+u*U,A[i])
00153     for (i = 0; i < 3; i++)
00154     {
00155         fLB = rkFrustum.LBound()*afB[i];
00156         fUA = rkFrustum.UBound()*afA[i];
00157         afLBpUA[i] = fLB + fUA;
00158         afLBmUA[i] = fLB - fUA;
00159     }
00160     for (i = 0; i < 3; i++)
00161     {
00162         fP = rkFrustum.LBound()*MgcMath::Abs(afNBmUC[i]) +
00163              rkFrustum.UBound()*MgcMath::Abs(afNAmLC[i]);
00164         fTmp = rkFrustum.DMin()*afLBmUA[i];
00165         fMin = fTmp - fP;
00166         if ( fMin < 0.0 )
00167             fMin *= rkFrustum.GetDRatio();
00168         fMax = fTmp + fP;
00169         if ( fMax > 0.0 )
00170             fMax *= rkFrustum.GetDRatio();
00171         fDdD = -afD[0]*afNBmUC[i] + afD[1]*afNAmLC[i] + afD[2]*afLBmUA[i];
00172         fR = 0.0;
00173         for (j = 0; j < 3; j++)
00174         {
00175             fR += afE[j]*MgcMath::Abs(-afA[j]*afNBmUC[i]+ afB[j]*afNAmLC[i]
00176                 + afC[j]*afLBmUA[i]);
00177         }
00178         if ( fDdD + fR < fMin || fDdD - fR > fMax )
00179             return false;
00180     }
00181 
00182     // M = Cross(n*D+l*L-u*U,A[i])
00183     for (i = 0; i < 3; i++)
00184     {
00185         fP = rkFrustum.LBound()*MgcMath::Abs(afNBpUC[i]) +
00186              rkFrustum.UBound()*MgcMath::Abs(afNAmLC[i]);
00187         fTmp = rkFrustum.DMin()*afLBpUA[i];
00188         fMin = fTmp - fP;
00189         if ( fMin < 0.0 )
00190             fMin *= rkFrustum.GetDRatio();
00191         fMax = fTmp + fP;
00192         if ( fMax > 0.0 )
00193             fMax *= rkFrustum.GetDRatio();
00194         fDdD = -afD[0]*afNBpUC[i] + afD[1]*afNAmLC[i] + afD[2]*afLBpUA[i];
00195         fR = 0.0;
00196         for (j = 0; j < 3; j++)
00197         {
00198             fR += afE[j]*MgcMath::Abs(-afA[j]*afNBpUC[i]+ afB[j]*afNAmLC[i]
00199                 + afC[j]*afLBpUA[i]);
00200         }
00201         if ( fDdD + fR < fMin || fDdD - fR > fMax )
00202             return false;
00203     }
00204 
00205     // M = Cross(n*D-l*L+u*U,A[i])
00206     for (i = 0; i < 3; i++)
00207     {
00208         fP = rkFrustum.LBound()*MgcMath::Abs(afNBmUC[i]) +
00209              rkFrustum.UBound()*MgcMath::Abs(afNApLC[i]);
00210         fTmp = -rkFrustum.DMin()*afLBpUA[i];
00211         fMin = fTmp - fP;
00212         if ( fMin < 0.0 )
00213             fMin *= rkFrustum.GetDRatio();
00214         fMax = fTmp + fP;
00215         if ( fMax > 0.0 )
00216             fMax *= rkFrustum.GetDRatio();
00217         fDdD = -afD[0]*afNBmUC[i] + afD[1]*afNApLC[i] - afD[2]*afLBpUA[i];
00218         fR = 0.0;
00219         for (j = 0; j < 3; j++)
00220         {
00221             fR += afE[j]*MgcMath::Abs(-afA[j]*afNBmUC[i]+ afB[j]*afNApLC[i]
00222                 - afC[j]*afLBpUA[i]);
00223         }
00224         if ( fDdD + fR < fMin || fDdD - fR > fMax )
00225             return false;
00226     }
00227 
00228     // M = Cross(n*D-l*L-u*U,A[i])
00229     for (i = 0; i < 3; i++)
00230     {
00231         fP = rkFrustum.LBound()*MgcMath::Abs(afNBpUC[i]) +
00232              rkFrustum.UBound()*MgcMath::Abs(afNApLC[i]);
00233         fTmp = -rkFrustum.DMin()*afLBmUA[i];
00234         fMin = fTmp - fP;
00235         if ( fMin < 0.0 )
00236             fMin *= rkFrustum.GetDRatio();
00237         fMax = fTmp + fP;
00238         if ( fMax > 0.0 )
00239             fMax *= rkFrustum.GetDRatio();
00240         fDdD = -afD[0]*afNBpUC[i] + afD[1]*afNApLC[i] - afD[2]*afLBmUA[i];
00241         fR = 0.0;
00242         for (j = 0; j < 3; j++)
00243         {
00244             fR += afE[j]*MgcMath::Abs(-afA[j]*afNBpUC[i]+ afB[j]*afNApLC[i]
00245                 - afC[j]*afLBmUA[i]);
00246         }
00247         if ( fDdD + fR < fMin || fDdD - fR > fMax )
00248             return false;
00249     }
00250 
00251     return true;
00252 }
00253 //----------------------------------------------------------------------------