ruby-bullet 0.0.2-x86-mingw32

data/deps/include/bullet/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
+
+	// �O�ς��O�̂Ƃ��̑΍�
+	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 ));
+	}
+}