C# btVector3.Sub方法代码示例

		internal static void calculateVelocityQuaternion( ref btVector3 pos0, ref btVector3 pos1, ref btQuaternion orn0, ref btQuaternion orn1, double timeStep, out btVector3 linVel, out btVector3 angVel )
		{
			btVector3 tmp;
			pos1.Sub( ref pos0, out tmp );
			tmp.Div( timeStep, out linVel );
			btVector3 axis;
			double angle;
			if( !orn0.Equals(ref orn1 ) )
			{
				calculateDiffAxisAngleQuaternion( ref orn0, ref orn1, out axis, out angle );
				axis.Mult( angle, out tmp );
				tmp.Div( timeStep, out angVel );
			}
			else
			{
				angVel = btVector3.Zero;
			}
		}

		public static void TriNormal( ref btVector3 v0, ref btVector3 v1, ref btVector3 v2, out btVector3 result )
		{
			// return the normal of the triangle
			// inscribed by v0, v1, and v2
			btVector3 tmp1;
			btVector3 tmp2;
			v1.Sub( ref v0, out tmp1 );
			v2.Sub( ref v1, out tmp2 );
			btVector3 cp;
			tmp1.cross( ref tmp2, out cp );
			double m = cp.length();
			if( m == 0 )
			{
				result = btVector3.xAxis;
				return;
			}
			cp.Mult( ( 1.0 / m ), out result );
		}

		bool pointInTriangle(
			ref btVector3 vertice0,
			ref btVector3 vertice1,
			ref btVector3 vertice2,
			ref btVector3 normal, ref btVector3 p )
		{

			btVector3 edge1; vertice1.Sub( ref vertice0, out edge1 );// ( *p2 - *p1 );
			btVector3 edge2; vertice2.Sub( ref vertice1, out edge2 );//( *p3 - *p2 );
			btVector3 edge3; vertice0.Sub( ref vertice2, out edge3 );//( *p1 - *p3 );

			btVector3 p1_to_p; p.Sub( ref vertice0, out p1_to_p );// ( *p - *p1 );
			btVector3 p2_to_p; p.Sub( ref vertice1, out p2_to_p );// ( *p - *p2 );
			btVector3 p3_to_p; p.Sub( ref vertice2, out p3_to_p );// ( *p - *p3 );

			btVector3 edge1_normal; edge1.cross( ref normal, out edge1_normal );
			btVector3 edge2_normal; edge2.cross( ref normal, out edge2_normal );
			btVector3 edge3_normal; edge3.cross( ref normal, out edge3_normal );

			double r1, r2, r3;
			r1 = edge1_normal.dot( ref p1_to_p );
			r2 = edge2_normal.dot( ref p2_to_p );
			r3 = edge3_normal.dot( ref p3_to_p );
			if( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
				 ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
				return true;
			return false;

		}

		public static bool btRayAabb( ref btVector3 rayFrom,
										 ref btVector3 rayTo,
										 ref btVector3 aabbMin,
										 ref btVector3 aabbMax,
							  double param, ref btVector3 normal )
		{
			btVector3 aabbHalfExtent; aabbMax.SubScale( ref aabbMin ,  (double)( 0.5 ), out aabbHalfExtent );
			btVector3 aabbCenter;  aabbMax.AddScale( ref aabbMin, (double)( 0.5 ), out aabbCenter );
			btVector3 source;  rayFrom.Sub( ref aabbCenter, out source );
			btVector3 target;  rayTo.Sub( ref aabbCenter, out target );
			int sourceOutcode = btOutcode( ref source, ref aabbHalfExtent );
			int targetOutcode = btOutcode( ref target, ref aabbHalfExtent );
			if( ( sourceOutcode & targetOutcode ) == 0x0 )
			{
				double lambda_enter = btScalar.BT_ZERO;
				double lambda_exit = param;
				btVector3 r; target.Sub( ref source, out r );
				int i;
				double normSign = 1;
				btVector3 hitNormal = btVector3.Zero;
				int bit = 1;

				for( int j = 0; j < 2; j++ )
				{
					for( i = 0; i != 3; ++i )
					{
						if( ( sourceOutcode & bit ) != 0 )
						{
							double lambda = ( -source[i] - aabbHalfExtent[i] * normSign ) / r[i];
							if( lambda_enter <= lambda )
							{
								lambda_enter = lambda;
								hitNormal.setValue( 0, 0, 0 );
								hitNormal[i] = normSign;
							}
						}
						else if( ( targetOutcode & bit ) != 0  )
						{
							double lambda = ( -source[i] - aabbHalfExtent[i] * normSign ) / r[i];
							btScalar.btSetMin( ref lambda_exit, lambda );
						}
						bit <<= 1;
					}
					normSign = (double)( -1.0);
				}
				if( lambda_enter <= lambda_exit )
				{
					param = lambda_enter;
					normal = hitNormal;
					return true;
				}
			}
			return false;
		}

		public static void rayTest( btDbvtNode root,
								ref btVector3 rayFrom,
								ref btVector3 rayTo,
								ICollide policy )
		{

			using( btDbvtStackDataBlock stackDataBlock = new btDbvtStackDataBlock() )
			{
				if( root != null )
				{
					btVector3 rayDir = ( rayTo - rayFrom );
					rayDir.normalize();

					///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
					btVector3 rayDirectionInverse = new btVector3(
						rayDir.x == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.x,
						rayDir.y == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.y,
						rayDir.z == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.z );

					stackDataBlock.signs[0] = rayDirectionInverse.x < 0.0f ? (uint)1 :0;
					stackDataBlock.signs[1] = rayDirectionInverse.y < 0.0f ? (uint)1 :0;
					stackDataBlock.signs[2] = rayDirectionInverse.z < 0.0f ? (uint)1 :0;


					btVector3 tmp; rayTo.Sub( ref rayFrom, out tmp );
					double lambda_max = btVector3.dot( ref rayDir, ref tmp );


					int depth = 1;
					int treshold = DOUBLE_STACKSIZE - 2;

					stackDataBlock.stack.Count = ( DOUBLE_STACKSIZE );
					stackDataBlock.stack[0] = root;
					do
					{
						btDbvtNode node = stackDataBlock.stack[--depth];

						stackDataBlock.bounds0 = node.volume._min;
						stackDataBlock.bounds1 = node.volume._max;

						double tmin = 1.0f, lambda_min = 0.0f;
						bool result1 = btAabbUtil.btRayAabb2( ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs
							, ref stackDataBlock.bounds0
							, ref stackDataBlock.bounds1
							, out tmin, lambda_min, lambda_max );

#if COMPARE_BTRAY_AABB2
				double param=1.0f;
				bool result2 = AabbUtil.RayAabb(ref rayFrom,ref rayTo,node.volume.Mins(),node.volume.Maxs(),param,resultNormal);
				Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2

						if( result1 )
						{
							if( node.IsInternal() )
							{
								if( depth > treshold )
								{
									stackDataBlock.stack.Count = ( stackDataBlock.stack.Count * 2 );
									treshold = stackDataBlock.stack.Count - 2;
								}
								stackDataBlock.stack[depth++] = node._children0;
								stackDataBlock.stack[depth++] = node._children1;
							}
							else
							{
								policy.Process( node );
							}
						}
					} while( depth != 0 );

				}
			}
		}

			static double projectorigin( ref btVector3 a,
				ref btVector3 b,
				ref btVector3 c,
				double[] w, out uint m )
			{
				m = 100;
				int[] imd3 = { 1, 2, 0 };
				btVector3[] vt = { a, b, c };
				btVector3[] dl = new btVector3[3];
				a.Sub( ref b, out dl[0] );
				b.Sub( ref c, out dl[1] );
				c.Sub( ref a, out dl[2] );

				btVector3 n; btVector3.btCross( ref dl[0], ref dl[1], out n );
				double l = n.length2();
				if( l > GJK_SIMPLEX3_EPS )
				{
					double mindist = -1;
					double[] subw = { 0, 0 };
					uint subm = ( 0 );
					for( int i = 0; i < 3; ++i )
					{
						btVector3 tmp;
						btVector3.btCross( ref dl[i], ref n, out tmp );
						if( btVector3.btDot( ref vt[i], ref tmp ) > 0 )
						{
							int j = imd3[i];
							double subd = ( projectorigin( ref vt[i], ref vt[j], subw, out subm ) );
							if( ( mindist < 0 ) || ( subd < mindist ) )
							{
								mindist = subd;
								m = (uint)( ( ( subm & 1 ) != 0 ? 1 << i : 0 ) + ( ( subm & 2 ) != 0 ? 1 << j : 0 ) );
								w[i] = subw[0];
								w[j] = subw[1];
								w[imd3[j]] = 0;
							}
						}
					}
					if( mindist < 0 )
					{
						double d = btVector3.btDot( ref a, ref n );
						double s = btScalar.btSqrt( l );
						btVector3 p; n.Mult( ( d / l ), out p );
						mindist = p.length2();
						m = 7;
						btVector3 tmp, tmp2;
						b.Sub( ref p, out tmp );
						dl[1].cross( ref tmp, out tmp2 );
						w[0] = ( tmp2 ).length() / s;
						c.Sub( ref p, out tmp );
						dl[2].cross( ref tmp, out tmp2 );
						w[1] = ( tmp2 ).length() / s;
						w[2] = 1 - ( w[0] + w[1] );
					}
					return ( mindist );
				}
				return ( -1 );
			}

		static public void segmentsClosestPoints(
			out btVector3 ptsVector,
			out btVector3 offsetA,
			out btVector3 offsetB,
			out double tA, out double tB,
			ref btVector3 translation,
			ref btVector3 dirA, double hlenA,
			ref btVector3 dirB, double hlenB )
		{
			// compute the parameters of the closest points on each line segment

			double dirA_dot_dirB = btVector3.btDot( ref dirA, ref dirB );
			double dirA_dot_trans = btVector3.btDot( ref dirA, ref translation );
			double dirB_dot_trans = btVector3.btDot( ref dirB, ref translation );

			double denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;

			if( denom == 0.0f )
			{
				tA = 0.0f;
			}
			else
			{
				tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
				if( tA < -hlenA )
					tA = -hlenA;
				else if( tA > hlenA )
					tA = hlenA;
			}

			tB = tA * dirA_dot_dirB - dirB_dot_trans;

			if( tB < -hlenB )
			{
				tB = -hlenB;
				tA = tB * dirA_dot_dirB + dirA_dot_trans;

				if( tA < -hlenA )
					tA = -hlenA;
				else if( tA > hlenA )
					tA = hlenA;
			}
			else if( tB > hlenB )
			{
				tB = hlenB;
				tA = tB * dirA_dot_dirB + dirA_dot_trans;

				if( tA < -hlenA )
					tA = -hlenA;
				else if( tA > hlenA )
					tA = hlenA;
			}

			// compute the closest points relative to segment centers.

			dirA.Mult( tA, out offsetA );
			dirB.Mult( tB, out offsetB );

			btVector3 tmp;
			translation.Sub( ref offsetA, out tmp );
			tmp.Add( ref offsetB, out ptsVector );
			//ptsVector = translation - offsetA + offsetB;
		}

		public void PlaneProject( btVector3 point, out btVector3 result )
		{
			btVector3 tmp;
			normal.Mult( ( point.dot( ref normal ) + dist ), out tmp );
			point.Sub( ref tmp, out result );
		}

		static double calcArea4Points( ref btVector3 p0, ref btVector3 p1, ref btVector3 p2, ref btVector3 p3 )
		{
			// It calculates possible 3 area constructed from random 4 points and returns the biggest one.
			btVector3 a1, a2, a3;
			btVector3 b1, b2, b3;
			p0.Sub( ref p1, out a1 );
			p0.Sub( ref p2, out a2 );
			p0.Sub( ref p3, out a3 );
			p2.Sub( ref p3, out b1 );
			p1.Sub( ref p2, out b2 );
			p1.Sub( ref p3, out b3 );

			//todo: Following 3 cross production can be easily optimized by SIMD.
			btVector3 tmp0; a1.cross( ref b1, out tmp0 );
			btVector3 tmp1; a2.cross( ref b2, out tmp1 );
			btVector3 tmp2; a3.cross( ref b3, out tmp2 );

			return btScalar.btMax( btScalar.btMax( tmp0.length2(), tmp1.length2() ), tmp2.length2() );
		}

		public static double btDelLength( ref btVector3 linVelB, ref btVector3 linVelA )
		{
			btVector3 tmp;
			linVelB.Sub( ref linVelA, out tmp );
			return tmp.length();
		}

		public static void btCross2Del( ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d, out btVector3 result )
		{
			btVector3 tmp1;
			btVector3 tmp2;
			a.Sub( ref b, out tmp1 );
			c.Sub( ref d, out tmp2 );
			btCross( ref tmp1, ref tmp2, out result );
		}

		/*@brief Return the distance between the ends of this and another vector
          This is symantically treating the vector like a point */
		public double distance( ref btVector3 v )
		{
			btVector3 tmp;
			v.Sub( ref this, out tmp );
			return tmp.length();
		}

		public static double btDelLength2( ref btVector3 max, ref btVector3 min )
		{
			btVector3 tmp;
			max.Sub( ref min, out tmp );
			return tmp.dot( ref tmp );
		}

		public static double BetweenLength2( ref btVector3 min, ref btVector3 max )
		{
			btVector3 tmp;
			max.Sub( ref min, out tmp );
			return tmp.dot( ref tmp );
		}

		static bool above( btVector3[] vertices, int3 t, ref btVector3 p, double epsilon )
		{
			btVector3 n;
			btPlane.TriNormal( ref vertices[t[0]], ref vertices[t[1]], ref vertices[t[2]], out n );
			p.Sub( ref vertices[t[0]], out p );
			return ( n.dot( ref p ) > epsilon ); // btScalar.SIMD_EPSILON???
		}