C# btVector3.Mult方法代码示例

		static void convexHullSupport( ref btVector3 localDirOrg, btVector3[] points, int numPoints, ref btVector3 localScaling, out btVector3 result )
		{
			btVector3 vec; localDirOrg.Mult( ref localScaling, out vec );
			double maxDot;
			long ptIndex = vec.maxDot( points, numPoints, out maxDot );
			Debug.Assert( ptIndex >= 0 );
			points[ptIndex].Mult( ref localScaling, out result );
		}

		public static void integrateTransform( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel
			, double timeStep, out btTransform predictedTransform )
		{
			btVector3 tmp;
			btVector3 tmp2;
			linvel.Mult( timeStep, out tmp );
			curTrans.getOrigin( out tmp2 );
            tmp2.Add( ref tmp, out predictedTransform.m_origin );
#if QUATERNION_DERIVATIVE
		btQuaternion predictedOrn = curTrans.getRotation();
		predictedOrn += (angvel predictedOrn) * (timeStep 0.5);
		predictedOrn.normalize();
#else
			//Exponential map
			//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

			btVector3 axis;
			double fAngle = angvel.length();
			//limit the angular motion
			if( fAngle * timeStep > ANGULAR_MOTION_THRESHOLD )
			{
				fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
			}

			if( fAngle < 0.001 )
			{
				// use Taylor's expansions of sync function
				angvel.Mult( ( btScalar.BT_HALF * timeStep - ( timeStep * timeStep * timeStep ) * ( 0.020833333333 ) * fAngle * fAngle ), out axis );
			}
			else
			{
				// sync(fAngle) = sin(cfAngle)/t
				angvel.Mult( ( btScalar.btSin( btScalar.BT_HALF * fAngle * timeStep ) / fAngle ), out axis );
			}
			btQuaternion dorn = new btQuaternion( axis.x, axis.y, axis.z, btScalar.btCos( fAngle * timeStep * 0.5 ) );
			btQuaternion orn0;
			curTrans.getRotation( out orn0 );

			btQuaternion predictedOrn;
			dorn.Mult( ref orn0, out predictedOrn );
			predictedOrn.normalize();
#endif
			btMatrix3x3.setRotation( out predictedTransform.m_basis, ref predictedOrn);
			//predictedTransform.setRotation( ref predictedOrn );
		}

		public override void calculateLocalInertia( double mass, out btVector3 inertia )
		{
			btVector3 aabbMin, aabbMax;
			getAabb( ref btTransform.Identity, out aabbMin, out aabbMax );
			btVector3 tmp;
			aabbMax.Sub( ref aabbMin, out tmp );
			btVector3 halfExtents;
			tmp.Mult( btScalar.BT_HALF, out halfExtents );

			double margin = getMargin();

			double lx = (double)( 2.0 ) * ( halfExtents.x + margin );
			double ly = (double)( 2.0 ) * ( halfExtents.y + margin );
			double lz = (double)( 2.0 ) * ( halfExtents.z + margin );
			double x2 = lx * lx;
			double y2 = ly * ly;
			double z2 = lz * lz;
			double scaledmass = mass * (double)( 0.08333333 );

			tmp = new btVector3( y2 + z2, x2 + z2, x2 + y2 );
			tmp.Mult( scaledmass, out inertia );
		}

		public btCylinderShape( ref btVector3 halfExtents )
		{
			m_upAxis = ( 1 );
			setSafeMargin( ref halfExtents );

			btVector3 margin = new btVector3( getMargin() );
			btVector3 tmp;
			halfExtents.Mult( ref m_localScaling, out tmp );
			tmp.Sub( ref margin, out m_implicitShapeDimensions );
			//m_implicitShapeDimensions = ( halfExtents * m_localScaling ) - margin;
			m_shapeType = BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE;
		}

		///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
		public void applyDamping( double timeStep )
		{
			//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
			//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway

			//#define USE_OLD_DAMPING_METHOD 1
#if USE_OLD_DAMPING_METHOD
			m_linearVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_linearDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) );
			m_angularVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_angularDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) );
#else
			m_linearVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ), out m_linearVelocity );
			m_angularVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ), out m_angularVelocity );
			//m_linearVelocity *= btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep );
			//m_angularVelocity *= btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep );
