aboutsummaryrefslogtreecommitdiff
path: root/tests/bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tests/bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp')
-rw-r--r--tests/bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp1160
1 files changed, 1160 insertions, 0 deletions
diff --git a/tests/bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp b/tests/bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp
new file mode 100644
index 00000000..5e1202c0
--- /dev/null
+++ b/tests/bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp
@@ -0,0 +1,1160 @@
+/*
+ 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 "PfxContactBoxBox.h"
+
+#include <math.h>
+#include "../PlatformDefinitions.h"
+#include "boxBoxDistance.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 vmVector3 & hA,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) scalesB )
+{
+ // compute a corner of box B in A's coordinate system
+
+ vmVector3 corner =
+ vmVector3( 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.
+
+ vmVector3 facePointB =
+ vmVector3( 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,
+ vmPoint3 & localPointA,
+ vmPoint3 & localPointB,
+ FeatureType & featureA,
+ FeatureType & featureB,
+ const vmVector3 & hA,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) 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 vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) scalesA )
+{
+ vmVector3 corner =
+ vmVector3( 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];
+
+ vmVector3 facePointA =
+ vmVector3( 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,
+ vmPoint3 & localPointA,
+ vmPoint3 & localPointB,
+ FeatureType & featureA,
+ FeatureType & featureB,
+ const vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) 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();
+ }
+}
+
+//-------------------------------------------------------------------------------------------------
+// CustomEdgeEdgeTest:
+//
+// 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 CustomEdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \
+{ \
+ vmVector3 edgeOffsetAB; \
+ vmVector3 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]; \
+ } \
+ \
+ vmVector3 edgeOffAB = vmVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\
+ vmVector3 edgeOffBA = vmVector3( 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
+CustomEdgeEdgeTest_0101(
+ bool & inVoronoi,
+ float & tA,
+ float & tB,
+ const vmVector3 & hA,
+ const vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) scalesA,
+ PE_REF(vmVector3) scalesB )
+{
+ CustomEdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );
+}
+
+float
+CustomEdgeEdgeTest_0110(
+ bool & inVoronoi,
+ float & tA,
+ float & tB,
+ const vmVector3 & hA,
+ const vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) scalesA,
+ PE_REF(vmVector3) scalesB )
+{
+ CustomEdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );
+}
+
+float
+CustomEdgeEdgeTest_1001(
+ bool & inVoronoi,
+ float & tA,
+ float & tB,
+ const vmVector3 & hA,
+ const vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) scalesA,
+ PE_REF(vmVector3) scalesB )
+{
+ CustomEdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );
+}
+
+float
+CustomEdgeEdgeTest_1010(
+ bool & inVoronoi,
+ float & tA,
+ float & tB,
+ const vmVector3 & hA,
+ const vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) scalesA,
+ PE_REF(vmVector3) scalesB )
+{
+ CustomEdgeEdgeTest( 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,
+ vmPoint3 & localPointA,
+ vmPoint3 & localPointB,
+ int & otherFaceDimA,
+ int & otherFaceDimB,
+ FeatureType & featureA,
+ FeatureType & featureB,
+ const vmVector3 & hA,
+ const vmVector3 & hB,
+ PE_REF(vmVector3) faceOffsetAB,
+ PE_REF(vmVector3) faceOffsetBA,
+ const vmMatrix3 & matrixAB,
+ const vmMatrix3 & matrixBA,
+ PE_REF(vmVector3) signsA,
+ PE_REF(vmVector3) signsB,
+ PE_REF(vmVector3) scalesA,
+ PE_REF(vmVector3) scalesB,
+ bool first )
+{
+
+ float distSqr;
+ float tA, tB;
+
+ int testOtherFaceDimA, testOtherFaceDimB;
+
+ testOtherFaceDimA = 0;
+ testOtherFaceDimB = 0;
+
+ distSqr = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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 = CustomEdgeEdgeTest_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(vmVector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
+ PE_REF(Box) boxA, const vmTransform3 & transformA, PE_REF(Box) boxB,
+ const vmTransform3 & transformB,
+ float distanceThreshold)
+{
+ vmMatrix3 identity;
+ identity = vmMatrix3::identity();
+ vmVector3 ident[3];
+ ident[0] = identity.getCol0();
+ ident[1] = identity.getCol1();
+ ident[2] = identity.getCol2();
+
+ // get relative transformations
+
+ vmTransform3 transformAB, transformBA;
+ vmMatrix3 matrixAB, matrixBA;
+ vmVector3 offsetAB, offsetBA;
+
+ transformAB = orthoInverse(transformA) * transformB;
+ transformBA = orthoInverse(transformAB);
+
+ matrixAB = transformAB.getUpper3x3();
+ offsetAB = transformAB.getTranslation();
+ matrixBA = transformBA.getUpper3x3();
+ offsetBA = transformBA.getTranslation();
+
+ vmMatrix3 absMatrixAB = absPerElem(matrixAB);
+ vmMatrix3 absMatrixBA = absPerElem(matrixBA);
+
+ // find separating axis with largest gap between projections
+
+ BoxSepAxisType axisType;
+ vmVector3 axisA(0.0f), axisB(0.0f);
+ float gap, maxGap;
+ int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;
+
+ // face axes
+
+ vmVector3 gapsA = absPerElem(offsetAB) - boxA.mHalf - absMatrixAB * boxB.mHalf;
+
+ AaxisTest(0,X,true);
+ AaxisTest(1,Y,false);
+ AaxisTest(2,Z,false);
+
+ vmVector3 gapsB = absPerElem(offsetBA) - boxB.mHalf - absMatrixBA * boxA.mHalf;
+
+ BaxisTest(0,X);
+ BaxisTest(1,Y);
+ BaxisTest(2,Z);
+
+ // cross product axes
+
+ // 外積が０のときの対策
+ absMatrixAB += vmMatrix3(1.0e-5f);
+ absMatrixBA += vmMatrix3(1.0e-5f);
+
+ vmMatrix3 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.mHalf.getZ() + absMatrixBA.getCol2() * boxA.mHalf.getY());
+ projAhalf.setCol1(absMatrixBA.getCol2() * boxA.mHalf.getX() + absMatrixBA.getCol0() * boxA.mHalf.getZ());
+ projAhalf.setCol2(absMatrixBA.getCol0() * boxA.mHalf.getY() + absMatrixBA.getCol1() * boxA.mHalf.getX());
+
+ projBhalf.setCol0(absMatrixAB.getCol1() * boxB.mHalf.getZ() + absMatrixAB.getCol2() * boxB.mHalf.getY());
+ projBhalf.setCol1(absMatrixAB.getCol2() * boxB.mHalf.getX() + absMatrixAB.getCol0() * boxB.mHalf.getZ());
+ projBhalf.setCol2(absMatrixAB.getCol0() * boxB.mHalf.getY() + absMatrixAB.getCol1() * boxB.mHalf.getX());
+
+ vmMatrix3 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;
+
+ vmVector3 absAxisB = vmVector3(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;
+
+ vmVector3 absAxisA = vmVector3(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;
+
+ vmVector3 absAxisA = vmVector3(absPerElem(axisA));
+ vmVector3 absAxisB = vmVector3(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;
+ }
+
+ vmMatrix3 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]]);
+
+ vmMatrix3 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
+
+ vmMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;
+ vmMatrix3 matrixBA_perm = transpose(matrixAB_perm);
+
+ vmVector3 offsetAB_perm, offsetBA_perm;
+
+ offsetAB_perm = aperm_row * offsetAB;
+ offsetBA_perm = bperm_row * offsetBA;
+
+ vmVector3 halfA_perm, halfB_perm;
+
+ halfA_perm = aperm_row * boxA.mHalf;
+ halfB_perm = bperm_row * boxB.mHalf;
+
+ // compute the vector between the centers of each face, in each face's coordinate frame
+
+ vmVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;
+
+ signsA_perm = copySignPerElem(vmVector3(1.0f),aperm_row * axisA);
+ signsB_perm = copySignPerElem(vmVector3(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.
+
+ vmPoint3 localPointA_perm, localPointB_perm;
+ float minDistSqr;
+ bool done;
+
+ vmVector3 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 = vmPoint3( aperm_col * vmVector3( localPointA_perm )) ;
+ boxPointB.localPoint = vmPoint3( bperm_col * vmVector3( localPointB_perm )) ;
+
+#if 0
+ // 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] );
+ }
+#endif
+
+ normal = transformA * axisA;
+
+ if ( maxGap < 0.0f ) {
+ return (maxGap);
+ } else {
+ return (sqrtf( minDistSqr ));
+ }
+}
generated by cgit v1.2.3-18-g5258 (git 2.32.0) at 2024-11-29 05:11:44 +0000