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