e6202f58ad
BulletMultiThreaded/NarrowPhaseCollision makes use of this boxBoxDistance. Cache some values in src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp, to avoid DMA transfers 2) Added btConvexSeparatingDistanceUtil: this allows caching of separating distance/vector as early-out to avoid convex-convex collision detection. btConvexSeparatingDistanceUtil is used in src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp and can be controlled by btDispatcherInfo.m_useConvexConservativeDistanceUtil/m_convexConservativeDistanceThreshold 3) Use BulletMultiThreaded/vectormath/scalar/cpp/vectormath/scalar/cpp/vectormath_aos.h as fallback for non-PlayStation 3 Cell SPU/PPU platforms (used by boxBoxDistance). Note there are other implementations in Extras/vectormath folder, that are potentially faster for IBM Cell SDK 3.0 SPU (libspe2)
1154 lines
41 KiB
C++
1154 lines
41 KiB
C++
/*
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsB,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsB,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
Vector3 signsB,
|
||
Vector3 scalesA,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
Vector3 signsB,
|
||
Vector3 scalesA,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
Vector3 signsB,
|
||
Vector3 scalesA,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
Vector3 signsB,
|
||
Vector3 scalesA,
|
||
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,
|
||
Vector3 faceOffsetAB,
|
||
Vector3 faceOffsetBA,
|
||
const Matrix3 & matrixAB,
|
||
const Matrix3 & matrixBA,
|
||
Vector3 signsA,
|
||
Vector3 signsB,
|
||
Vector3 scalesA,
|
||
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,
|
||
Box boxA, const Transform3& transformA,
|
||
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
|
||
|
||
// <20>O<EFBFBD>ς<EFBFBD><CF82>O<EFBFBD>̂Ƃ<CC82><C682>̑<CC91>
|
||
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 ));
|
||
}
|
||
}
|