boxBoxDistance.cpp

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 
00015 */
00016 
00017 
00018 //#include "PfxContactBoxBox.h"
00019 
00020 #include <math.h>
00021 #include "../PlatformDefinitions.h"
00022 #include "boxBoxDistance.h"
00023 
00024 static inline float sqr( float a )
00025 {
00026         return (a * a);
00027 }
00028 
00029 enum BoxSepAxisType
00030 {
00031         A_AXIS, B_AXIS, CROSS_AXIS
00032 };
00033 
00034 //-------------------------------------------------------------------------------------------------
00035 // voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.
00036 //-------------------------------------------------------------------------------------------------
00037 
00038 static const float voronoiTol = -1.0e-5f;
00039 
00040 //-------------------------------------------------------------------------------------------------
00041 // separating axis tests: gaps along each axis are computed, and the axis with the maximum
00042 // gap is stored.  cross product axes are normalized.
00043 //-------------------------------------------------------------------------------------------------
00044 
00045 #define AaxisTest( dim, letter, first )                                                         \
00046 {                                                                                               \
00047    if ( first )                                                                                 \
00048    {                                                                                            \
00049       maxGap = gap = gapsA.get##letter();                                                      \
00050       if ( gap > distanceThreshold ) return gap;                                                \
00051       axisType = A_AXIS;                                                                        \
00052       faceDimA = dim;                                                                           \
00053       axisA = identity.getCol##dim();                                                          \
00054    }                                                                                            \
00055    else                                                                                         \
00056    {                                                                                            \
00057       gap = gapsA.get##letter();                                                               \
00058       if ( gap > distanceThreshold ) return gap;                                                \
00059       else if ( gap > maxGap )                                                                  \
00060       {                                                                                         \
00061          maxGap = gap;                                                                          \
00062          axisType = A_AXIS;                                                                     \
00063          faceDimA = dim;                                                                        \
00064          axisA = identity.getCol##dim();                                                       \
00065       }                                                                                         \
00066    }                                                                                            \
00067 }
00068 
00069 
00070 #define BaxisTest( dim, letter )                                                                \
00071 {                                                                                               \
00072    gap = gapsB.get##letter();                                                                  \
00073    if ( gap > distanceThreshold ) return gap;                                                   \
00074    else if ( gap > maxGap )                                                                     \
00075    {                                                                                            \
00076       maxGap = gap;                                                                             \
00077       axisType = B_AXIS;                                                                        \
00078       faceDimB = dim;                                                                           \
00079       axisB = identity.getCol##dim();                                                          \
00080    }                                                                                            \
00081 }
00082 
00083 #define CrossAxisTest( dima, dimb, letterb )                                                    \
00084 {                                                                                               \
00085    const float lsqr_tolerance = 1.0e-30f;                                                       \
00086    float lsqr;                                                                                  \
00087                                                                                                 \
00088    lsqr = lsqrs.getCol##dima().get##letterb();                                                \
00089                                                                                                 \
00090    if ( lsqr > lsqr_tolerance )                                                                 \
00091    {                                                                                            \
00092       float l_recip = 1.0f / sqrtf( lsqr );                                                     \
00093       gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip;                           \
00094                                                                                                 \
00095       if ( gap > distanceThreshold )                                                            \
00096       {                                                                                         \
00097          return gap;                                                                            \
00098       }                                                                                         \
00099                                                                                                 \
00100       if ( gap > maxGap )                                                                       \
00101       {                                                                                         \
00102          maxGap = gap;                                                                          \
00103          axisType = CROSS_AXIS;                                                                 \
00104          edgeDimA = dima;                                                                       \
00105          edgeDimB = dimb;                                                                       \
00106          axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip;            \
00107       }                                                                                         \
00108    }                                                                                            \
00109 }
00110 
00111 //-------------------------------------------------------------------------------------------------
00112 // tests whether a vertex of box B and a face of box A are the closest features
00113 //-------------------------------------------------------------------------------------------------
00114 
00115 inline
00116 float
00117 VertexBFaceATest(
00118         bool & inVoronoi,
00119         float & t0,
00120         float & t1,
00121         const vmVector3 & hA,
00122         PE_REF(vmVector3) faceOffsetAB,
00123         PE_REF(vmVector3) faceOffsetBA,
00124         const vmMatrix3 & matrixAB,
00125         const vmMatrix3 & matrixBA,
00126         PE_REF(vmVector3) signsB,
00127         PE_REF(vmVector3) scalesB )
00128 {
00129         // compute a corner of box B in A's coordinate system
00130 
00131         vmVector3 corner =
00132                 vmVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );
00133 
00134         // compute the parameters of the point on A, closest to this corner
00135 
00136         t0 = corner[0];
00137         t1 = corner[1];
00138 
00139         if ( t0 > hA[0] )
00140                 t0 = hA[0];
00141         else if ( t0 < -hA[0] )
00142                 t0 = -hA[0];
00143         if ( t1 > hA[1] )
00144                 t1 = hA[1];
00145         else if ( t1 < -hA[1] )
00146                 t1 = -hA[1];
00147 
00148         // do the Voronoi test: already know the point on B is in the Voronoi region of the
00149         // point on A, check the reverse.
00150 
00151         vmVector3 facePointB =
00152                 vmVector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );
00153 
00154         inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) &&
00155                                   ( facePointB[1] >= voronoiTol * facePointB[0] ) &&
00156                                   ( facePointB[2] >= voronoiTol * facePointB[1] ) );
00157 
00158         return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] ));
00159 }
00160 
00161 #define VertexBFaceA_SetNewMin()                \
00162 {                                               \
00163    minDistSqr = distSqr;                        \
00164    localPointA.setX(t0);                        \
00165    localPointA.setY(t1);                        \
00166    localPointB.setX( scalesB.getX() );          \
00167    localPointB.setY( scalesB.getY() );          \
00168    featureA = F;                                \
00169    featureB = V;                                \
00170 }
00171 
00172 void
00173 VertexBFaceATests(
00174         bool & done,
00175         float & minDistSqr,
00176         vmPoint3 & localPointA,
00177         vmPoint3 & localPointB,
00178         FeatureType & featureA,
00179         FeatureType & featureB,
00180         const vmVector3 & hA,
00181         PE_REF(vmVector3) faceOffsetAB,
00182         PE_REF(vmVector3) faceOffsetBA,
00183         const vmMatrix3 & matrixAB,
00184         const vmMatrix3 & matrixBA,
00185         PE_REF(vmVector3) signsB,
00186         PE_REF(vmVector3) scalesB,
00187         bool first )
00188 {
00189                 
00190         float t0, t1;
00191         float distSqr;
00192 
00193         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
00194                                                                 matrixAB, matrixBA, signsB, scalesB );
00195 
00196         if ( first ) {
00197                 VertexBFaceA_SetNewMin();
00198         } else {
00199                 if ( distSqr < minDistSqr ) {
00200                         VertexBFaceA_SetNewMin();
00201                 }
00202         }
00203 
00204         if ( done )
00205                 return;
00206 
00207         signsB.setX( -signsB.getX() );
00208         scalesB.setX( -scalesB.getX() );
00209 
00210         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
00211                                                                 matrixAB, matrixBA, signsB, scalesB );
00212 
00213         if ( distSqr < minDistSqr ) {
00214                 VertexBFaceA_SetNewMin();
00215         }
00216 
00217         if ( done )
00218                 return;
00219 
00220         signsB.setY( -signsB.getY() );
00221         scalesB.setY( -scalesB.getY() );
00222 
00223         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
00224                                                                 matrixAB, matrixBA, signsB, scalesB );
00225 
00226         if ( distSqr < minDistSqr ) {
00227                 VertexBFaceA_SetNewMin();
00228         }
00229 
00230         if ( done )
00231                 return;
00232 
00233         signsB.setX( -signsB.getX() );
00234         scalesB.setX( -scalesB.getX() );
00235 
00236         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
00237                                                                 matrixAB, matrixBA, signsB, scalesB );
00238 
00239         if ( distSqr < minDistSqr ) {
00240                 VertexBFaceA_SetNewMin();
00241         }
00242 }
00243 
00244 //-------------------------------------------------------------------------------------------------
00245 // VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features
00246 //-------------------------------------------------------------------------------------------------
00247 
00248 inline
00249 float
00250 VertexAFaceBTest(
00251         bool & inVoronoi,
00252         float & t0,
00253         float & t1,
00254         const vmVector3 & hB,
00255         PE_REF(vmVector3) faceOffsetAB,
00256         PE_REF(vmVector3) faceOffsetBA,
00257         const vmMatrix3 & matrixAB,
00258         const vmMatrix3 & matrixBA,
00259         PE_REF(vmVector3) signsA,
00260         PE_REF(vmVector3) scalesA )
00261 {
00262         vmVector3 corner =
00263                 vmVector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );
00264 
00265         t0 = corner[0];
00266         t1 = corner[1];
00267 
00268         if ( t0 > hB[0] )
00269                 t0 = hB[0];
00270         else if ( t0 < -hB[0] )
00271                 t0 = -hB[0];
00272         if ( t1 > hB[1] )
00273                 t1 = hB[1];
00274         else if ( t1 < -hB[1] )
00275                 t1 = -hB[1];
00276 
00277         vmVector3 facePointA =
00278                 vmVector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );
00279 
00280         inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) &&
00281                                   ( facePointA[1] >= voronoiTol * facePointA[0] ) &&
00282                                   ( facePointA[2] >= voronoiTol * facePointA[1] ) );
00283 
00284         return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] ));
00285 }
00286 
00287 #define VertexAFaceB_SetNewMin()                \
00288 {                                               \
00289    minDistSqr = distSqr;                        \
00290    localPointB.setX(t0);                        \
00291    localPointB.setY(t1);                        \
00292    localPointA.setX( scalesA.getX() );          \
00293    localPointA.setY( scalesA.getY() );          \
00294    featureA = V;                                \
00295    featureB = F;                                \
00296 }
00297 
00298 void
00299 VertexAFaceBTests(
00300         bool & done,
00301         float & minDistSqr,
00302         vmPoint3 & localPointA,
00303         vmPoint3 & localPointB,
00304         FeatureType & featureA,
00305         FeatureType & featureB,
00306         const vmVector3 & hB,
00307         PE_REF(vmVector3) faceOffsetAB,
00308         PE_REF(vmVector3) faceOffsetBA,
00309         const vmMatrix3 & matrixAB,
00310         const vmMatrix3 & matrixBA,
00311         PE_REF(vmVector3) signsA,
00312         PE_REF(vmVector3) scalesA,
00313         bool first )
00314 {
00315         float t0, t1;
00316         float distSqr;
00317 
00318         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
00319                                                                 matrixAB, matrixBA, signsA, scalesA );
00320 
00321         if ( first ) {
00322                 VertexAFaceB_SetNewMin();
00323         } else {
00324                 if ( distSqr < minDistSqr ) {
00325                         VertexAFaceB_SetNewMin();
00326                 }
00327         }
00328 
00329         if ( done )
00330                 return;
00331 
00332         signsA.setX( -signsA.getX() );
00333         scalesA.setX( -scalesA.getX() );
00334 
00335         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
00336                                                                 matrixAB, matrixBA, signsA, scalesA );
00337 
00338         if ( distSqr < minDistSqr ) {
00339                 VertexAFaceB_SetNewMin();
00340         }
00341 
00342         if ( done )
00343                 return;
00344 
00345         signsA.setY( -signsA.getY() );
00346         scalesA.setY( -scalesA.getY() );
00347 
00348         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
00349                                                                 matrixAB, matrixBA, signsA, scalesA );
00350 
00351         if ( distSqr < minDistSqr ) {
00352                 VertexAFaceB_SetNewMin();
00353         }
00354 
00355         if ( done )
00356                 return;
00357 
00358         signsA.setX( -signsA.getX() );
00359         scalesA.setX( -scalesA.getX() );
00360 
00361         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
00362                                                                 matrixAB, matrixBA, signsA, scalesA );
00363 
00364         if ( distSqr < minDistSqr ) {
00365                 VertexAFaceB_SetNewMin();
00366         }
00367 }
00368 
00369 //-------------------------------------------------------------------------------------------------
00370 // CustomEdgeEdgeTest:
00371 //
00372 // tests whether a pair of edges are the closest features
00373 //
00374 // note on the shorthand:
00375 // 'a' & 'b' refer to the edges.
00376 // 'c' is the dimension of the axis that points from the face center to the edge Center
00377 // 'd' is the dimension of the edge Direction
00378 // the dimension of the face normal is 2
00379 //-------------------------------------------------------------------------------------------------
00380 
00381 #define CustomEdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter )              \
00382 {                                                                                               \
00383    vmVector3 edgeOffsetAB;                                                                          \
00384    vmVector3 edgeOffsetBA;                                                                          \
00385                                                                                                 \
00386    edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter();            \
00387    edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() );  \
00388                                                                                                 \
00389    edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter();            \
00390    edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() );  \
00391                                                                                                 \
00392    float dirDot = matrixAB.getCol##bd().get##ad_letter();                                     \
00393    float denom = 1.0f - dirDot*dirDot;                                                          \
00394    float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter();                                      \
00395    float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter();                                      \
00396                                                                                                 \
00397    if ( denom == 0.0f )                                                                         \
00398    {                                                                                            \
00399       tA = 0.0f;                                                                                \
00400    }                                                                                            \
00401    else                                                                                         \
00402    {                                                                                            \
00403       tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom;                              \
00404    }                                                                                            \
00405                                                                                                 \
00406    if ( tA < -hA[ad] ) tA = -hA[ad];                                                            \
00407    else if ( tA > hA[ad] ) tA = hA[ad];                                                         \
00408                                                                                                 \
00409    tB = tA * dirDot + edgeOffsetBA_bd;                                                          \
00410                                                                                                 \
00411    if ( tB < -hB[bd] )                                                                          \
00412    {                                                                                            \
00413       tB = -hB[bd];                                                                             \
00414       tA = tB * dirDot + edgeOffsetAB_ad;                                                       \
00415                                                                                                 \
00416       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \
00417       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \
00418    }                                                                                            \
00419    else if ( tB > hB[bd] )                                                                      \
00420    {                                                                                            \
00421       tB = hB[bd];                                                                              \
00422       tA = tB * dirDot + edgeOffsetAB_ad;                                                       \
00423                                                                                                 \
00424       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \
00425       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \
00426    }                                                                                            \
00427                                                                                                 \
00428    vmVector3 edgeOffAB = vmVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\
00429    vmVector3 edgeOffBA = vmVector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\
00430                                                                                                 \
00431    inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) &&                                \
00432                ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) &&                                \
00433                ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) &&                                \
00434                ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] );                                  \
00435                                                                                                 \
00436    edgeOffAB[ad] -= tA;                                                                         \
00437    edgeOffBA[bd] -= tB;                                                                         \
00438                                                                                                 \
00439    return dot(edgeOffAB,edgeOffAB);                                                             \
00440 }
00441 
00442 float
00443 CustomEdgeEdgeTest_0101(
00444         bool & inVoronoi,
00445         float & tA,
00446         float & tB,
00447         const vmVector3 & hA,
00448         const vmVector3 & hB,
00449         PE_REF(vmVector3) faceOffsetAB,
00450         PE_REF(vmVector3) faceOffsetBA,
00451         const vmMatrix3 & matrixAB,
00452         const vmMatrix3 & matrixBA,
00453         PE_REF(vmVector3) signsA,
00454         PE_REF(vmVector3) signsB,
00455         PE_REF(vmVector3) scalesA,
00456         PE_REF(vmVector3) scalesB )
00457 {
00458         CustomEdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );
00459 }
00460 
00461 float
00462 CustomEdgeEdgeTest_0110(
00463         bool & inVoronoi,
00464         float & tA,
00465         float & tB,
00466         const vmVector3 & hA,
00467         const vmVector3 & hB,
00468         PE_REF(vmVector3) faceOffsetAB,
00469         PE_REF(vmVector3) faceOffsetBA,
00470         const vmMatrix3 & matrixAB,
00471         const vmMatrix3 & matrixBA,
00472         PE_REF(vmVector3) signsA,
00473         PE_REF(vmVector3) signsB,
00474         PE_REF(vmVector3) scalesA,
00475         PE_REF(vmVector3) scalesB )
00476 {
00477         CustomEdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );
00478 }
00479 
00480 float
00481 CustomEdgeEdgeTest_1001(
00482         bool & inVoronoi,
00483         float & tA,
00484         float & tB,
00485         const vmVector3 & hA,
00486         const vmVector3 & hB,
00487         PE_REF(vmVector3) faceOffsetAB,
00488         PE_REF(vmVector3) faceOffsetBA,
00489         const vmMatrix3 & matrixAB,
00490         const vmMatrix3 & matrixBA,
00491         PE_REF(vmVector3) signsA,
00492         PE_REF(vmVector3) signsB,
00493         PE_REF(vmVector3) scalesA,
00494         PE_REF(vmVector3) scalesB )
00495 {
00496         CustomEdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );
00497 }
00498 
00499 float
00500 CustomEdgeEdgeTest_1010(
00501         bool & inVoronoi,
00502         float & tA,
00503         float & tB,
00504         const vmVector3 & hA,
00505         const vmVector3 & hB,
00506         PE_REF(vmVector3) faceOffsetAB,
00507         PE_REF(vmVector3) faceOffsetBA,
00508         const vmMatrix3 & matrixAB,
00509         const vmMatrix3 & matrixBA,
00510         PE_REF(vmVector3) signsA,
00511         PE_REF(vmVector3) signsB,
00512         PE_REF(vmVector3) scalesA,
00513         PE_REF(vmVector3) scalesB )
00514 {
00515         CustomEdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );
00516 }
00517 
00518 #define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter )   \
00519 {                                                                          \
00520    minDistSqr = distSqr;                                                   \
00521    localPointA.set##ac_letter(scalesA.get##ac_letter());                 \
00522    localPointA.set##ad_letter(tA);                                        \
00523    localPointB.set##bc_letter(scalesB.get##bc_letter());                 \
00524    localPointB.set##bd_letter(tB);                                        \
00525    otherFaceDimA = testOtherFaceDimA;                                      \
00526    otherFaceDimB = testOtherFaceDimB;                                      \
00527    featureA = E;                                                           \
00528    featureB = E;                                                           \
00529 }
00530 
00531 void
00532 EdgeEdgeTests(
00533         bool & done,
00534         float & minDistSqr,
00535         vmPoint3 & localPointA,
00536         vmPoint3 & localPointB,
00537         int & otherFaceDimA,
00538         int & otherFaceDimB,
00539         FeatureType & featureA,
00540         FeatureType & featureB,
00541         const vmVector3 & hA,
00542         const vmVector3 & hB,
00543         PE_REF(vmVector3) faceOffsetAB,
00544         PE_REF(vmVector3) faceOffsetBA,
00545         const vmMatrix3 & matrixAB,
00546         const vmMatrix3 & matrixBA,
00547         PE_REF(vmVector3) signsA,
00548         PE_REF(vmVector3) signsB,
00549         PE_REF(vmVector3) scalesA,
00550         PE_REF(vmVector3) scalesB,
00551         bool first )
00552 {
00553 
00554         float distSqr;
00555         float tA, tB;
00556 
00557         int testOtherFaceDimA, testOtherFaceDimB;
00558 
00559         testOtherFaceDimA = 0;
00560         testOtherFaceDimB = 0;
00561 
00562         distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00563                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00564 
00565         if ( first ) {
00566                 EdgeEdge_SetNewMin( X, Y, X, Y );
00567         } else {
00568                 if ( distSqr < minDistSqr ) {
00569                         EdgeEdge_SetNewMin( X, Y, X, Y );
00570                 }
00571         }
00572 
00573         if ( done )
00574                 return;
00575 
00576         signsA.setX( -signsA.getX() );
00577         scalesA.setX( -scalesA.getX() );
00578 
00579         distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00580                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00581 
00582         if ( distSqr < minDistSqr ) {
00583                 EdgeEdge_SetNewMin( X, Y, X, Y );
00584         }
00585 
00586         if ( done )
00587                 return;
00588 
00589         signsB.setX( -signsB.getX() );
00590         scalesB.setX( -scalesB.getX() );
00591 
00592         distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00593                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00594 
00595         if ( distSqr < minDistSqr ) {
00596                 EdgeEdge_SetNewMin( X, Y, X, Y );
00597         }
00598 
00599         if ( done )
00600                 return;
00601 
00602         signsA.setX( -signsA.getX() );
00603         scalesA.setX( -scalesA.getX() );
00604 
00605         distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00606                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00607 
00608         if ( distSqr < minDistSqr ) {
00609                 EdgeEdge_SetNewMin( X, Y, X, Y );
00610         }
00611 
00612         if ( done )
00613                 return;
00614 
00615         testOtherFaceDimA = 1;
00616         testOtherFaceDimB = 0;
00617         signsB.setX( -signsB.getX() );
00618         scalesB.setX( -scalesB.getX() );
00619 
00620         distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00621                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00622 
00623         if ( distSqr < minDistSqr ) {
00624                 EdgeEdge_SetNewMin( Y, X, X, Y );
00625         }
00626 
00627         if ( done )
00628                 return;
00629 
00630         signsA.setY( -signsA.getY() );
00631         scalesA.setY( -scalesA.getY() );
00632 
00633         distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00634                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00635 
00636         if ( distSqr < minDistSqr ) {
00637                 EdgeEdge_SetNewMin( Y, X, X, Y );
00638         }
00639 
00640         if ( done )
00641                 return;
00642 
00643         signsB.setX( -signsB.getX() );
00644         scalesB.setX( -scalesB.getX() );
00645 
00646         distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00647                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00648 
00649         if ( distSqr < minDistSqr ) {
00650                 EdgeEdge_SetNewMin( Y, X, X, Y );
00651         }
00652 
00653         if ( done )
00654                 return;
00655 
00656         signsA.setY( -signsA.getY() );
00657         scalesA.setY( -scalesA.getY() );
00658 
00659         distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00660                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00661 
00662         if ( distSqr < minDistSqr ) {
00663                 EdgeEdge_SetNewMin( Y, X, X, Y );
00664         }
00665 
00666         if ( done )
00667                 return;
00668 
00669         testOtherFaceDimA = 0;
00670         testOtherFaceDimB = 1;
00671         signsB.setX( -signsB.getX() );
00672         scalesB.setX( -scalesB.getX() );
00673 
00674         distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00675                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00676 
00677         if ( distSqr < minDistSqr ) {
00678                 EdgeEdge_SetNewMin( X, Y, Y, X );
00679         }
00680 
00681         if ( done )
00682                 return;
00683 
00684         signsA.setX( -signsA.getX() );
00685         scalesA.setX( -scalesA.getX() );
00686 
00687         distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00688                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00689 
00690         if ( distSqr < minDistSqr ) {
00691                 EdgeEdge_SetNewMin( X, Y, Y, X );
00692         }
00693 
00694         if ( done )
00695                 return;
00696 
00697         signsB.setY( -signsB.getY() );
00698         scalesB.setY( -scalesB.getY() );
00699 
00700         distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00701                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00702 
00703         if ( distSqr < minDistSqr ) {
00704                 EdgeEdge_SetNewMin( X, Y, Y, X );
00705         }
00706 
00707         if ( done )
00708                 return;
00709 
00710         signsA.setX( -signsA.getX() );
00711         scalesA.setX( -scalesA.getX() );
00712 
00713         distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00714                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00715 
00716         if ( distSqr < minDistSqr ) {
00717                 EdgeEdge_SetNewMin( X, Y, Y, X );
00718         }
00719 
00720         if ( done )
00721                 return;
00722 
00723         testOtherFaceDimA = 1;
00724         testOtherFaceDimB = 1;
00725         signsB.setY( -signsB.getY() );
00726         scalesB.setY( -scalesB.getY() );
00727 
00728         distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00729                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00730 
00731         if ( distSqr < minDistSqr ) {
00732                 EdgeEdge_SetNewMin( Y, X, Y, X );
00733         }
00734 
00735         if ( done )
00736                 return;
00737 
00738         signsA.setY( -signsA.getY() );
00739         scalesA.setY( -scalesA.getY() );
00740 
00741         distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00742                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00743 
00744         if ( distSqr < minDistSqr ) {
00745                 EdgeEdge_SetNewMin( Y, X, Y, X );
00746         }
00747 
00748         if ( done )
00749                 return;
00750 
00751         signsB.setY( -signsB.getY() );
00752         scalesB.setY( -scalesB.getY() );
00753 
00754         distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00755                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00756 
00757         if ( distSqr < minDistSqr ) {
00758                 EdgeEdge_SetNewMin( Y, X, Y, X );
00759         }
00760 
00761         if ( done )
00762                 return;
00763 
00764         signsA.setY( -signsA.getY() );
00765         scalesA.setY( -scalesA.getY() );
00766 
00767         distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
00768                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
00769 
00770         if ( distSqr < minDistSqr ) {
00771                 EdgeEdge_SetNewMin( Y, X, Y, X );
00772         }
00773 }
00774 
00775 
00776 float
00777 boxBoxDistance(vmVector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
00778                            PE_REF(Box) boxA, const vmTransform3 & transformA, PE_REF(Box) boxB,
00779                            const vmTransform3 & transformB,
00780                            float distanceThreshold)
00781 {
00782         vmMatrix3 identity;
00783         identity = vmMatrix3::identity();
00784         vmVector3 ident[3];
00785         ident[0] = identity.getCol0();
00786         ident[1] = identity.getCol1();
00787         ident[2] = identity.getCol2();
00788 
00789         // get relative transformations
00790 
00791         vmTransform3 transformAB, transformBA;
00792         vmMatrix3 matrixAB, matrixBA;
00793         vmVector3 offsetAB, offsetBA;
00794 
00795         transformAB = orthoInverse(transformA) * transformB;
00796         transformBA = orthoInverse(transformAB);
00797 
00798         matrixAB = transformAB.getUpper3x3();
00799         offsetAB = transformAB.getTranslation();
00800         matrixBA = transformBA.getUpper3x3();
00801         offsetBA = transformBA.getTranslation();
00802 
00803         vmMatrix3 absMatrixAB = absPerElem(matrixAB);
00804         vmMatrix3 absMatrixBA = absPerElem(matrixBA);
00805 
00806         // find separating axis with largest gap between projections
00807 
00808         BoxSepAxisType axisType;
00809         vmVector3 axisA(0.0f), axisB(0.0f);
00810         float gap, maxGap;
00811         int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;
00812 
00813         // face axes
00814 
00815         vmVector3  gapsA   = absPerElem(offsetAB) - boxA.mHalf - absMatrixAB * boxB.mHalf;
00816 
00817         AaxisTest(0,X,true);
00818         AaxisTest(1,Y,false);
00819         AaxisTest(2,Z,false);
00820 
00821         vmVector3  gapsB   = absPerElem(offsetBA) - boxB.mHalf - absMatrixBA * boxA.mHalf;
00822 
00823         BaxisTest(0,X);
00824         BaxisTest(1,Y);
00825         BaxisTest(2,Z);
00826 
00827         // cross product axes
00828 
00829         // 外積が0のときの対策
00830         absMatrixAB += vmMatrix3(1.0e-5f);
00831         absMatrixBA += vmMatrix3(1.0e-5f);
00832 
00833         vmMatrix3 lsqrs, projOffset, projAhalf, projBhalf;
00834 
00835         lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
00836                                    mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) );
00837         lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
00838                                    mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );
00839         lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) +
00840                                    mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );
00841 
00842         projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY());
00843         projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ());
00844         projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX());
00845 
00846         projAhalf.setCol0(absMatrixBA.getCol1() * boxA.mHalf.getZ() + absMatrixBA.getCol2() * boxA.mHalf.getY());
00847         projAhalf.setCol1(absMatrixBA.getCol2() * boxA.mHalf.getX() + absMatrixBA.getCol0() * boxA.mHalf.getZ());
00848         projAhalf.setCol2(absMatrixBA.getCol0() * boxA.mHalf.getY() + absMatrixBA.getCol1() * boxA.mHalf.getX());
00849 
00850         projBhalf.setCol0(absMatrixAB.getCol1() * boxB.mHalf.getZ() + absMatrixAB.getCol2() * boxB.mHalf.getY());
00851         projBhalf.setCol1(absMatrixAB.getCol2() * boxB.mHalf.getX() + absMatrixAB.getCol0() * boxB.mHalf.getZ());
00852         projBhalf.setCol2(absMatrixAB.getCol0() * boxB.mHalf.getY() + absMatrixAB.getCol1() * boxB.mHalf.getX());
00853 
00854         vmMatrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);
00855 
00856         CrossAxisTest(0,0,X);
00857         CrossAxisTest(0,1,Y);
00858         CrossAxisTest(0,2,Z);
00859         CrossAxisTest(1,0,X);
00860         CrossAxisTest(1,1,Y);
00861         CrossAxisTest(1,2,Z);
00862         CrossAxisTest(2,0,X);
00863         CrossAxisTest(2,1,Y);
00864         CrossAxisTest(2,2,Z);
00865 
00866         // need to pick the face on each box whose normal best matches the separating axis.
00867         // will transform vectors to be in the coordinate system of this face to simplify things later.
00868         // for this, a permutation matrix can be used, which the next section computes.
00869 
00870         int dimA[3], dimB[3];
00871 
00872         if ( axisType == A_AXIS ) {
00873                 if ( dot(axisA,offsetAB) < 0.0f )
00874                         axisA = -axisA;
00875                 axisB = matrixBA * -axisA;
00876 
00877                 vmVector3 absAxisB = vmVector3(absPerElem(axisB));
00878 
00879                 if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) )
00880                         faceDimB = 0;
00881                 else if ( absAxisB[1] > absAxisB[2] )
00882                         faceDimB = 1;
00883                 else
00884                         faceDimB = 2;
00885         } else if ( axisType == B_AXIS ) {
00886                 if ( dot(axisB,offsetBA) < 0.0f )
00887                         axisB = -axisB;
00888                 axisA = matrixAB * -axisB;
00889 
00890                 vmVector3 absAxisA = vmVector3(absPerElem(axisA));
00891 
00892                 if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) )
00893                         faceDimA = 0;
00894                 else if ( absAxisA[1] > absAxisA[2] )
00895                         faceDimA = 1;
00896                 else
00897                         faceDimA = 2;
00898         }
00899 
00900         if ( axisType == CROSS_AXIS ) {
00901                 if ( dot(axisA,offsetAB) < 0.0f )
00902                         axisA = -axisA;
00903                 axisB = matrixBA * -axisA;
00904 
00905                 vmVector3 absAxisA = vmVector3(absPerElem(axisA));
00906                 vmVector3 absAxisB = vmVector3(absPerElem(axisB));
00907 
00908                 dimA[1] = edgeDimA;
00909                 dimB[1] = edgeDimB;
00910 
00911                 if ( edgeDimA == 0 ) {
00912                         if ( absAxisA[1] > absAxisA[2] ) {
00913                                 dimA[0] = 2;
00914                                 dimA[2] = 1;
00915                         } else                             {
00916                                 dimA[0] = 1;
00917                                 dimA[2] = 2;
00918                         }
00919                 } else if ( edgeDimA == 1 ) {
00920                         if ( absAxisA[2] > absAxisA[0] ) {
00921                                 dimA[0] = 0;
00922                                 dimA[2] = 2;
00923                         } else                             {
00924                                 dimA[0] = 2;
00925                                 dimA[2] = 0;
00926                         }
00927                 } else {
00928                         if ( absAxisA[0] > absAxisA[1] ) {
00929                                 dimA[0] = 1;
00930                                 dimA[2] = 0;
00931                         } else                             {
00932                                 dimA[0] = 0;
00933                                 dimA[2] = 1;
00934                         }
00935                 }
00936 
00937                 if ( edgeDimB == 0 ) {
00938                         if ( absAxisB[1] > absAxisB[2] ) {
00939                                 dimB[0] = 2;
00940                                 dimB[2] = 1;
00941                         } else                             {
00942                                 dimB[0] = 1;
00943                                 dimB[2] = 2;
00944                         }
00945                 } else if ( edgeDimB == 1 ) {
00946                         if ( absAxisB[2] > absAxisB[0] ) {
00947                                 dimB[0] = 0;
00948                                 dimB[2] = 2;
00949                         } else                             {
00950                                 dimB[0] = 2;
00951                                 dimB[2] = 0;
00952                         }
00953                 } else {
00954                         if ( absAxisB[0] > absAxisB[1] ) {
00955                                 dimB[0] = 1;
00956                                 dimB[2] = 0;
00957                         } else                             {
00958                                 dimB[0] = 0;
00959                                 dimB[2] = 1;
00960                         }
00961                 }
00962         } else {
00963                 dimA[2] = faceDimA;
00964                 dimA[0] = (faceDimA+1)%3;
00965                 dimA[1] = (faceDimA+2)%3;
00966                 dimB[2] = faceDimB;
00967                 dimB[0] = (faceDimB+1)%3;
00968                 dimB[1] = (faceDimB+2)%3;
00969         }
00970 
00971         vmMatrix3 aperm_col, bperm_col;
00972 
00973         aperm_col.setCol0(ident[dimA[0]]);
00974         aperm_col.setCol1(ident[dimA[1]]);
00975         aperm_col.setCol2(ident[dimA[2]]);
00976 
00977         bperm_col.setCol0(ident[dimB[0]]);
00978         bperm_col.setCol1(ident[dimB[1]]);
00979         bperm_col.setCol2(ident[dimB[2]]);
00980 
00981         vmMatrix3 aperm_row, bperm_row;
00982 
00983         aperm_row = transpose(aperm_col);
00984         bperm_row = transpose(bperm_col);
00985 
00986         // permute all box parameters to be in the face coordinate systems
00987 
00988         vmMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;
00989         vmMatrix3 matrixBA_perm = transpose(matrixAB_perm);
00990 
00991         vmVector3 offsetAB_perm, offsetBA_perm;
00992 
00993         offsetAB_perm = aperm_row * offsetAB;
00994         offsetBA_perm = bperm_row * offsetBA;
00995 
00996         vmVector3 halfA_perm, halfB_perm;
00997 
00998         halfA_perm = aperm_row * boxA.mHalf;
00999         halfB_perm = bperm_row * boxB.mHalf;
01000 
01001         // compute the vector between the centers of each face, in each face's coordinate frame
01002 
01003         vmVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;
01004 
01005         signsA_perm = copySignPerElem(vmVector3(1.0f),aperm_row * axisA);
01006         signsB_perm = copySignPerElem(vmVector3(1.0f),bperm_row * axisB);
01007         scalesA_perm = mulPerElem( signsA_perm, halfA_perm );
01008         scalesB_perm = mulPerElem( signsB_perm, halfB_perm );
01009 
01010         faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ();
01011         faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() );
01012 
01013         faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ();
01014         faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() );
01015 
01016         if ( maxGap < 0.0f ) {
01017                 // if boxes overlap, this will separate the faces for finding points of penetration.
01018 
01019                 faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;
01020                 faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f;
01021         }
01022 
01023         // for each vertex/face or edge/edge pair of the two faces, find the closest points.
01024         //
01025         // these points each have an associated box feature (vertex, edge, or face).  if each
01026         // point is in the external Voronoi region of the other's feature, they are the
01027         // closest points of the boxes, and the algorithm can exit.
01028         //
01029         // the feature pairs are arranged so that in the general case, the first test will
01030         // succeed.  degenerate cases (parallel faces) may require up to all tests in the
01031         // worst case.
01032         //
01033         // if for some reason no case passes the Voronoi test, the features with the minimum
01034         // distance are returned.
01035 
01036         vmPoint3 localPointA_perm, localPointB_perm;
01037         float minDistSqr;
01038         bool done;
01039 
01040         vmVector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );
01041 
01042         localPointA_perm.setZ( scalesA_perm.getZ() );
01043         localPointB_perm.setZ( scalesB_perm.getZ() );
01044         scalesA_perm.setZ(0.0f);
01045         scalesB_perm.setZ(0.0f);
01046 
01047         int otherFaceDimA, otherFaceDimB;
01048         FeatureType featureA, featureB;
01049 
01050         if ( axisType == CROSS_AXIS ) {
01051                 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
01052                                            otherFaceDimA, otherFaceDimB, featureA, featureB,
01053                                            hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01054                                            matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
01055                                            scalesA_perm, scalesB_perm, true );
01056 
01057                 if ( !done ) {
01058                         VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
01059                                                            featureA, featureB,
01060                                                            hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01061                                                            matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );
01062 
01063                         if ( !done ) {
01064                                 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
01065                                                                    featureA, featureB,
01066                                                                    hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01067                                                                    matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );
01068                         }
01069                 }
01070         } else if ( axisType == B_AXIS ) {
01071                 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
01072                                                    featureA, featureB,
01073                                                    hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01074                                                    matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true );
01075 
01076                 if ( !done ) {
01077                         VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
01078                                                            featureA, featureB,
01079                                                            hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01080                                                            matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );
01081 
01082                         if ( !done ) {
01083                                 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
01084                                                            otherFaceDimA, otherFaceDimB, featureA, featureB,
01085                                                            hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01086                                                            matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
01087                                                            scalesA_perm, scalesB_perm, false );
01088                         }
01089                 }
01090         } else {
01091                 VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
01092                                                    featureA, featureB,
01093                                                    hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01094                                                    matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true );
01095 
01096                 if ( !done ) {
01097                         VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
01098                                                            featureA, featureB,
01099                                                            hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01100                                                            matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );
01101 
01102                         if ( !done ) {
01103                                 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
01104                                                            otherFaceDimA, otherFaceDimB, featureA, featureB,
01105                                                            hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
01106                                                            matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
01107                                                            scalesA_perm, scalesB_perm, false );
01108                         }
01109                 }
01110         }
01111 
01112         // convert local points from face-local to box-local coordinate system
01113 
01114         
01115         boxPointA.localPoint = vmPoint3( aperm_col * vmVector3( localPointA_perm )) ;
01116         boxPointB.localPoint = vmPoint3( bperm_col * vmVector3( localPointB_perm )) ;
01117 
01118 #if 0
01119         // find which features of the boxes are involved.
01120         // the only feature pairs which occur in this function are VF, FV, and EE, even though the
01121         // closest points might actually lie on sub-features, as in a VF contact might be used for
01122         // what's actually a VV contact.  this means some feature pairs could possibly seem distinct
01123         // from others, although their contact positions are the same.  don't know yet whether this
01124         // matters.
01125 
01126         int sA[3], sB[3];
01127 
01128         sA[0] = boxPointA.localPoint.getX() > 0.0f;
01129         sA[1] = boxPointA.localPoint.getY() > 0.0f;
01130         sA[2] = boxPointA.localPoint.getZ() > 0.0f;
01131 
01132         sB[0] = boxPointB.localPoint.getX() > 0.0f;
01133         sB[1] = boxPointB.localPoint.getY() > 0.0f;
01134         sB[2] = boxPointB.localPoint.getZ() > 0.0f;
01135 
01136         if ( featureA == F ) {
01137                 boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] );
01138         } else if ( featureA == E ) {
01139                 boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] );
01140         } else {
01141                 boxPointA.setVertexFeature( sA[0], sA[1], sA[2] );
01142         }
01143 
01144         if ( featureB == F ) {
01145                 boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] );
01146         } else if ( featureB == E ) {
01147                 boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] );
01148         } else {
01149                 boxPointB.setVertexFeature( sB[0], sB[1], sB[2] );
01150         }
01151 #endif
01152 
01153         normal = transformA * axisA;
01154 
01155         if ( maxGap < 0.0f ) {
01156                 return (maxGap);
01157         } else {
01158                 return (sqrtf( minDistSqr ));
01159         }
01160 }