diff options
Diffstat (limited to 'extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp')
| -rw-r--r-- | extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp | 1155 |
1 files changed, 1155 insertions, 0 deletions
diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp new file mode 100644 index 00000000000..30642a39294 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp @@ -0,0 +1,1155 @@ +/* + Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. + All rights reserved. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + + +#include "Box.h" + +static inline float sqr( float a ) +{ + return (a * a); +} + +enum BoxSepAxisType +{ + A_AXIS, B_AXIS, CROSS_AXIS +}; + +//------------------------------------------------------------------------------------------------- +// voronoiTol: bevels Voronoi planes slightly which helps when features are parallel. +//------------------------------------------------------------------------------------------------- + +static const float voronoiTol = -1.0e-5f; + +//------------------------------------------------------------------------------------------------- +// separating axis tests: gaps along each axis are computed, and the axis with the maximum +// gap is stored. cross product axes are normalized. +//------------------------------------------------------------------------------------------------- + +#define AaxisTest( dim, letter, first ) \ +{ \ + if ( first ) \ + { \ + maxGap = gap = gapsA.get##letter(); \ + if ( gap > distanceThreshold ) return gap; \ + axisType = A_AXIS; \ + faceDimA = dim; \ + axisA = identity.getCol##dim(); \ + } \ + else \ + { \ + gap = gapsA.get##letter(); \ + if ( gap > distanceThreshold ) return gap; \ + else if ( gap > maxGap ) \ + { \ + maxGap = gap; \ + axisType = A_AXIS; \ + faceDimA = dim; \ + axisA = identity.getCol##dim(); \ + } \ + } \ +} + + +#define BaxisTest( dim, letter ) \ +{ \ + gap = gapsB.get##letter(); \ + if ( gap > distanceThreshold ) return gap; \ + else if ( gap > maxGap ) \ + { \ + maxGap = gap; \ + axisType = B_AXIS; \ + faceDimB = dim; \ + axisB = identity.getCol##dim(); \ + } \ +} + +#define CrossAxisTest( dima, dimb, letterb ) \ +{ \ + const float lsqr_tolerance = 1.0e-30f; \ + float lsqr; \ + \ + lsqr = lsqrs.getCol##dima().get##letterb(); \ + \ + if ( lsqr > lsqr_tolerance ) \ + { \ + float l_recip = 1.0f / sqrtf( lsqr ); \ + gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip; \ + \ + if ( gap > distanceThreshold ) \ + { \ + return gap; \ + } \ + \ + if ( gap > maxGap ) \ + { \ + maxGap = gap; \ + axisType = CROSS_AXIS; \ + edgeDimA = dima; \ + edgeDimB = dimb; \ + axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip; \ + } \ + } \ +} + +//------------------------------------------------------------------------------------------------- +// tests whether a vertex of box B and a face of box A are the closest features +//------------------------------------------------------------------------------------------------- + +inline +float +VertexBFaceATest( + bool & inVoronoi, + float & t0, + float & t1, + const Vector3 & hA, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesB ) +{ + // compute a corner of box B in A's coordinate system + + Vector3 corner = + Vector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() ); + + // compute the parameters of the point on A, closest to this corner + + t0 = corner[0]; + t1 = corner[1]; + + if ( t0 > hA[0] ) + t0 = hA[0]; + else if ( t0 < -hA[0] ) + t0 = -hA[0]; + if ( t1 > hA[1] ) + t1 = hA[1]; + else if ( t1 < -hA[1] ) + t1 = -hA[1]; + + // do the Voronoi test: already know the point on B is in the Voronoi region of the + // point on A, check the reverse. + + Vector3 facePointB = + Vector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) ); + + inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) && + ( facePointB[1] >= voronoiTol * facePointB[0] ) && + ( facePointB[2] >= voronoiTol * facePointB[1] ) ); + + return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); +} + +#define VertexBFaceA_SetNewMin() \ +{ \ + minDistSqr = distSqr; \ + localPointA.setX(t0); \ + localPointA.setY(t1); \ + localPointB.setX( scalesB.getX() ); \ + localPointB.setY( scalesB.getY() ); \ + featureA = F; \ + featureB = V; \ +} + +void +VertexBFaceATests( + bool & done, + float & minDistSqr, + Point3 & localPointA, + Point3 & localPointB, + FeatureType & featureA, + FeatureType & featureB, + const Vector3 & hA, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesB, + bool first ) +{ + + float t0, t1; + float distSqr; + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( first ) { + VertexBFaceA_SetNewMin(); + } else { + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } + + if ( done ) + return; + + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } +} + +//------------------------------------------------------------------------------------------------- +// VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features +//------------------------------------------------------------------------------------------------- + +inline +float +VertexAFaceBTest( + bool & inVoronoi, + float & t0, + float & t1, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) scalesA ) +{ + Vector3 corner = + Vector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() ); + + t0 = corner[0]; + t1 = corner[1]; + + if ( t0 > hB[0] ) + t0 = hB[0]; + else if ( t0 < -hB[0] ) + t0 = -hB[0]; + if ( t1 > hB[1] ) + t1 = hB[1]; + else if ( t1 < -hB[1] ) + t1 = -hB[1]; + + Vector3 facePointA = + Vector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) ); + + inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) && + ( facePointA[1] >= voronoiTol * facePointA[0] ) && + ( facePointA[2] >= voronoiTol * facePointA[1] ) ); + + return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); +} + +#define VertexAFaceB_SetNewMin() \ +{ \ + minDistSqr = distSqr; \ + localPointB.setX(t0); \ + localPointB.setY(t1); \ + localPointA.setX( scalesA.getX() ); \ + localPointA.setY( scalesA.getY() ); \ + featureA = V; \ + featureB = F; \ +} + +void +VertexAFaceBTests( + bool & done, + float & minDistSqr, + Point3 & localPointA, + Point3 & localPointB, + FeatureType & featureA, + FeatureType & featureB, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) scalesA, + bool first ) +{ + float t0, t1; + float distSqr; + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( first ) { + VertexAFaceB_SetNewMin(); + } else { + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } +} + +//------------------------------------------------------------------------------------------------- +// EdgeEdgeTest: +// +// tests whether a pair of edges are the closest features +// +// note on the shorthand: +// 'a' & 'b' refer to the edges. +// 'c' is the dimension of the axis that points from the face center to the edge Center +// 'd' is the dimension of the edge Direction +// the dimension of the face normal is 2 +//------------------------------------------------------------------------------------------------- + +#define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \ +{ \ + Vector3 edgeOffsetAB; \ + Vector3 edgeOffsetBA; \ + \ + edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \ + edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \ + \ + edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \ + edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \ + \ + float dirDot = matrixAB.getCol##bd().get##ad_letter(); \ + float denom = 1.0f - dirDot*dirDot; \ + float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \ + float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \ + \ + if ( denom == 0.0f ) \ + { \ + tA = 0.0f; \ + } \ + else \ + { \ + tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \ + } \ + \ + if ( tA < -hA[ad] ) tA = -hA[ad]; \ + else if ( tA > hA[ad] ) tA = hA[ad]; \ + \ + tB = tA * dirDot + edgeOffsetBA_bd; \ + \ + if ( tB < -hB[bd] ) \ + { \ + tB = -hB[bd]; \ + tA = tB * dirDot + edgeOffsetAB_ad; \ + \ + if ( tA < -hA[ad] ) tA = -hA[ad]; \ + else if ( tA > hA[ad] ) tA = hA[ad]; \ + } \ + else if ( tB > hB[bd] ) \ + { \ + tB = hB[bd]; \ + tA = tB * dirDot + edgeOffsetAB_ad; \ + \ + if ( tA < -hA[ad] ) tA = -hA[ad]; \ + else if ( tA > hA[ad] ) tA = hA[ad]; \ + } \ + \ + Vector3 edgeOffAB = Vector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\ + Vector3 edgeOffBA = Vector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\ + \ + inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \ + ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \ + ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \ + ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \ + \ + edgeOffAB[ad] -= tA; \ + edgeOffBA[bd] -= tB; \ + \ + return dot(edgeOffAB,edgeOffAB); \ +} + +float +EdgeEdgeTest_0101( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y ); +} + +float +EdgeEdgeTest_0110( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X ); +} + +float +EdgeEdgeTest_1001( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y ); +} + +float +EdgeEdgeTest_1010( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X ); +} + +#define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \ +{ \ + minDistSqr = distSqr; \ + localPointA.set##ac_letter(scalesA.get##ac_letter()); \ + localPointA.set##ad_letter(tA); \ + localPointB.set##bc_letter(scalesB.get##bc_letter()); \ + localPointB.set##bd_letter(tB); \ + otherFaceDimA = testOtherFaceDimA; \ + otherFaceDimB = testOtherFaceDimB; \ + featureA = E; \ + featureB = E; \ +} + +void +EdgeEdgeTests( + bool & done, + float & minDistSqr, + Point3 & localPointA, + Point3 & localPointB, + int & otherFaceDimA, + int & otherFaceDimB, + FeatureType & featureA, + FeatureType & featureB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB, + bool first ) +{ + + float distSqr; + float tA, tB; + + int testOtherFaceDimA, testOtherFaceDimB; + + testOtherFaceDimA = 0; + testOtherFaceDimB = 0; + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( first ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } else { + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + + if ( done ) + return; + + testOtherFaceDimA = 1; + testOtherFaceDimB = 0; + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + testOtherFaceDimA = 0; + testOtherFaceDimB = 1; + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + testOtherFaceDimA = 1; + testOtherFaceDimB = 1; + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } + + if ( done ) + return; + + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } +} + +float +boxBoxDistance( + Vector3& normal, + BoxPoint& boxPointA, + BoxPoint& boxPointB, + PE_REF(Box) boxA, const Transform3& transformA, + PE_REF(Box) boxB, const Transform3& transformB, + float distanceThreshold ) +{ + Matrix3 identity; + identity = Matrix3::identity(); + Vector3 ident[3]; + ident[0] = identity.getCol0(); + ident[1] = identity.getCol1(); + ident[2] = identity.getCol2(); + + // get relative transformations + + Transform3 transformAB, transformBA; + Matrix3 matrixAB, matrixBA; + Vector3 offsetAB, offsetBA; + + transformAB = orthoInverse(transformA) * transformB; + transformBA = orthoInverse(transformAB); + + matrixAB = transformAB.getUpper3x3(); + offsetAB = transformAB.getTranslation(); + matrixBA = transformBA.getUpper3x3(); + offsetBA = transformBA.getTranslation(); + + Matrix3 absMatrixAB = absPerElem(matrixAB); + Matrix3 absMatrixBA = absPerElem(matrixBA); + + // find separating axis with largest gap between projections + + BoxSepAxisType axisType; + Vector3 axisA(0.0f), axisB(0.0f); + float gap, maxGap; + int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0; + + // face axes + + Vector3 gapsA = absPerElem(offsetAB) - boxA.half - absMatrixAB * boxB.half; + + AaxisTest(0,X,true); + AaxisTest(1,Y,false); + AaxisTest(2,Z,false); + + Vector3 gapsB = absPerElem(offsetBA) - boxB.half - absMatrixBA * boxA.half; + + BaxisTest(0,X); + BaxisTest(1,Y); + BaxisTest(2,Z); + + // cross product axes + + // 外積が０のときの対策 + absMatrixAB += Matrix3(1.0e-5f); + absMatrixBA += Matrix3(1.0e-5f); + + Matrix3 lsqrs, projOffset, projAhalf, projBhalf; + + lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + + mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) ); + lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + + mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); + lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) + + mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); + + projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY()); + projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ()); + projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX()); + + projAhalf.setCol0(absMatrixBA.getCol1() * boxA.half.getZ() + absMatrixBA.getCol2() * boxA.half.getY()); + projAhalf.setCol1(absMatrixBA.getCol2() * boxA.half.getX() + absMatrixBA.getCol0() * boxA.half.getZ()); + projAhalf.setCol2(absMatrixBA.getCol0() * boxA.half.getY() + absMatrixBA.getCol1() * boxA.half.getX()); + + projBhalf.setCol0(absMatrixAB.getCol1() * boxB.half.getZ() + absMatrixAB.getCol2() * boxB.half.getY()); + projBhalf.setCol1(absMatrixAB.getCol2() * boxB.half.getX() + absMatrixAB.getCol0() * boxB.half.getZ()); + projBhalf.setCol2(absMatrixAB.getCol0() * boxB.half.getY() + absMatrixAB.getCol1() * boxB.half.getX()); + + Matrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf); + + CrossAxisTest(0,0,X); + CrossAxisTest(0,1,Y); + CrossAxisTest(0,2,Z); + CrossAxisTest(1,0,X); + CrossAxisTest(1,1,Y); + CrossAxisTest(1,2,Z); + CrossAxisTest(2,0,X); + CrossAxisTest(2,1,Y); + CrossAxisTest(2,2,Z); + + // need to pick the face on each box whose normal best matches the separating axis. + // will transform vectors to be in the coordinate system of this face to simplify things later. + // for this, a permutation matrix can be used, which the next section computes. + + int dimA[3], dimB[3]; + + if ( axisType == A_AXIS ) { + if ( dot(axisA,offsetAB) < 0.0f ) + axisA = -axisA; + axisB = matrixBA * -axisA; + + Vector3 absAxisB = Vector3(absPerElem(axisB)); + + if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) ) + faceDimB = 0; + else if ( absAxisB[1] > absAxisB[2] ) + faceDimB = 1; + else + faceDimB = 2; + } else if ( axisType == B_AXIS ) { + if ( dot(axisB,offsetBA) < 0.0f ) + axisB = -axisB; + axisA = matrixAB * -axisB; + + Vector3 absAxisA = Vector3(absPerElem(axisA)); + + if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) ) + faceDimA = 0; + else if ( absAxisA[1] > absAxisA[2] ) + faceDimA = 1; + else + faceDimA = 2; + } + + if ( axisType == CROSS_AXIS ) { + if ( dot(axisA,offsetAB) < 0.0f ) + axisA = -axisA; + axisB = matrixBA * -axisA; + + Vector3 absAxisA = Vector3(absPerElem(axisA)); + Vector3 absAxisB = Vector3(absPerElem(axisB)); + + dimA[1] = edgeDimA; + dimB[1] = edgeDimB; + + if ( edgeDimA == 0 ) { + if ( absAxisA[1] > absAxisA[2] ) { + dimA[0] = 2; + dimA[2] = 1; + } else { + dimA[0] = 1; + dimA[2] = 2; + } + } else if ( edgeDimA == 1 ) { + if ( absAxisA[2] > absAxisA[0] ) { + dimA[0] = 0; + dimA[2] = 2; + } else { + dimA[0] = 2; + dimA[2] = 0; + } + } else { + if ( absAxisA[0] > absAxisA[1] ) { + dimA[0] = 1; + dimA[2] = 0; + } else { + dimA[0] = 0; + dimA[2] = 1; + } + } + + if ( edgeDimB == 0 ) { + if ( absAxisB[1] > absAxisB[2] ) { + dimB[0] = 2; + dimB[2] = 1; + } else { + dimB[0] = 1; + dimB[2] = 2; + } + } else if ( edgeDimB == 1 ) { + if ( absAxisB[2] > absAxisB[0] ) { + dimB[0] = 0; + dimB[2] = 2; + } else { + dimB[0] = 2; + dimB[2] = 0; + } + } else { + if ( absAxisB[0] > absAxisB[1] ) { + dimB[0] = 1; + dimB[2] = 0; + } else { + dimB[0] = 0; + dimB[2] = 1; + } + } + } else { + dimA[2] = faceDimA; + dimA[0] = (faceDimA+1)%3; + dimA[1] = (faceDimA+2)%3; + dimB[2] = faceDimB; + dimB[0] = (faceDimB+1)%3; + dimB[1] = (faceDimB+2)%3; + } + + Matrix3 aperm_col, bperm_col; + + aperm_col.setCol0(ident[dimA[0]]); + aperm_col.setCol1(ident[dimA[1]]); + aperm_col.setCol2(ident[dimA[2]]); + + bperm_col.setCol0(ident[dimB[0]]); + bperm_col.setCol1(ident[dimB[1]]); + bperm_col.setCol2(ident[dimB[2]]); + + Matrix3 aperm_row, bperm_row; + + aperm_row = transpose(aperm_col); + bperm_row = transpose(bperm_col); + + // permute all box parameters to be in the face coordinate systems + + Matrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col; + Matrix3 matrixBA_perm = transpose(matrixAB_perm); + + Vector3 offsetAB_perm, offsetBA_perm; + + offsetAB_perm = aperm_row * offsetAB; + offsetBA_perm = bperm_row * offsetBA; + + Vector3 halfA_perm, halfB_perm; + + halfA_perm = aperm_row * boxA.half; + halfB_perm = bperm_row * boxB.half; + + // compute the vector between the centers of each face, in each face's coordinate frame + + Vector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm; + + signsA_perm = copySignPerElem(Vector3(1.0f),aperm_row * axisA); + signsB_perm = copySignPerElem(Vector3(1.0f),bperm_row * axisB); + scalesA_perm = mulPerElem( signsA_perm, halfA_perm ); + scalesB_perm = mulPerElem( signsB_perm, halfB_perm ); + + faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ(); + faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() ); + + faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ(); + faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() ); + + if ( maxGap < 0.0f ) { + // if boxes overlap, this will separate the faces for finding points of penetration. + + faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f; + faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f; + } + + // for each vertex/face or edge/edge pair of the two faces, find the closest points. + // + // these points each have an associated box feature (vertex, edge, or face). if each + // point is in the external Voronoi region of the other's feature, they are the + // closest points of the boxes, and the algorithm can exit. + // + // the feature pairs are arranged so that in the general case, the first test will + // succeed. degenerate cases (parallel faces) may require up to all tests in the + // worst case. + // + // if for some reason no case passes the Voronoi test, the features with the minimum + // distance are returned. + + Point3 localPointA_perm, localPointB_perm; + float minDistSqr; + bool done; + + Vector3 hA_perm( halfA_perm ), hB_perm( halfB_perm ); + + localPointA_perm.setZ( scalesA_perm.getZ() ); + localPointB_perm.setZ( scalesB_perm.getZ() ); + scalesA_perm.setZ(0.0f); + scalesB_perm.setZ(0.0f); + + int otherFaceDimA, otherFaceDimB; + FeatureType featureA, featureB; + + if ( axisType == CROSS_AXIS ) { + EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, + otherFaceDimA, otherFaceDimB, featureA, featureB, + hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, + scalesA_perm, scalesB_perm, true ); + + if ( !done ) { + VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); + + if ( !done ) { + VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); + } + } + } else if ( axisType == B_AXIS ) { + VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true ); + + if ( !done ) { + VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); + + if ( !done ) { + EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, + otherFaceDimA, otherFaceDimB, featureA, featureB, + hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, + scalesA_perm, scalesB_perm, false ); + } + } + } else { + VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true ); + + if ( !done ) { + VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); + + if ( !done ) { + EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, + otherFaceDimA, otherFaceDimB, featureA, featureB, + hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, + scalesA_perm, scalesB_perm, false ); + } + } + } + + // convert local points from face-local to box-local coordinate system + + boxPointA.localPoint = Point3( aperm_col * Vector3( localPointA_perm ) ); + boxPointB.localPoint = Point3( bperm_col * Vector3( localPointB_perm ) ); + + // find which features of the boxes are involved. + // the only feature pairs which occur in this function are VF, FV, and EE, even though the + // closest points might actually lie on sub-features, as in a VF contact might be used for + // what's actually a VV contact. this means some feature pairs could possibly seem distinct + // from others, although their contact positions are the same. don't know yet whether this + // matters. + + int sA[3], sB[3]; + + sA[0] = boxPointA.localPoint.getX() > 0.0f; + sA[1] = boxPointA.localPoint.getY() > 0.0f; + sA[2] = boxPointA.localPoint.getZ() > 0.0f; + + sB[0] = boxPointB.localPoint.getX() > 0.0f; + sB[1] = boxPointB.localPoint.getY() > 0.0f; + sB[2] = boxPointB.localPoint.getZ() > 0.0f; + + if ( featureA == F ) { + boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] ); + } else if ( featureA == E ) { + boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] ); + } else { + boxPointA.setVertexFeature( sA[0], sA[1], sA[2] ); + } + + if ( featureB == F ) { + boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] ); + } else if ( featureB == E ) { + boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] ); + } else { + boxPointB.setVertexFeature( sB[0], sB[1], sB[2] ); + } + + normal = transformA * axisA; + + if ( maxGap < 0.0f ) { + return (maxGap); + } else { + return (sqrtf( minDistSqr )); + } +} |