internal btWithoutMarginResult( btDiscreteCollisionDetectorInterface.Result result
, double marginOnA, double marginOnB )
{
m_originalResult = ( result );
m_marginOnA = ( marginOnA );
m_marginOnB = ( marginOnB );
m_foundResult = ( false );
}
internal static void getClosestPoints( btBoxShape m_box1, btBoxShape m_box2
, btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output
, btIDebugDraw debugDraw, bool swapResults = false )
{
//btTransform transformA; input.m_transformA.Get( out transformA );
//btTransform transformB; input.m_transformB.Get( out transformB );
int skip = 0;
//dContactGeom* contact = 0;
double[] R1 = new double[12];
double[] R2 = new double[12];
//dMatrix3 R1;
//dMatrix3 R2;
for( int j = 0; j < 3; j++ )
{
R1[0 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformA.m_basis, j,0 );
R2[0 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformB.m_basis, j, 0 );
R1[1 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformA.m_basis,j,1);
R2[1 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformB.m_basis,j,1 );
R1[2 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformA.m_basis,j,2 );
R2[2 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformB.m_basis,j,2 );
}
//btVector3 normal;
//double depth;
//int return_code;
int maxc = 4;
btVector3 half1, half2;
m_box1.getHalfExtentsWithMargin( out half1 );
half1.Mult( btScalar.BT_TWO, out half1 );
m_box2.getHalfExtentsWithMargin( out half2 );
half2.Mult( btScalar.BT_TWO, out half2 );
dBoxBox2( ref input.m_transformA.m_origin,
R1,
ref half1,
ref input.m_transformB.m_origin,
R2,
ref half2,
//out normal, out depth, out return_code,
maxc, skip,
output
);
}
static int dBoxBox2( ref btVector3 p1, double[] R1,
ref btVector3 side1, ref btVector3 p2,
double[] R2, ref btVector3 side2,
// out btVector3 normal, out double depth, out int return_code,
int maxc, int skip, btDiscreteCollisionDetectorInterface.Result output )
{
double[] p = new double[3], pp = new double[3];
btVector3 normalC = btVector3.Zero;
double[] normalR = null;
int normalRstart = 0;
double[] A = new double[3], B = new double[3];
double R11, R12, R13, R21, R22, R23, R31, R32, R33,
Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s;
int i, j, code;
bool invert_normal;
//normal = btVector3.Zero;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2.x - p1.x;
p[1] = p2.y - p1.y;
p[2] = p2.z - p1.z;
dMULTIPLY1_331( pp, R1, p ); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0] * (double)( 0.5 );
A[1] = side1[1] * (double)( 0.5 );
A[2] = side1[2] * (double)( 0.5 );
B[0] = side2[0] * (double)( 0.5 );
B[1] = side2[1] * (double)( 0.5 );
B[2] = side2[2] * (double)( 0.5 );
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44( R1, +0, R2, +0 ); R12 = dDOT44( R1, +0, R2, +1 ); R13 = dDOT44( R1, +0, R2, +2 );
R21 = dDOT44( R1, +1, R2, +0 ); R22 = dDOT44( R1, +1, R2, +1 ); R23 = dDOT44( R1, +1, R2, +2 );
R31 = dDOT44( R1, +2, R2, +0 ); R32 = dDOT44( R1, +2, R2, +1 ); R33 = dDOT44( R1, +2, R2, +2 );
Q11 = btScalar.btFabs( R11 ); Q12 = btScalar.btFabs( R12 ); Q13 = btScalar.btFabs( R13 );
Q21 = btScalar.btFabs( R21 ); Q22 = btScalar.btFabs( R22 ); Q23 = btScalar.btFabs( R23 );
Q31 = btScalar.btFabs( R31 ); Q32 = btScalar.btFabs( R32 ); Q33 = btScalar.btFabs( R33 );
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
s = btScalar.BT_MIN_FLOAT;
invert_normal = false;
code = 0;
//depth = 0;
//return_code = 0;
// separating axis = u1,u2,u3
if( !TST1( ref s, ref normalR, ref normalRstart, ref invert_normal, ref code, pp[0]
, ( A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13 ), R1, 0, 1 ) )
return 0;
if( !TST1( ref s, ref normalR, ref normalRstart, ref invert_normal, ref code, pp[1]
, ( A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23 ), R1, 1, 2 ) )
return 0;
if( !TST1( ref s, ref normalR, ref normalRstart, ref invert_normal, ref code, pp[2]
, ( A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33 ), R1, 2, 3 ) )
return 0;
// separating axis = v1,v2,v3
if( !TST1( ref s, ref normalR, ref normalRstart, ref invert_normal, ref code
, dDOT41( R2, 0, p ), ( A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0] ), R2, +0, 4 ) )
return 0;
if( !TST1( ref s, ref normalR, ref normalRstart, ref invert_normal, ref code
, dDOT41( R2, 1, p ), ( A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1] ), R2, +1, 5 ) )
return 0;
if( !TST1( ref s, ref normalR, ref normalRstart, ref invert_normal, ref code
, dDOT41( R2, 2, p ), ( A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2] ), R2, +2, 6 ) )
return 0;
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
Q11 += fudge2;
Q12 += fudge2;
Q13 += fudge2;
Q21 += fudge2;
Q22 += fudge2;
Q23 += fudge2;
Q31 += fudge2;
Q32 += fudge2;
Q33 += fudge2;
// separating axis = u1 x (v1,v2,v3)
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[2] * R21 - pp[1] * R31, ( A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12 ), 0, -R31, R21, 7 ) )
return 0;
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[2] * R22 - pp[1] * R32, ( A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11 ), 0, -R32, R22, 8 ) )
return 0;
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[2] * R23 - pp[1] * R33, ( A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11 ), 0, -R33, R23, 9 ) )
return 0;
// separating axis = u2 x (v1,v2,v3)
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[0] * R31 - pp[2] * R11, ( A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22 ), R31, 0, -R11, 10 ) )
return 0;
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[0] * R32 - pp[2] * R12, ( A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21 ), R32, 0, -R12, 11 ) )
return 0;
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[0] * R33 - pp[2] * R13, ( A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21 ), R33, 0, -R13, 12 ) )
return 0;
// separating axis = u3 x (v1,v2,v3)
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[1] * R11 - pp[0] * R21, ( A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32 ), -R21, R11, 0, 13 ) )
return 0;
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[1] * R12 - pp[0] * R22, ( A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31 ), -R22, R12, 0, 14 ) )
return 0;
if( !TST2( ref s, ref normalR, ref normalC, ref invert_normal, ref code,
pp[1] * R13 - pp[0] * R23, ( A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31 ), -R23, R13, 0, 15 ) )
return 0;
if( code == 0 ) return 0;
btVector3 normal;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if( normalR != null )
{
normal.x = normalR[normalRstart + 0];
normal.y = normalR[normalRstart + 4];
normal.z = normalR[normalRstart + 8];
normal.w = 0;
}
else
{
dMULTIPLY0_331( out normal, R1, ref normalC );
}
if( invert_normal )
{
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
double depth = -s;
// compute contact point(s)
if( code > 6 )
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
btVector3 pa2;
double sign;
pa2 = p1;
//for( i = 0; i < 3; i++ ) pa[i] = p1[i];
for( j = 0; j < 3; j++ )
{
sign = ( dDOT14( ref normal, R1, j ) > 0 ) ? (double)( 1.0 ) : (double)( -1.0 );
for( i = 0; i < 3; i++ ) pa2[i] += sign * A[j] * R1[i * 4 + j];
}
// find a point pb on the intersecting edge of box 2
btVector3 pb2 = p2;
//for( i = 0; i < 3; i++ ) pb[i] = p2[i];
for( j = 0; j < 3; j++ )
{
sign = ( dDOT14( ref normal, R2, j ) > 0 ) ? (double)( -1.0 ) : (double)( 1.0 );
for( i = 0; i < 3; i++ ) pb2[i] += sign * B[j] * R2[i * 4 + j];
}
double alpha, beta;
btVector3 ua, ub;
btVector3.setValue( out ua, R1[( ( code ) - 7 ) / 3 + 0 * 4]
, R1[( ( code ) - 7 ) / 3 + 1 * 4]
, R1[( ( code ) - 7 ) / 3 + 2 * 4] );
//for( i = 0; i < 3; i++ ) ua[i] = R1[( ( code ) - 7 ) / 3 + i * 4];
btVector3.setValue( out ub, R2[( ( code ) - 7 ) % 3 + 0 * 4]
, R2[( ( code ) - 7 ) % 3 + 1 * 4]
, R2[( ( code ) - 7 ) % 3 + 2 * 4] );
//for( i = 0; i < 3; i++ ) ub[i] = R2[( ( code ) - 7 ) % 3 + i * 4];
dLineClosestApproach( ref pa2, ref ua, ref pb2, ref ub, out alpha, out beta );
for( i = 0; i < 3; i++ ) pa2[i] += ua[i] * alpha;
for( i = 0; i < 3; i++ ) pb2[i] += ub[i] * beta;
{
//contact[0].pos[i] = (double)(0.5)*(pa2[i]+pb2[i]);
//contact[0].depth = *depth;
//btVector3 pointInWorld;
#if USE_CENTER_POINT
for (i=0; i<3; i++)
pointInWorld[i] = (pa[i]+pb[i])*(double)(0.5);
output.addContactPoint(-normal,pointInWorld,-*depth);
#else
btVector3 tmp;
normal.Invert( out tmp );
output.addContactPoint( ref tmp, ref pb2, -depth );
#endif //
//return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
double[] Ra, Rb, pa = new double[3], pb = new double[3], Sa, Sb;
if( code <= 3 )
{
Ra = R1;
Rb = R2;
pa[0] = p1.x; pa[1] = p1.y; pa[2] = p1.z;
pb[0] = p2.x; pb[1] = p2.y; pb[2] = p2.z;
//pa = p1;
//pb = p2;
Sa = A;
Sb = B;
}
else
{
Ra = R2;
Rb = R1;
pa[0] = p2.x; pa[1] = p2.y; pa[2] = p2.z;
pb[0] = p1.x; pb[1] = p1.y; pb[2] = p1.z;
//pa = p2;
//pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
btVector3 normal2, nr, anr;
if( code <= 3 )
{
normal2 = normal;
}
else
{
normal.Invert( out normal2 );
}
dMULTIPLY1_331( out nr, Rb, ref normal2 );
btVector3.setValue( out anr
, btScalar.btFabs( nr[0] )
, btScalar.btFabs( nr[1] )
, btScalar.btFabs( nr[2] ) );
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr, a1, a2;
if( anr[1] > anr[0] )
{
if( anr[1] > anr[2] )
{
a1 = 0;
lanr = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else
{
if( anr[0] > anr[2] )
{
lanr = 0;
a1 = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
btVector3 center = btVector3.Zero;
if( nr[lanr] < 0 )
{
for( i = 0; i < 3; i++ ) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
}
else
{
for( i = 0; i < 3; i++ ) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
if( code <= 3 ) codeN = code - 1; else codeN = code - 4;
if( codeN == 0 )
{
code1 = 1;
code2 = 2;
}
else if( codeN == 1 )
{
code1 = 0;
code2 = 2;
}
else
{
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
double[] quad = new double[8]; // 2D coordinate of incident face (x,y pairs)
double c1, c2, m11, m12, m21, m22;
c1 = dDOT14( ref center, Ra, code1 );
c2 = dDOT14( ref center, Ra, code2 );
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT44( Ra, code1, Rb, a1 );
m12 = dDOT44( Ra, code1, Rb, a2 );
m21 = dDOT44( Ra, code2, Rb, a1 );
m22 = dDOT44( Ra, code2, Rb, a2 );
{
double k1 = m11 * Sb[a1];
double k2 = m21 * Sb[a1];
double k3 = m12 * Sb[a2];
double k4 = m22 * Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
double[] rect = new double[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
double[] ret = new double[16];
int n = intersectRectQuad2( rect, quad, ret );
if( n < 1 ) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
double[] point = new double[3 * 8]; // penetrating contact points
double[] dep = new double[8]; // depths for those points
double det1 = 1 / ( m11 * m22 - m12 * m21 );
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for( j = 0; j < n; j++ )
{
double k1 = m22 * ( ret[j * 2] - c1 ) - m12 * ( ret[j * 2 + 1] - c2 );
double k2 = -m21 * ( ret[j * 2] - c1 ) + m11 * ( ret[j * 2 + 1] - c2 );
for( i = 0; i < 3; i++ )
point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
dep[cnum] = Sa[codeN] - dDOT( ref normal2, point, cnum * 3 );
if( dep[cnum] >= 0 )
{
ret[cnum * 2] = ret[j * 2];
ret[cnum * 2 + 1] = ret[j * 2 + 1];
cnum++;
}
}
if( cnum < 1 ) return 0; // this should never happen
// we can't generate more contacts than we actually have
if( maxc > cnum ) maxc = cnum;
if( maxc < 1 ) maxc = 1;
btVector3 pointInWorld = btVector3.Zero;
if( cnum <= maxc )
{
if( code < 4 )
{
// we have less contacts than we need, so we use them all
for( j = 0; j < cnum; j++ )
{
for( i = 0; i < 3; i++ )
pointInWorld[i] = point[j * 3 + i] + pa[i];
btVector3 tmp;
normal.Invert( out tmp );
output.addContactPoint( ref tmp, ref pointInWorld, -dep[j] );
}
}
else
{
// we have less contacts than we need, so we use them all
for( j = 0; j < cnum; j++ )
{
for( i = 0; i < 3; i++ )
pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
//pointInWorld[i] = point[j*3+i] + pa[i];
btVector3 tmp;
normal.Invert( out tmp );
output.addContactPoint( ref tmp, ref pointInWorld, -dep[j] );
}
}
}
else
{
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
double maxdepth = dep[0];
for( i = 1; i < cnum; i++ )
{
if( dep[i] > maxdepth )
{
maxdepth = dep[i];
i1 = i;
}
}
int[] iret = new int[8];
cullPoints2( cnum, ret, maxc, i1, iret );
for( j = 0; j < maxc; j++ )
{
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con.pos[i] = point[iret[j]*3+i] + pa[i];
// con.depth = dep[iret[j]];
for( i = 0; i < 3; i++ )
pointInWorld[i] = point[iret[j] * 3 + i] + pa[i];
if( code < 4 )
{
btVector3 tmp;
normal.Invert( out tmp );
output.addContactPoint( ref tmp, ref pointInWorld, -dep[iret[j]] );
}
else
{
btVector3 tmp, tmp2;
normal.Invert( out tmp );
pointInWorld.SubScale( ref normal, dep[iret[j]], out tmp2 );
//pointInWorld - normal * dep[iret[j]]
output.addContactPoint( ref tmp, ref tmp2, -dep[iret[j]] );
}
}
cnum = maxc;
}
//return_code = code;
return cnum;
}
internal void getClosestPointsNonVirtual( btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output, btIDebugDraw debugDraw )
#endif
{
m_cachedSeparatingDistance = 0;
double distance = btScalar.BT_ZERO;
btVector3 normalInB = btVector3.Zero;
btVector3 pointOnA, pointOnB = btVector3.Zero;
btTransform localTransA; input.m_transformA.Get( out localTransA );
btTransform localTransB; input.m_transformB.Get( out localTransB );
btVector3 positionOffset; localTransA.m_origin.Add( ref localTransB.m_origin, out positionOffset );
positionOffset.Mult( (double)( 0.5 ), out positionOffset );
localTransA.m_origin.Sub( ref positionOffset, out localTransA.m_origin );
localTransB.m_origin.Sub( ref positionOffset, out localTransB.m_origin );
bool check2d = m_minkowskiA.isConvex2d() && m_minkowskiB.isConvex2d();
double marginA = m_marginA;
double marginB = m_marginB;
gNumGjkChecks++;
//for CCD we don't use margins
if( m_ignoreMargin )
{
marginA = btScalar.BT_ZERO;
marginB = btScalar.BT_ZERO;
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue( 0, 1, 0 );
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
{
double squaredDistance = btScalar.BT_LARGE_FLOAT;
double delta = btScalar.BT_ZERO;
double margin = marginA + marginB;
m_simplexSolver.reset();
for( ;;)
//while (true)
{
btVector3 tmp;
m_cachedSeparatingAxis.Invert( out tmp );
btVector3 seperatingAxisInA; localTransA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
btVector3 seperatingAxisInB; localTransA.m_basis.ApplyInverse( ref m_cachedSeparatingAxis, out seperatingAxisInB );
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btScalar.Dbg( "pWorld is " + pWorld + " qWorld is " + qWorld );
if( check2d )
{
pWorld[2] = 0;
qWorld[2] = 0;
}
btVector3 w; pWorld.Sub( ref qWorld, out w );
delta = m_cachedSeparatingAxis.dot( ref w );
// potential exit, they don't overlap
if( ( delta > (double)( 0.0 ) ) && ( delta * delta > squaredDistance * input.m_maximumDistanceSquared ) )
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if( m_simplexSolver.inSimplex( ref w ) )
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
double f0 = squaredDistance - delta;
double f1 = squaredDistance * REL_ERROR2;
btScalar.Dbg( "f0 is " + f0.ToString( "g17" ) + " f1 is " + f1.ToString( "g17" ) );
if( f0 <= f1 )
{
if( f0 <= btScalar.BT_ZERO )
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.addVertex( ref w, ref pWorld, ref qWorld );
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if( !m_simplexSolver.closest( out newCachedSeparatingAxis ) )
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if( newCachedSeparatingAxis.length2() < REL_ERROR2 )
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
double previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
#if asdfasdf
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver.compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if( previousSquaredDistance - squaredDistance <= btScalar.SIMD_EPSILON * previousSquaredDistance )
{
// m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if( m_curIter++ > gGjkMaxIter )
{
#if DEBUG
Console.WriteLine( "btGjkPairDetector maxIter exceeded:{0}", m_curIter );
Console.WriteLine( "sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4},shapeTypeB={5}\n",
m_cachedSeparatingAxis.x,
m_cachedSeparatingAxis.y,
m_cachedSeparatingAxis.z,
squaredDistance,
m_minkowskiA.getShapeType(),
m_minkowskiB.getShapeType() );
#endif
break;
}
bool check = ( !m_simplexSolver.fullSimplex() );
//bool check = (!m_simplexSolver.fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver.maxVertex());
if( !check )
{
//do we need this backup_closest here ?
// m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if( checkSimplex )
{
m_simplexSolver.compute_points( out pointOnA, out pointOnB );
btScalar.Dbg( "new simplex points " + pointOnA.ToString() + " and " + pointOnB );
normalInB = m_cachedSeparatingAxis;
double lenSqr = m_cachedSeparatingAxis.length2();
//valid normal
if( lenSqr < 0.0001 )
{
m_degenerateSimplex = 5;
}
if( lenSqr > btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
{
double rlen = btScalar.BT_ONE / btScalar.btSqrt( lenSqr );
normalInB.Mult( rlen, out normalInB );
//normalInB *= rlen; //normalize
double s = btScalar.btSqrt( squaredDistance );
Debug.Assert( s > (double)( 0.0 ) );
pointOnA.SubScale( ref m_cachedSeparatingAxis, ( marginA / s ), out pointOnA );
pointOnB.AddScale( ref m_cachedSeparatingAxis, ( marginB / s ), out pointOnB );
//pointOnA -= m_cachedSeparatingAxis * ( marginA / s );
//pointOnB += m_cachedSeparatingAxis * ( marginB / s );
distance = ( ( btScalar.BT_ONE / rlen ) - margin );
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
( m_catchDegeneracies != 0
&& m_penetrationDepthSolver != null
&& m_degenerateSimplex != 0
&& ( ( distance + margin ) < 0.01 ) );
//if (checkPenetration && !isValid)
if( checkPenetration && ( !isValid || catchDegeneratePenetrationCase ) )
{
//penetration case
//if there is no way to handle penetrations, bail out
if( m_penetrationDepthSolver != null )
{
// Penetration depth case.
btVector3 tmpPointOnA, tmpPointOnB;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw
);
btScalar.Dbg( "points are " + tmpPointOnA.ToString() + " and " + tmpPointOnB.ToString() );
if( isValid2 )
{
btVector3 tmpNormalInB; tmpPointOnB.Sub( ref tmpPointOnA, out tmpNormalInB );
double lenSqr = tmpNormalInB.length2();
if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.length2();
}
if( lenSqr > ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
tmpNormalInB.Mult( btScalar.btSqrt( lenSqr ), out tmpNormalInB );
btVector3 tmp;
tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
double distance2 = -tmp.length();
m_lastUsedMethod = 3;
//only replace valid penetrations when the result is deeper (check)
if( !isValid || ( distance2 < distance ) )
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
///todo: need to track down this EPA penetration solver degeneracy
///the penetration solver reports penetration but the contact normal
///connecting the contact points is pointing in the opposite direction
///until then, detect the issue and revert the normal
{
btScalar d1 = 0;
{
normalInB.Invert( out tmp );
btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInA );
btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref tmp, out seperatingAxisInB );
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btVector3 w; pWorld.Sub( ref qWorld, out w );
d1 = ( tmp ).dot( w );
}
btScalar d0 = btScalar.BT_ZERO;
{
normalInB.Invert( out tmp );
btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInB ) ;
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btVector3 w; pWorld.Sub( ref qWorld, out w );
d0 = normalInB.dot( w );
}
if( d1 > d0 )
{
m_lastUsedMethod = 10;
normalInB *= -1;
}
}
isValid = true;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if( m_cachedSeparatingAxis.length2() > btScalar.BT_ZERO )
{
btVector3 tmp;
tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
double distance2 = tmp.length() - margin;
//only replace valid distances when the distance is less
btScalar.Dbg( "old distance " + distance2.ToString( "g17" ) + " new distance " + distance.ToString( "g17" ) );
if( !isValid || ( distance2 < distance ) )
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA.SubScale( ref m_cachedSeparatingAxis, marginA, out pointOnA );
pointOnB.AddScale( ref m_cachedSeparatingAxis, marginB, out pointOnA );
//pointOnA -= m_cachedSeparatingAxis * marginA;
//pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
btScalar.Dbg( "Pair detector : valid=" + (isValid?"1":"0" )+ " distance=" + distance.ToString( "g17" ) + " maxDistance=" + input.m_maximumDistanceSquared.ToString( "g17" ) );
if( isValid && ( ( distance < 0 ) || ( distance * distance < input.m_maximumDistanceSquared ) ) )
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
btVector3 tmp;
pointOnB.Add( ref positionOffset, out tmp );
output.addContactPoint(
ref normalInB,
ref tmp,
distance );
}
}
internal override void getClosestPoints( btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output
, btIDebugDraw debugDraw, bool swapResults = false )
{
//(void)swapResults;
getClosestPointsNonVirtual( input, output, debugDraw );
}
