diff options
Diffstat (limited to 'extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp')
| -rw-r--r-- | extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp | 1155 |
1 files changed, 0 insertions, 1155 deletions
diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp deleted file mode 100644 index 30642a39294..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp +++ /dev/null @@ -1,1155 +0,0 @@ -/* - 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 )); - } -} |