2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.
\r
5 Physics Effects is open software; you can redistribute it and/or
\r
6 modify it under the terms of the BSD License.
\r
8 Physics Effects is distributed in the hope that it will be useful,
\r
9 but WITHOUT ANY WARRANTY; without even the implied warranty of
\r
10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
\r
11 See the BSD License for more details.
\r
13 A copy of the BSD License is distributed with
\r
14 Physics Effects under the filename: physics_effects_license.txt
\r
17 #include "../../../include/physics_effects/base_level/collision/pfx_box.h"
\r
18 #include "pfx_contact_box_box.h"
\r
21 namespace PhysicsEffects {
\r
25 A_AXIS, B_AXIS, CROSS_AXIS
\r
28 //-------------------------------------------------------------------------------------------------
\r
29 // voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.
\r
30 //-------------------------------------------------------------------------------------------------
\r
32 static const PfxFloat voronoiTol = -1.0e-5f;
\r
34 //-------------------------------------------------------------------------------------------------
\r
35 // separating axis tests: gaps along each axis are computed, and the axis with the maximum
\r
36 // gap is stored. cross product axes are normalized.
\r
37 //-------------------------------------------------------------------------------------------------
\r
39 #define AaxisTest( dim, letter, first ) \
\r
43 maxGap = gap = gapsA.get##letter(); \
\r
44 if ( gap > distanceThreshold ) return gap; \
\r
45 axisType = A_AXIS; \
\r
47 axisA = ident[dim]; \
\r
51 gap = gapsA.get##letter(); \
\r
52 if ( gap > distanceThreshold ) return gap; \
\r
53 else if ( gap > maxGap ) \
\r
56 axisType = A_AXIS; \
\r
58 axisA = ident[dim]; \
\r
64 #define BaxisTest( dim, letter ) \
\r
66 gap = gapsB.get##letter(); \
\r
67 if ( gap > distanceThreshold ) return gap; \
\r
68 else if ( gap > maxGap ) \
\r
71 axisType = B_AXIS; \
\r
73 axisB = ident[dim]; \
\r
77 #define CrossAxisTest( dima, dimb, letterb ) \
\r
79 const PfxFloat lsqr_tolerance = 1.0e-30f; \
\r
82 lsqr = lsqrs.getCol##dima().get##letterb(); \
\r
84 if ( lsqr > lsqr_tolerance ) \
\r
86 PfxFloat l_recip = 1.0f / sqrtf( lsqr ); \
\r
87 gap = PfxFloat(gapsAxB.getCol##dima().get##letterb()) * l_recip; \
\r
89 if ( gap > distanceThreshold ) \
\r
94 if ( gap > maxGap ) \
\r
97 axisType = CROSS_AXIS; \
\r
100 axisA = cross(ident[dima],matrixAB.getCol##dimb()) * l_recip; \
\r
105 //-------------------------------------------------------------------------------------------------
\r
106 // tests whether a vertex of box B and a face of box A are the closest features
\r
107 //-------------------------------------------------------------------------------------------------
\r
112 PfxBool & inVoronoi,
\r
115 const PfxVector3 & hA,
\r
116 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
117 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
118 const PfxMatrix3 & matrixAB,
\r
119 const PfxMatrix3 & matrixBA,
\r
120 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
121 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )
\r
123 // compute a corner of box B in A's coordinate system
\r
125 PfxVector3 corner =
\r
126 PfxVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );
\r
128 // compute the parameters of the point on A, closest to this corner
\r
135 else if ( t0 < -hA[0] )
\r
139 else if ( t1 < -hA[1] )
\r
142 // do the Voronoi test: already know the point on B is in the Voronoi region of the
\r
143 // point on A, check the reverse.
\r
145 PfxVector3 facePointB =
\r
146 PfxVector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );
\r
148 inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) &&
\r
149 ( facePointB[1] >= voronoiTol * facePointB[0] ) &&
\r
150 ( facePointB[2] >= voronoiTol * facePointB[1] ) );
\r
152 return (SCE_PFX_SQR( corner[0] - t0 ) + SCE_PFX_SQR( corner[1] - t1 ) + SCE_PFX_SQR( corner[2] ));
\r
155 #define VertexBFaceA_SetNewMin() \
\r
157 minDistSqr = distSqr; \
\r
158 localPointA.setX(t0); \
\r
159 localPointA.setY(t1); \
\r
160 localPointB.setX( scalesB.getX() ); \
\r
161 localPointB.setY( scalesB.getY() ); \
\r
167 PfxFloat & minDistSqr,
\r
168 PfxPoint3 & localPointA,
\r
169 PfxPoint3 & localPointB,
\r
170 const PfxVector3 & hA,
\r
171 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
172 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
173 const PfxMatrix3 & matrixAB,
\r
174 const PfxMatrix3 & matrixBA,
\r
175 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
176 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB,
\r
182 distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
\r
183 matrixAB, matrixBA, signsB, scalesB );
\r
186 VertexBFaceA_SetNewMin();
\r
188 if ( distSqr < minDistSqr ) {
\r
189 VertexBFaceA_SetNewMin();
\r
196 signsB.setX( -signsB.getX() );
\r
197 scalesB.setX( -scalesB.getX() );
\r
199 distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
\r
200 matrixAB, matrixBA, signsB, scalesB );
\r
202 if ( distSqr < minDistSqr ) {
\r
203 VertexBFaceA_SetNewMin();
\r
209 signsB.setY( -signsB.getY() );
\r
210 scalesB.setY( -scalesB.getY() );
\r
212 distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
\r
213 matrixAB, matrixBA, signsB, scalesB );
\r
215 if ( distSqr < minDistSqr ) {
\r
216 VertexBFaceA_SetNewMin();
\r
222 signsB.setX( -signsB.getX() );
\r
223 scalesB.setX( -scalesB.getX() );
\r
225 distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
\r
226 matrixAB, matrixBA, signsB, scalesB );
\r
228 if ( distSqr < minDistSqr ) {
\r
229 VertexBFaceA_SetNewMin();
\r
233 //-------------------------------------------------------------------------------------------------
\r
234 // VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features
\r
235 //-------------------------------------------------------------------------------------------------
\r
240 PfxBool & inVoronoi,
\r
243 const PfxVector3 & hB,
\r
244 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
245 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
246 const PfxMatrix3 & matrixAB,
\r
247 const PfxMatrix3 & matrixBA,
\r
248 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
249 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA )
\r
251 PfxVector3 corner =
\r
252 PfxVector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );
\r
259 else if ( t0 < -hB[0] )
\r
263 else if ( t1 < -hB[1] )
\r
266 PfxVector3 facePointA =
\r
267 PfxVector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );
\r
269 inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) &&
\r
270 ( facePointA[1] >= voronoiTol * facePointA[0] ) &&
\r
271 ( facePointA[2] >= voronoiTol * facePointA[1] ) );
\r
273 return (SCE_PFX_SQR( corner[0] - t0 ) + SCE_PFX_SQR( corner[1] - t1 ) + SCE_PFX_SQR( corner[2] ));
\r
276 #define VertexAFaceB_SetNewMin() \
\r
278 minDistSqr = distSqr; \
\r
279 localPointB.setX(t0); \
\r
280 localPointB.setY(t1); \
\r
281 localPointA.setX( scalesA.getX() ); \
\r
282 localPointA.setY( scalesA.getY() ); \
\r
288 PfxFloat & minDistSqr,
\r
289 PfxPoint3 & localPointA,
\r
290 PfxPoint3 & localPointB,
\r
291 const PfxVector3 & hB,
\r
292 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
293 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
294 const PfxMatrix3 & matrixAB,
\r
295 const PfxMatrix3 & matrixBA,
\r
296 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
297 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
\r
303 distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
\r
304 matrixAB, matrixBA, signsA, scalesA );
\r
307 VertexAFaceB_SetNewMin();
\r
309 if ( distSqr < minDistSqr ) {
\r
310 VertexAFaceB_SetNewMin();
\r
317 signsA.setX( -signsA.getX() );
\r
318 scalesA.setX( -scalesA.getX() );
\r
320 distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
\r
321 matrixAB, matrixBA, signsA, scalesA );
\r
323 if ( distSqr < minDistSqr ) {
\r
324 VertexAFaceB_SetNewMin();
\r
330 signsA.setY( -signsA.getY() );
\r
331 scalesA.setY( -scalesA.getY() );
\r
333 distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
\r
334 matrixAB, matrixBA, signsA, scalesA );
\r
336 if ( distSqr < minDistSqr ) {
\r
337 VertexAFaceB_SetNewMin();
\r
343 signsA.setX( -signsA.getX() );
\r
344 scalesA.setX( -scalesA.getX() );
\r
346 distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
\r
347 matrixAB, matrixBA, signsA, scalesA );
\r
349 if ( distSqr < minDistSqr ) {
\r
350 VertexAFaceB_SetNewMin();
\r
354 //-------------------------------------------------------------------------------------------------
\r
357 // tests whether a pair of edges are the closest features
\r
359 // note on the shorthand:
\r
360 // 'a' & 'b' refer to the edges.
\r
361 // 'c' is the dimension of the axis that points from the face center to the edge Center
\r
362 // 'd' is the dimension of the edge Direction
\r
363 // the dimension of the face normal is 2
\r
364 //-------------------------------------------------------------------------------------------------
\r
366 #define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \
\r
368 PfxVector3 edgeOffsetAB; \
\r
369 PfxVector3 edgeOffsetBA; \
\r
371 edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \
\r
372 edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \
\r
374 edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \
\r
375 edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \
\r
377 PfxFloat dirDot = matrixAB.getCol##bd().get##ad_letter(); \
\r
378 PfxFloat denom = 1.0f - dirDot*dirDot; \
\r
379 PfxFloat edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \
\r
380 PfxFloat edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \
\r
382 if ( denom == 0.0f ) \
\r
388 tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \
\r
391 if ( tA < -hA[ad] ) tA = -hA[ad]; \
\r
392 else if ( tA > hA[ad] ) tA = hA[ad]; \
\r
394 tB = tA * dirDot + edgeOffsetBA_bd; \
\r
396 if ( tB < -hB[bd] ) \
\r
399 tA = tB * dirDot + edgeOffsetAB_ad; \
\r
401 if ( tA < -hA[ad] ) tA = -hA[ad]; \
\r
402 else if ( tA > hA[ad] ) tA = hA[ad]; \
\r
404 else if ( tB > hB[bd] ) \
\r
407 tA = tB * dirDot + edgeOffsetAB_ad; \
\r
409 if ( tA < -hA[ad] ) tA = -hA[ad]; \
\r
410 else if ( tA > hA[ad] ) tA = hA[ad]; \
\r
413 PfxVector3 edgeOffAB = PfxVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\
\r
414 PfxVector3 edgeOffBA = PfxVector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\
\r
416 inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \
\r
417 ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \
\r
418 ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \
\r
419 ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \
\r
421 edgeOffAB[ad] -= tA; \
\r
422 edgeOffBA[bd] -= tB; \
\r
424 return dot(edgeOffAB,edgeOffAB); \
\r
429 PfxBool & inVoronoi,
\r
432 const PfxVector3 & hA,
\r
433 const PfxVector3 & hB,
\r
434 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
435 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
436 const PfxMatrix3 & matrixAB,
\r
437 const PfxMatrix3 & matrixBA,
\r
438 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
439 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
440 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
\r
441 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )
\r
443 EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );
\r
448 PfxBool & inVoronoi,
\r
451 const PfxVector3 & hA,
\r
452 const PfxVector3 & hB,
\r
453 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
454 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
455 const PfxMatrix3 & matrixAB,
\r
456 const PfxMatrix3 & matrixBA,
\r
457 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
458 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
459 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
\r
460 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )
\r
462 EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );
\r
467 PfxBool & inVoronoi,
\r
470 const PfxVector3 & hA,
\r
471 const PfxVector3 & hB,
\r
472 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
473 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
474 const PfxMatrix3 & matrixAB,
\r
475 const PfxMatrix3 & matrixBA,
\r
476 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
477 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
478 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
\r
479 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )
\r
481 EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );
\r
486 PfxBool & inVoronoi,
\r
489 const PfxVector3 & hA,
\r
490 const PfxVector3 & hB,
\r
491 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
492 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
493 const PfxMatrix3 & matrixAB,
\r
494 const PfxMatrix3 & matrixBA,
\r
495 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
496 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
497 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
\r
498 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )
\r
500 EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );
\r
503 #define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \
\r
505 minDistSqr = distSqr; \
\r
506 localPointA.set##ac_letter(scalesA.get##ac_letter()); \
\r
507 localPointA.set##ad_letter(tA); \
\r
508 localPointB.set##bc_letter(scalesB.get##bc_letter()); \
\r
509 localPointB.set##bd_letter(tB); \
\r
510 otherFaceDimA = testOtherFaceDimA; \
\r
511 otherFaceDimB = testOtherFaceDimB; \
\r
517 PfxFloat & minDistSqr,
\r
518 PfxPoint3 & localPointA,
\r
519 PfxPoint3 & localPointB,
\r
520 int & otherFaceDimA,
\r
521 int & otherFaceDimB,
\r
522 const PfxVector3 & hA,
\r
523 const PfxVector3 & hB,
\r
524 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,
\r
525 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,
\r
526 const PfxMatrix3 & matrixAB,
\r
527 const PfxMatrix3 & matrixBA,
\r
528 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
\r
529 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,
\r
530 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
\r
531 PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB,
\r
537 int testOtherFaceDimA, testOtherFaceDimB;
\r
539 testOtherFaceDimA = 0;
\r
540 testOtherFaceDimB = 0;
\r
542 distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
543 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
546 EdgeEdge_SetNewMin( X, Y, X, Y );
\r
548 if ( distSqr < minDistSqr ) {
\r
549 EdgeEdge_SetNewMin( X, Y, X, Y );
\r
556 signsA.setX( -signsA.getX() );
\r
557 scalesA.setX( -scalesA.getX() );
\r
559 distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
560 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
562 if ( distSqr < minDistSqr ) {
\r
563 EdgeEdge_SetNewMin( X, Y, X, Y );
\r
569 signsB.setX( -signsB.getX() );
\r
570 scalesB.setX( -scalesB.getX() );
\r
572 distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
573 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
575 if ( distSqr < minDistSqr ) {
\r
576 EdgeEdge_SetNewMin( X, Y, X, Y );
\r
582 signsA.setX( -signsA.getX() );
\r
583 scalesA.setX( -scalesA.getX() );
\r
585 distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
586 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
588 if ( distSqr < minDistSqr ) {
\r
589 EdgeEdge_SetNewMin( X, Y, X, Y );
\r
595 testOtherFaceDimA = 1;
\r
596 testOtherFaceDimB = 0;
\r
597 signsB.setX( -signsB.getX() );
\r
598 scalesB.setX( -scalesB.getX() );
\r
600 distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
601 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
603 if ( distSqr < minDistSqr ) {
\r
604 EdgeEdge_SetNewMin( Y, X, X, Y );
\r
610 signsA.setY( -signsA.getY() );
\r
611 scalesA.setY( -scalesA.getY() );
\r
613 distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
614 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
616 if ( distSqr < minDistSqr ) {
\r
617 EdgeEdge_SetNewMin( Y, X, X, Y );
\r
623 signsB.setX( -signsB.getX() );
\r
624 scalesB.setX( -scalesB.getX() );
\r
626 distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
627 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
629 if ( distSqr < minDistSqr ) {
\r
630 EdgeEdge_SetNewMin( Y, X, X, Y );
\r
636 signsA.setY( -signsA.getY() );
\r
637 scalesA.setY( -scalesA.getY() );
\r
639 distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
640 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
642 if ( distSqr < minDistSqr ) {
\r
643 EdgeEdge_SetNewMin( Y, X, X, Y );
\r
649 testOtherFaceDimA = 0;
\r
650 testOtherFaceDimB = 1;
\r
651 signsB.setX( -signsB.getX() );
\r
652 scalesB.setX( -scalesB.getX() );
\r
654 distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
655 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
657 if ( distSqr < minDistSqr ) {
\r
658 EdgeEdge_SetNewMin( X, Y, Y, X );
\r
664 signsA.setX( -signsA.getX() );
\r
665 scalesA.setX( -scalesA.getX() );
\r
667 distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
668 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
670 if ( distSqr < minDistSqr ) {
\r
671 EdgeEdge_SetNewMin( X, Y, Y, X );
\r
677 signsB.setY( -signsB.getY() );
\r
678 scalesB.setY( -scalesB.getY() );
\r
680 distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
681 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
683 if ( distSqr < minDistSqr ) {
\r
684 EdgeEdge_SetNewMin( X, Y, Y, X );
\r
690 signsA.setX( -signsA.getX() );
\r
691 scalesA.setX( -scalesA.getX() );
\r
693 distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
694 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
696 if ( distSqr < minDistSqr ) {
\r
697 EdgeEdge_SetNewMin( X, Y, Y, X );
\r
703 testOtherFaceDimA = 1;
\r
704 testOtherFaceDimB = 1;
\r
705 signsB.setY( -signsB.getY() );
\r
706 scalesB.setY( -scalesB.getY() );
\r
708 distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
709 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
711 if ( distSqr < minDistSqr ) {
\r
712 EdgeEdge_SetNewMin( Y, X, Y, X );
\r
718 signsA.setY( -signsA.getY() );
\r
719 scalesA.setY( -scalesA.getY() );
\r
721 distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
722 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
724 if ( distSqr < minDistSqr ) {
\r
725 EdgeEdge_SetNewMin( Y, X, Y, X );
\r
731 signsB.setY( -signsB.getY() );
\r
732 scalesB.setY( -scalesB.getY() );
\r
734 distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
735 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
737 if ( distSqr < minDistSqr ) {
\r
738 EdgeEdge_SetNewMin( Y, X, Y, X );
\r
744 signsA.setY( -signsA.getY() );
\r
745 scalesA.setY( -scalesA.getY() );
\r
747 distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
\r
748 matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
\r
750 if ( distSqr < minDistSqr ) {
\r
751 EdgeEdge_SetNewMin( Y, X, Y, X );
\r
755 PfxFloat pfxContactBoxBox(
\r
756 PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
\r
757 void *shapeA,const PfxTransform3 &transformA,
\r
758 void *shapeB,const PfxTransform3 &transformB,
\r
759 PfxFloat distanceThreshold)
\r
761 PfxBox &boxA = *((PfxBox*)shapeA);
\r
762 PfxBox &boxB = *((PfxBox*)shapeB);
\r
764 PfxVector3 ident[3] = {
\r
765 PfxVector3(1.0,0.0,0.0),
\r
766 PfxVector3(0.0,1.0,0.0),
\r
767 PfxVector3(0.0,0.0,1.0),
\r
770 // get relative transformations
\r
772 PfxTransform3 transformAB, transformBA;
\r
773 PfxMatrix3 matrixAB, matrixBA;
\r
774 PfxVector3 offsetAB, offsetBA;
\r
776 transformAB = orthoInverse(transformA) * transformB;
\r
777 transformBA = orthoInverse(transformAB);
\r
779 matrixAB = transformAB.getUpper3x3();
\r
780 offsetAB = transformAB.getTranslation();
\r
781 matrixBA = transformBA.getUpper3x3();
\r
782 offsetBA = transformBA.getTranslation();
\r
784 PfxMatrix3 absMatrixAB = absPerElem(matrixAB);
\r
785 PfxMatrix3 absMatrixBA = absPerElem(matrixBA);
\r
787 // find separating axis with largest gap between projections
\r
789 BoxSepAxisType axisType;
\r
790 PfxVector3 axisA(0.0f), axisB(0.0f);
\r
791 PfxFloat gap, maxGap;
\r
792 int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;
\r
796 PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - absMatrixAB * boxB.m_half;
\r
798 AaxisTest(0,X,true);
\r
799 AaxisTest(1,Y,false);
\r
800 AaxisTest(2,Z,false);
\r
802 PfxVector3 gapsB = absPerElem(offsetBA) - boxB.m_half - absMatrixBA * boxA.m_half;
\r
808 // cross product axes
\r
811 absMatrixAB += PfxMatrix3(1.0e-5f);
\r
812 absMatrixBA += PfxMatrix3(1.0e-5f);
\r
814 PfxMatrix3 lsqrs, projOffset, projAhalf, projBhalf;
\r
816 lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
\r
817 mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) );
\r
818 lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
\r
819 mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );
\r
820 lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) +
\r
821 mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );
\r
823 projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY());
\r
824 projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ());
\r
825 projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX());
\r
827 projAhalf.setCol0(absMatrixBA.getCol1() * boxA.m_half.getZ() + absMatrixBA.getCol2() * boxA.m_half.getY());
\r
828 projAhalf.setCol1(absMatrixBA.getCol2() * boxA.m_half.getX() + absMatrixBA.getCol0() * boxA.m_half.getZ());
\r
829 projAhalf.setCol2(absMatrixBA.getCol0() * boxA.m_half.getY() + absMatrixBA.getCol1() * boxA.m_half.getX());
\r
831 projBhalf.setCol0(absMatrixAB.getCol1() * boxB.m_half.getZ() + absMatrixAB.getCol2() * boxB.m_half.getY());
\r
832 projBhalf.setCol1(absMatrixAB.getCol2() * boxB.m_half.getX() + absMatrixAB.getCol0() * boxB.m_half.getZ());
\r
833 projBhalf.setCol2(absMatrixAB.getCol0() * boxB.m_half.getY() + absMatrixAB.getCol1() * boxB.m_half.getX());
\r
835 PfxMatrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);
\r
837 CrossAxisTest(0,0,X);
\r
838 CrossAxisTest(0,1,Y);
\r
839 CrossAxisTest(0,2,Z);
\r
840 CrossAxisTest(1,0,X);
\r
841 CrossAxisTest(1,1,Y);
\r
842 CrossAxisTest(1,2,Z);
\r
843 CrossAxisTest(2,0,X);
\r
844 CrossAxisTest(2,1,Y);
\r
845 CrossAxisTest(2,2,Z);
\r
847 // need to pick the face on each box whose normal best matches the separating axis.
\r
848 // will transform vectors to be in the coordinate system of this face to simplify things later.
\r
849 // for this, a permutation matrix can be used, which the next section computes.
\r
851 int dimA[3], dimB[3];
\r
853 if ( axisType == A_AXIS ) {
\r
854 if ( dot(axisA,offsetAB) < 0.0f )
\r
856 axisB = matrixBA * -axisA;
\r
858 PfxVector3 absAxisB = PfxVector3(absPerElem(axisB));
\r
860 if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) )
\r
862 else if ( absAxisB[1] > absAxisB[2] )
\r
866 } else if ( axisType == B_AXIS ) {
\r
867 if ( dot(axisB,offsetBA) < 0.0f )
\r
869 axisA = matrixAB * -axisB;
\r
871 PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));
\r
873 if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) )
\r
875 else if ( absAxisA[1] > absAxisA[2] )
\r
881 if ( axisType == CROSS_AXIS ) {
\r
882 if ( dot(axisA,offsetAB) < 0.0f )
\r
884 axisB = matrixBA * -axisA;
\r
886 PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));
\r
887 PfxVector3 absAxisB = PfxVector3(absPerElem(axisB));
\r
889 dimA[1] = edgeDimA;
\r
890 dimB[1] = edgeDimB;
\r
892 if ( edgeDimA == 0 ) {
\r
893 if ( absAxisA[1] > absAxisA[2] ) {
\r
900 } else if ( edgeDimA == 1 ) {
\r
901 if ( absAxisA[2] > absAxisA[0] ) {
\r
909 if ( absAxisA[0] > absAxisA[1] ) {
\r
918 if ( edgeDimB == 0 ) {
\r
919 if ( absAxisB[1] > absAxisB[2] ) {
\r
926 } else if ( edgeDimB == 1 ) {
\r
927 if ( absAxisB[2] > absAxisB[0] ) {
\r
935 if ( absAxisB[0] > absAxisB[1] ) {
\r
944 dimA[2] = faceDimA;
\r
945 dimA[0] = (faceDimA+1)%3;
\r
946 dimA[1] = (faceDimA+2)%3;
\r
947 dimB[2] = faceDimB;
\r
948 dimB[0] = (faceDimB+1)%3;
\r
949 dimB[1] = (faceDimB+2)%3;
\r
952 PfxMatrix3 aperm_col, bperm_col;
\r
954 aperm_col.setCol0(ident[dimA[0]]);
\r
955 aperm_col.setCol1(ident[dimA[1]]);
\r
956 aperm_col.setCol2(ident[dimA[2]]);
\r
958 bperm_col.setCol0(ident[dimB[0]]);
\r
959 bperm_col.setCol1(ident[dimB[1]]);
\r
960 bperm_col.setCol2(ident[dimB[2]]);
\r
962 PfxMatrix3 aperm_row, bperm_row;
\r
964 aperm_row = transpose(aperm_col);
\r
965 bperm_row = transpose(bperm_col);
\r
967 // permute all box parameters to be in the face coordinate systems
\r
969 PfxMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;
\r
970 PfxMatrix3 matrixBA_perm = transpose(matrixAB_perm);
\r
972 PfxVector3 offsetAB_perm, offsetBA_perm;
\r
974 offsetAB_perm = aperm_row * offsetAB;
\r
975 offsetBA_perm = bperm_row * offsetBA;
\r
977 PfxVector3 halfA_perm, halfB_perm;
\r
979 halfA_perm = aperm_row * boxA.m_half;
\r
980 halfB_perm = bperm_row * boxB.m_half;
\r
982 // compute the vector between the centers of each face, in each face's coordinate frame
\r
984 PfxVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;
\r
986 signsA_perm = copySignPerElem(PfxVector3(1.0f),aperm_row * axisA);
\r
987 signsB_perm = copySignPerElem(PfxVector3(1.0f),bperm_row * axisB);
\r
988 scalesA_perm = mulPerElem( signsA_perm, halfA_perm );
\r
989 scalesB_perm = mulPerElem( signsB_perm, halfB_perm );
\r
991 faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ();
\r
992 faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() );
\r
994 faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ();
\r
995 faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() );
\r
997 if ( maxGap < 0.0f ) {
\r
998 // if boxes overlap, this will separate the faces for finding points of penetration.
\r
1000 faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;
\r
1001 faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f;
\r
1004 // for each vertex/face or edge/edge pair of the two faces, find the closest points.
\r
1006 // these points each have an associated box feature (vertex, edge, or face). if each
\r
1007 // point is in the external Voronoi region of the other's feature, they are the
\r
1008 // closest points of the boxes, and the algorithm can exit.
\r
1010 // the feature pairs are arranged so that in the general case, the first test will
\r
1011 // succeed. degenerate cases (parallel faces) may require up to all tests in the
\r
1014 // if for some reason no case passes the Voronoi test, the features with the minimum
\r
1015 // distance are returned.
\r
1017 PfxPoint3 localPointA_perm, localPointB_perm;
\r
1018 PfxFloat minDistSqr;
\r
1021 PfxVector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );
\r
1023 localPointA_perm.setZ( scalesA_perm.getZ() );
\r
1024 localPointB_perm.setZ( scalesB_perm.getZ() );
\r
1025 scalesA_perm.setZ(0.0f);
\r
1026 scalesB_perm.setZ(0.0f);
\r
1028 int otherFaceDimA, otherFaceDimB;
\r
1030 if ( axisType == CROSS_AXIS ) {
\r
1031 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1032 otherFaceDimA, otherFaceDimB,
\r
1033 hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1034 matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
\r
1035 scalesA_perm, scalesB_perm, true );
\r
1038 VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1039 hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1040 matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );
\r
1043 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1044 hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1045 matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );
\r
1048 } else if ( axisType == B_AXIS ) {
\r
1049 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1050 hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1051 matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true );
\r
1054 VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1055 hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1056 matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );
\r
1059 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1060 otherFaceDimA, otherFaceDimB,
\r
1061 hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1062 matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
\r
1063 scalesA_perm, scalesB_perm, false );
\r
1067 VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1068 hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1069 matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true );
\r
1072 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1073 hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1074 matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );
\r
1077 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
\r
1078 otherFaceDimA, otherFaceDimB,
\r
1079 hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
\r
1080 matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
\r
1081 scalesA_perm, scalesB_perm, false );
\r
1086 // convert local points from face-local to box-local coordinate system
\r
1088 pointA = PfxPoint3( aperm_col * PfxVector3( localPointA_perm ) );
\r
1089 pointB = PfxPoint3( bperm_col * PfxVector3( localPointB_perm ) );
\r
1091 normal = transformA * axisA;
\r
1093 //SCE_PFX_PRINTF("minDistSqr %f maxGap %f\n",minDistSqr,maxGap);
\r
1094 //SCE_PFX_PRINTF("normal %f %f %f\n",normal[0],normal[1],normal[2]);
\r
1095 //SCE_PFX_PRINTF("pointA %f %f %f\n",pointA[0],pointA[1],pointA[2]);
\r
1096 //SCE_PFX_PRINTF("pointB %f %f %f\n",pointB[0],pointB[1],pointB[2]);
\r
1098 if ( maxGap < 0.0f ) {
\r
1101 return (sqrtf( minDistSqr ));
\r
1105 } //namespace PhysicsEffects
\r