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 ); }