C# btVector3.AddScale方法代码示例

		public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 supVertex )
		{

			localGetSupportingVertexWithoutMargin( ref vec, out supVertex );

			if( getMargin() != btScalar.BT_ZERO )
			{
				btVector3 vecnorm = vec;
				if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
				{
					vecnorm.setValue( (double)( -1.0 ), (double)( -1.0 ), (double)( -1.0 ) );
				}
				vecnorm.normalize();
				supVertex.AddScale( ref vecnorm, getMargin(), out supVertex );
			}
		}

		public static void btTransformAabb( ref btVector3 halfExtents, double margin,  btITransform t
				, out btVector3 aabbMinOut, out btVector3 aabbMaxOut )
		{
			btVector3 halfExtentsWithMargin; halfExtents.AddScale( ref btVector3.One, margin, out halfExtentsWithMargin );
			btMatrix3x3 abs_b; t.getBasis().absolute( out abs_b );
			btVector3 extent; halfExtentsWithMargin.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
			t.m_origin.Sub( ref extent, out aabbMinOut );
			t.m_origin.Add( ref extent, out aabbMaxOut );
		}

		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 virtual void drawSpherePatch( ref btVector3 center, ref btVector3 up, ref btVector3 axis, double radius,
			double minTh, double maxTh, double minPs, double maxPs, ref btVector3 color
			, double stepDegrees = btScalar.BT_TEN
			, bool drawCenter = true )
		{
			btVector3[] vA = new btVector3[74];
			btVector3[] vB = new btVector3[74];
			btVector3[] vT;
			//btVector3* pvA = vA, *pvB = vB, *pT;

			btVector3 npole; center.AddScale( ref up, radius, out npole );
			btVector3 spole;  center.AddScale( ref up, -radius, out spole );
			btVector3 arcStart = btVector3.Zero;
			double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG;
			btVector3 kv = up;
			btVector3 iv = axis;
			btVector3 jv; kv.cross( ref iv, out jv );
			bool drawN = false;
			bool drawS = false;
			if( minTh <= -btScalar.SIMD_HALF_PI )
			{
				minTh = -btScalar.SIMD_HALF_PI + step;
				drawN = true;
			}
			if( maxTh >= btScalar.SIMD_HALF_PI )
			{
				maxTh = btScalar.SIMD_HALF_PI - step;
				drawS = true;
			}
			if( minTh > maxTh )
			{
				minTh = -btScalar.SIMD_HALF_PI + step;
				maxTh = btScalar.SIMD_HALF_PI - step;
				drawN = drawS = true;
			}
			int n_hor = (int)( ( maxTh - minTh ) / step ) + 1;
			if( n_hor < 2 ) n_hor = 2;
			double step_h = ( maxTh - minTh ) / (double)( n_hor - 1 );
			bool isClosed = false;
			if( minPs > maxPs )
			{
				minPs = -btScalar.SIMD_PI + step;
				maxPs = btScalar.SIMD_PI;
				isClosed = true;
			}
			else if( ( maxPs - minPs ) >= btScalar.SIMD_2_PI )
			{
				isClosed = true;
			}
			else
			{
				isClosed = false;
			}
			int n_vert = (int)( ( maxPs - minPs ) / step ) + 1;
			if( n_vert < 2 ) n_vert = 2;
			double step_v = ( maxPs - minPs ) / (double)( n_vert - 1 );
			for( int i = 0; i < n_hor; i++ )
			{
				double th = minTh + (double)( i ) * step_h;
				double sth = radius * btScalar.btSin( th );
				double cth = radius * btScalar.btCos( th );
				for( int j = 0; j < n_vert; j++ )
				{
					double psi = minPs + (double)( j ) * step_v;
					double sps = btScalar.btSin( psi );
					double cps = btScalar.btCos( psi );

					center.AddScale( ref iv, cth * cps, out vB[j] );
					vB[j].AddScale( ref jv, cth * sps, out vB[j] );
					vB[j].AddScale( ref kv, sth, out vB[j] );
					//pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
					if( i != 0 )
					{
						drawLine( ref vA[j], ref vB[j], ref color );
					}
					else if( drawS )
					{
						drawLine( ref spole, ref vB[j], ref color );
					}
					if( j != 0 )
					{
						drawLine( ref vB[j - 1], ref vB[j], ref color );
					}
					else
					{
						arcStart = vB[j];
					}
					if( ( i == ( n_hor - 1 ) ) && drawN )
					{
						drawLine( ref npole, ref vB[j], ref color );
					}

					if( drawCenter )
					{
						if( isClosed )
						{
							if( j == ( n_vert - 1 ) )
							{
								drawLine( ref arcStart, ref vB[j], ref color );
							}
//.........这里部分代码省略.........

		public virtual void drawArc( ref btVector3 center, ref btVector3 normal, ref btVector3 axis
			, double radiusA, double radiusB, double minAngle, double maxAngle,
					ref btVector3 color, bool drawSect, double stepDegrees = btScalar.BT_TEN )
		{
			btVector3 vx = axis;
			btVector3 vy; normal.cross( ref axis, out vy );
			double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG;
			int nSteps = (int)btScalar.btFabs( ( maxAngle - minAngle ) / step );
			if( nSteps == 0 ) nSteps = 1;
			btVector3 prev;
			center.AddScale( ref vx, radiusA * btScalar.btCos( minAngle ), out prev );
			prev.AddScale( ref vy, radiusB * btScalar.btSin( minAngle ), out prev );
			if( drawSect )
			{
				drawLine( ref center, ref prev, ref color );
			}
			for( int i = 1; i <= nSteps; i++ )
			{
				double angle = minAngle + ( maxAngle - minAngle ) * i / (double)nSteps;
				btVector3 next;
				center.AddScale( ref vx, radiusA * btScalar.btCos( angle ), out next );
				next.AddScale( ref vy, radiusB * btScalar.btSin( angle ), out next );
				drawLine( ref prev, ref next, ref color );
				prev = next;
			}
			if( drawSect )
			{
				drawLine( ref center, ref prev, ref color );
			}
		}

		public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 supVertex )
		{
			localGetSupportingVertexWithoutMargin( ref vec, out supVertex );

			btVector3 vecnorm = vec;
			if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
			{
				vecnorm = btVector3.NegOne;
			}
			vecnorm.normalize();
			supVertex.AddScale( ref vecnorm, getMargin(), out supVertex );
			//supVertex += getMargin() * vecnorm;
		}

			static double projectorigin( ref btVector3 a,
				ref btVector3 b,
				double[] w, out uint m )
			{
				btVector3 d; b.Sub( ref a, out d );
				double l = d.length2();
				if( l > GJK_SIMPLEX2_EPS )
				{
					double t = ( l > 0 ? -btVector3.btDot( ref a, ref d ) / l : 0 );
					if( t >= 1 ) { w[0] = 0; w[1] = 1; m = 2; return ( b.length2() ); }
					else if( t <= 0 ) { w[0] = 1; w[1] = 0; m = 1; return ( a.length2() ); }
					else
					{
						w[0] = 1 - ( w[1] = t ); m = 3;
						btVector3 result;
						a.AddScale( ref d, t, out result );
						return ( result.length2() );
					}
				}
				m = 10;
				return ( -1 );
			}

			public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double orgDepth )
			{
				btVector3 endPt, startPt;
				double newDepth;
				//btVector3 newNormal;

				if( m_perturbA )
				{
					btVector3 endPtOrg; pointInWorld.AddScale( ref normalOnBInWorld, orgDepth, out endPtOrg );
					btTransform inv; m_transformA.inverse( out inv );
					btTransform tmp;
					m_unPerturbedTransform.Apply( ref inv, out tmp );
					/*endPt =*/
					tmp.Apply( ref endPtOrg, out endPt );
					btVector3 tmp2;
					endPt.Sub( ref pointInWorld, out tmp2 );
					newDepth = tmp2.dot( ref normalOnBInWorld );
					endPt.AddScale( ref normalOnBInWorld, newDepth, out startPt );
				}
				else
				{
					pointInWorld.AddScale( ref normalOnBInWorld, orgDepth, out endPt );
					btTransform inv; m_transformB.inverse( out inv );
					btTransform tmp;
					m_unPerturbedTransform.Apply( ref inv, out tmp );
					tmp.Apply( ref pointInWorld, out startPt );
					//startPt = ( m_unPerturbedTransform * m_transformB.inverse() )( pointInWorld );
					btVector3 tmp2;
					endPt.Sub( ref startPt, out tmp2 );
					newDepth = tmp2.dot( ref normalOnBInWorld );

				}

				//#define DEBUG_CONTACTS 1
#if DEBUG_CONTACTS
		m_debugDrawer.drawLine(ref startPt,ref endPt,btVector3(1,0,0));
		m_debugDrawer.drawSphere(ref startPt,0.05,btVector3(0,1,0));
		m_debugDrawer.drawSphere(ref endPt,0.05,btVector3(0,0,1));
#endif //DEBUG_CONTACTS


				m_originalManifoldResult.addContactPoint( ref normalOnBInWorld, ref startPt, newDepth );
			}

		internal void calculateTransforms( ref btTransform transA, ref btTransform transB )
		{
			if( m_useLinearReferenceFrameA /*|| ( !m_useSolveConstraintObsolete )*/ )
			{
				transA.Apply( ref m_frameInA, out m_calculatedTransformA );
				transB.Apply( ref m_frameInB, out m_calculatedTransformB );
			}
			else
			{
				transA.Apply( ref m_frameInA, out m_calculatedTransformB );
				transB.Apply( ref m_frameInB, out m_calculatedTransformA );
			}
			m_realPivotAInW = m_calculatedTransformA.m_origin;
			m_realPivotBInW = m_calculatedTransformB.m_origin;
			m_sliderAxis = m_calculatedTransformA.m_basis.getColumn( 0 ); // along X
			/*
			if( m_useLinearReferenceFrameA || m_useSolveConstraintObsolete )
			{
			  m_delta = m_realPivotBInW - m_realPivotAInW;
			}
			else
			*/
			{
				m_delta = m_realPivotAInW - m_realPivotBInW;
			}
			m_realPivotAInW.AddScale( ref m_sliderAxis, m_sliderAxis.dot( ref m_delta ), out m_projPivotInW );
			//m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot( ref m_delta ) * m_sliderAxis;
			btVector3 normalWorld;
			int i;
			//linear part
			for( i = 0; i < 3; i++ )
			{
				normalWorld = m_calculatedTransformA.m_basis.getColumn( i );
				m_depth[i] = m_delta.dot( normalWorld );
			}
		}

			public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
			{
				bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject;
				btVector3 pointA; pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA );
				btVector3 localA;
				btVector3 localB;
				if( isSwapped )
				{
					m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
					m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
				}
				else
				{
					m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
					m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
				}

				btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get();
				newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth );
				btScalar.Dbg( "2 add contact point " + pointA.ToString() + " and " + pointInWorld.ToString() );
				newPt.m_positionWorldOnA = pointA;
				newPt.m_positionWorldOnB = pointInWorld;

				//BP mod, store contact triangles.
				if( isSwapped )
				{
					newPt.m_partId0 = m_partId1;
					newPt.m_partId1 = m_partId0;
					newPt.m_index0 = m_index1;
					newPt.m_index1 = m_index0;
				}
				else
				{
					newPt.m_partId0 = m_partId0;
					newPt.m_partId1 = m_partId1;
					newPt.m_index0 = m_index0;
					newPt.m_index1 = m_index1;
				}

				//experimental feature info, for per-triangle material etc.
				btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
				btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
				m_resultCallback.addSingleResult( newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 );

			}

		// See also geometrictools.com
		// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
		double SegmentSqrDistance( ref btVector3 from, ref btVector3 to, ref btVector3 p, out btVector3 nearest )
		{
			btVector3 diff = p - from;
			btVector3 v = to - from;
			double t = v.dot( diff );

			if( t > 0 )
			{
				double dotVV = v.dot( v );
				if( t < dotVV )
				{
					t /= dotVV;
					diff.AddScale( ref v, -t, out diff );
					//diff -= t * v;
				}
				else
				{
					t = 1;
					diff.Sub( ref v, out diff );
					//diff -= v;
				}
			}
			else
				t = 0;

			from.AddScale( ref v, t, out nearest );
			//nearest = from + t * v;
			return diff.dot( ref diff );
		}

		public virtual void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
		{
			Debug.Assert( m_manifoldPtr != null );
			//order in manifold needs to match

			if( depth > m_manifoldPtr.getContactBreakingThreshold() )
				//	if (depth > m_manifoldPtr.getContactProcessingThreshold())
				return;

			bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject;

			btVector3 pointA;// = pointInWorld + normalOnBInWorld * depth;
			pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA );

			btVector3 localA;
			btVector3 localB;

			if( isSwapped )
			{
				m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
				m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
			}
			else
			{
				m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
				m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
			}

			btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get();
			newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth );
			btScalar.Dbg( "add contact point " + pointA + " and " + pointInWorld );
			newPt.m_positionWorldOnA = pointA;
			newPt.m_positionWorldOnB = pointInWorld;

			int insertIndex = m_manifoldPtr.getCacheEntry( ref newPt );
			newPt.m_combinedFriction = calculateCombinedFriction( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject );
			newPt.m_combinedRestitution = calculateCombinedRestitution( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject );
			newPt.m_combinedRollingFriction = calculateCombinedRollingFriction( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject );
			btVector3.btPlaneSpace1( ref newPt.m_normalWorldOnB, out newPt.m_lateralFrictionDir1, out newPt.m_lateralFrictionDir2 );



			//BP mod, store contact triangles.
			if( isSwapped )
			{
				newPt.m_partId0 = m_partId1;
				newPt.m_partId1 = m_partId0;
				newPt.m_index0 = m_index1;
				newPt.m_index1 = m_index0;
			}
			else
			{
				newPt.m_partId0 = m_partId0;
				newPt.m_partId1 = m_partId1;
				newPt.m_index0 = m_index0;
				newPt.m_index1 = m_index1;
			}
			//Console.WriteLine("depth=%f\n",depth);
			///@todo, check this for any side effects
			if( insertIndex >= 0 )
			{
				//btManifoldPoint oldPoint = m_manifoldPtr.getContactPoint(insertIndex);
				m_manifoldPtr.replaceContactPoint( newPt, insertIndex );
			}
			else
			{
				insertIndex = m_manifoldPtr.addManifoldPoint( newPt );
			}

			//User can override friction and/or restitution
			if( gContactAddedCallback != null &&
				 //and if either of the two bodies requires custom material
				 ( ( m_body0Wrap.m_collisionObject.getCollisionFlags() & btCollisionObject.CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK ) != 0 ||
				   ( m_body1Wrap.m_collisionObject.getCollisionFlags() & btCollisionObject.CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK ) != 0 ) )
			{
				//experimental feature info, for per-triangle material etc.
				btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
				btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
				gContactAddedCallback( m_manifoldPtr.getContactPoint( insertIndex ), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 );
			}

		}