00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00036
00037
00038 static const float voronoiTol = -1.0e-5f;
00039
00040
00041
00042
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
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
00130
00131 vmVector3 corner =
00132 vmVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );
00133
00134
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
00149
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
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
00371
00372
00373
00374
00375
00376
00377
00378
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
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
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
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
00828
00829
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
00867
00868
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
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
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
01018
01019 faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;
01020 faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f;
01021 }
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034
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
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
01120
01121
01122
01123
01124
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 }