Ejemplo n.º 1
0
			internal btWithoutMarginResult( btDiscreteCollisionDetectorInterface.Result result
				, double marginOnA, double marginOnB )
			{
				m_originalResult = ( result );
				m_marginOnA = ( marginOnA );
				m_marginOnB = ( marginOnB );
				m_foundResult = ( false );
			}
Ejemplo n.º 2
0
		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
			);

		}
Ejemplo n.º 3
0
		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;
		}
Ejemplo n.º 4
0
		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 );

			}


		}
Ejemplo n.º 5
0
		internal override void getClosestPoints( btDiscreteCollisionDetectorInterface.ClosestPointInput input
											, btDiscreteCollisionDetectorInterface.Result output
											, btIDebugDraw debugDraw, bool swapResults = false )
		{
			//(void)swapResults;

			getClosestPointsNonVirtual( input, output, debugDraw );
		}