#endif

			if( m_additionalDamping )
			{
				//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
				//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
				if( ( m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr ) &
					( m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr ) )
				{
					m_linearVelocity.Mult( m_additionalDampingFactor, out m_linearVelocity );
					m_angularVelocity.Mult( m_additionalDampingFactor, out m_angularVelocity );
					//m_angularVelocity *= m_additionalDampingFactor;
					//m_linearVelocity *= m_additionalDampingFactor;
				}


				double speed = m_linearVelocity.length();
				if( speed < m_linearDamping )
				{
					double dampVel = (double)( 0.005 );
					if( speed > dampVel )
					{
						btVector3 dir; m_linearVelocity.normalized( out dir );
						dir.Mult( dampVel, out dir );
						m_linearVelocity.Sub( ref dir, out m_linearVelocity );
						//m_linearVelocity -= dir * dampVel;
					}
					else
					{
						m_linearVelocity = btVector3.Zero;
					}
				}

				double angSpeed = m_angularVelocity.length();
				if( angSpeed < m_angularDamping )
				{
					double angDampVel = (double)( 0.005 );
					if( angSpeed > angDampVel )
					{
						btVector3 dir; m_angularVelocity.normalized( out dir );
						dir.Mult( angDampVel, out dir );
						m_angularVelocity.Sub( ref dir, out m_angularVelocity );
						//m_angularVelocity -= dir * angDampVel;
					}
					else
					{
						m_angularVelocity = btVector3.Zero;
					}
				}
			}
		}

		public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt,
			ref btMatrix3x3 I, out btVector3 result )
		{
			btVector3 Iw0;
			btVector3 dtT;
			btVector3 Iw1;
			btVector3 cross;
			T.Mult( dt, out dtT );
			I.Apply( ref w0, out Iw0 );
			dtT.Add( ref Iw0, out Iw0 );
			// Iw0 = ( T * dt + I * w0 )

			I.Apply( ref w1, out Iw1 );
			w1.cross( ref Iw1, out cross );
			cross.Mult( dt, out cross );
			// cross = w1.cross( I * w1 ) * dt
			Iw1.Add( ref cross, out Iw1 );
			// Iw1 = Iw1 + cross

			Iw1.Sub( ref Iw0, out result );
			//btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 );
			//return w2;
		}

		//! apply the correction impulses for two bodies
		//double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
		internal double solveAngularLimits(
			double timeStep, ref btVector3 axis, double jacDiagABInv,
			btRigidBody body0, btRigidBody body1 )
		{
			if( needApplyTorques() == false ) return 0.0f;

			double target_velocity = m_targetVelocity;
			double maxMotorForce = m_maxMotorForce;

			//current error correction
			if( m_currentLimit != 0 )
			{
				target_velocity = -m_stopERP * m_currentLimitError / ( timeStep );
				maxMotorForce = m_maxLimitForce;
			}

			maxMotorForce *= timeStep;

			// current velocity difference

			btVector3 angVelA = body0.getAngularVelocity();
			btVector3 angVelB = body1.getAngularVelocity();

			btVector3 vel_diff;
			vel_diff = angVelA - angVelB;



			double rel_vel = axis.dot( vel_diff );

			// correction velocity
			double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel );


			if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON )
			{
				return 0.0f;//no need for applying force
			}


			// correction impulse
			double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv;

			// clip correction impulse
			double clippedMotorImpulse;

			///@todo: should clip against accumulated impulse
			if( unclippedMotorImpulse > 0.0f )
			{
				clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
			}
			else
			{
				clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
			}


			// sort with accumulated impulses
			double lo = (double)( -btScalar.BT_LARGE_FLOAT );
			double hi = (double)( btScalar.BT_LARGE_FLOAT );

			double oldaccumImpulse = m_accumulatedImpulse;
			double sum = oldaccumImpulse + clippedMotorImpulse;
			m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;

			clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;

			btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp );

			body0.applyTorqueImpulse( ref motorImp );
			btVector3 tmp;
			motorImp.Invert( out tmp );
			body1.applyTorqueImpulse( ref tmp );

			return clippedMotorImpulse;


		}

		///setupRigidBody is only used internally by the constructor
		public void setupRigidBody( btRigidBodyConstructionInfo constructionInfo )
		{

			m_internalType = CollisionObjectTypes.CO_RIGID_BODY;

			m_linearVelocity = btVector3.Zero;
			m_angularVelocity = btVector3.Zero;
			m_angularFactor = btVector3.One;
			m_linearFactor = btVector3.One;
			m_gravity = btVector3.Zero;
			m_gravity_acceleration = btVector3.Zero;
			m_totalForce = btVector3.Zero;
			m_totalTorque = btVector3.Zero;
			setDamping( constructionInfo.m_linearDamping, constructionInfo.m_angularDamping );

			m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
			m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
			m_optionalMotionState = constructionInfo.m_motionState;
			//m_contactSolverType = 0;
			//m_frictionSolverType = 0;
			m_additionalDamping = constructionInfo.m_additionalDamping;
			m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
			m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
			m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
			m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;

			if( m_optionalMotionState != null )
			{
				m_optionalMotionState.getWorldTransform( out m_worldTransform );
			}
			else
			{
				m_worldTransform = constructionInfo.m_startWorldTransform;
			}

			m_interpolationWorldTransform = m_worldTransform;
			m_interpolationLinearVelocity.setValue( 0, 0, 0 );
			m_interpolationAngularVelocity.setValue( 0, 0, 0 );

			//moved to btCollisionObject
			m_friction = constructionInfo.m_friction;
			m_rollingFriction = constructionInfo.m_rollingFriction;
			m_restitution = constructionInfo.m_restitution;

			setCollisionShape( constructionInfo.m_collisionShape );
			m_debugBodyId = uniqueId++;

			setMassProps( constructionInfo.m_mass, ref constructionInfo.m_localInertia );
			updateInertiaTensor();

			m_rigidbodyFlags = btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;

			m_deltaLinearVelocity.setZero();
			m_deltaAngularVelocity.setZero();
			m_linearFactor.Mult( m_inverseMass, out m_invMass );
			//m_invMass = m_inverseMass * m_linearFactor;
			m_pushVelocity.setZero();
			m_turnVelocity.setZero();
		}

		//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
		public void applyImpulse( ref btVector3 linearComponent, ref btVector3 angularComponent, double impulseMagnitude )
		{
			if( m_originalBody != null )
			{
				btVector3 tmp;
				linearComponent.Mult( ref m_linearFactor, out tmp );
				m_deltaLinearVelocity.AddScale( ref tmp, impulseMagnitude, out m_deltaLinearVelocity );
				//m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
				angularComponent.Mult( ref m_angularFactor, out tmp );
				m_deltaAngularVelocity.AddScale( ref tmp, impulseMagnitude, out m_deltaAngularVelocity );
				//if( m_deltaLinearVelocity.length() > 20 )
				//	Debugger.Break();
				//m_deltaAngularVelocity += angularComponent * ( impulseMagnitude * m_angularFactor );
				btScalar.Dbg( "apply impulse delta linear velocity is " + m_deltaLinearVelocity.ToString() );

			}
		}

		protected void initSolverBody( btSolverBody solverBody, btRigidBody rb, double timeStep )
		{
			//btRigidBody rb = collisionObject != null ? btRigidBody.upcast( collisionObject ) : null;

			solverBody.m_deltaLinearVelocity = btVector3.Zero;
			solverBody.m_deltaAngularVelocity = btVector3.Zero;
			solverBody.m_pushVelocity = btVector3.Zero;
			solverBody.m_turnVelocity = btVector3.Zero;
			solverBody.modified = false;
			solverBody.pushed = false;

			if( rb != null )
			{
				solverBody.m_worldTransform = rb.m_worldTransform;
				btVector3 tmp = new btVector3( rb.getInvMass() );
				btVector3 tmp2;
				btVector3 tmp3;
				rb.getLinearFactor( out tmp2 );
				tmp.Mult( ref tmp2, out tmp3 );
				solverBody.internalSetInvMass( ref tmp3 );
				solverBody.m_originalBody = rb;
				rb.getAngularFactor( out solverBody.m_angularFactor );
				rb.getLinearFactor( out solverBody.m_linearFactor );
				rb.getLinearVelocity( out solverBody.m_linearVelocity );
				rb.getAngularVelocity( out solverBody.m_angularVelocity );
				rb.m_totalForce.Mult( rb.m_inverseMass * timeStep, out solverBody.m_externalForceImpulse );
				//= rb.getTotalForce() * rb.getInvMass() * timeStep;
				rb.m_invInertiaTensorWorld.Apply( ref rb.m_totalTorque, out tmp );
				tmp.Mult( timeStep, out solverBody.m_externalTorqueImpulse );
				btScalar.Dbg( "Setup external impulses " + solverBody.m_externalForceImpulse + " " + solverBody.m_externalTorqueImpulse );
				///solverBody.m_externalTorqueImpulse = rb.getTotalTorque() * rb.getInvInertiaTensorWorld() * timeStep;

			}
			else
			{
				solverBody.modified = false;
				solverBody.m_worldTransform = btTransform.Identity;
				solverBody.internalSetInvMass( ref btVector3.Zero );
				solverBody.m_originalBody = null;
				solverBody.m_angularFactor = btVector3.One;
				solverBody.m_linearFactor = btVector3.One;
				solverBody.m_linearVelocity = btVector3.Zero;
				solverBody.m_angularVelocity = btVector3.Zero;
				solverBody.m_externalForceImpulse = btVector3.Zero;
				solverBody.m_externalTorqueImpulse = btVector3.Zero;
			}


		}

		public override void localGetSupportingVertexWithoutMargin( ref btVector3 vec, out btVector3 result )
		{
			result = btVector3.Zero;
			double maxDot = (double)( -btScalar.BT_LARGE_FLOAT );

			// Here we take advantage of dot(a, b*c) = dot(a*b, c).  Note: This is true mathematically, but not numerically. 
			if( 0 < m_unscaledPoints.Count )
			{
				btVector3 scaled; vec.Mult(ref m_localScaling, out scaled );
				int index = (int)scaled.maxDot( m_unscaledPoints.InternalArray, m_unscaledPoints.Count, out maxDot ); // FIXME: may violate encapsulation of m_unscaledPoints
				m_unscaledPoints[index].Mult( ref m_localScaling, out result );
			}

			//return supVec;
		}

		public void applyPushImpulse( ref btVector3 linearComponent, ref btVector3 angularComponent, double impulseMagnitude )
		{
			if( m_originalBody != null )
			{
				btVector3 tmp;
				pushed = true;
				linearComponent.Mult( ref m_linearFactor, out tmp );
				m_pushVelocity.AddScale( ref tmp, impulseMagnitude, out m_pushVelocity );
				//m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
				angularComponent.Mult( ref m_angularFactor, out tmp );
				m_turnVelocity.AddScale( ref tmp, impulseMagnitude, out m_turnVelocity );
				//m_turnVelocity += angularComponent * ( impulseMagnitude * m_angularFactor );
			}
		}

		static public double capsuleCapsuleDistance(
			out btVector3 normalOnB,
			out btVector3 pointOnB,
			double capsuleLengthA,
			double capsuleRadiusA,
			double capsuleLengthB,
			double capsuleRadiusB,
			int capsuleAxisA,
			int capsuleAxisB,
			ref btTransform transformA,
			ref btTransform transformB,
			double distanceThreshold )
		{
			btVector3 directionA; transformA.m_basis.getColumn( capsuleAxisA, out directionA );
			btVector3 translationA; transformA.getOrigin( out translationA );
			btVector3 directionB; transformB.m_basis.getColumn( capsuleAxisB, out directionB );
			btVector3 translationB; transformB.getOrigin( out translationB );

			// translation between centers

			btVector3 translation; translationB.Sub( ref translationA, out translation );

			// compute the closest points of the capsule line segments

			btVector3 ptsVector;           // the vector between the closest points

			btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
			double tA, tB;              // parameters on line segment

			segmentsClosestPoints( out ptsVector, out offsetA, out offsetB, out tA, out tB, ref translation,
								   ref directionA, capsuleLengthA, ref directionB, capsuleLengthB );

			double distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;

			if( distance > distanceThreshold )
			{
				pointOnB = btVector3.Zero;
				normalOnB = btVector3.xAxis;
				return distance;
			}

			double lenSqr = ptsVector.length2();
			if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
			{
				//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
				btVector3 q;
				btVector3.btPlaneSpace1( ref directionA, out normalOnB, out q );
			}
			else
			{
				// compute the contact normal
				ptsVector.Mult( -btScalar.btRecipSqrt( lenSqr ), out normalOnB );
			}
			btVector3 tmp;
			btVector3 tmp2;
			translationB.Add( ref offsetB, out tmp );
			normalOnB.Mult( capsuleRadiusB, out tmp2 );
			tmp.Add( ref tmp2, out pointOnB );
			//pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB;

			return distance;
		}

		public virtual void drawAabb( ref btVector3 from, ref btVector3 to, ref btVector3 color )
		{

			btVector3 halfExtents; to.SubAndScale( ref from, 0.5f, out halfExtents );
			btVector3 center; to.AddAndScale( ref from, 0.5f, out center );
			int i, j;

			btVector3 edgecoord = new btVector3( 1f, 1f, 1f ), pa, pb;
			for( i = 0; i < 4; i++ )
			{
				for( j = 0; j < 3; j++ )
				{
					edgecoord.Mult( ref halfExtents, out pa );
					pa.Add( ref center, out pa );


					int othercoord = j % 3;
					edgecoord[othercoord] *= -1f;

					edgecoord.Mult( ref halfExtents, out pb );
					pb.Add( ref center, out pb );

					drawLine( ref pa, ref pb, ref color );
				}
				edgecoord.setValue( -1, -1, -1 );
				if( i < 3 )
					edgecoord[i] *= -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;
		}