C# btVector3.length方法代码示例

		/*@brief Set the rotation using axis angle notation 
		  @param axis The axis around which to rotate
		  @param angle The magnitude of the rotation in Radians */
		public void setRotation( ref btVector3 axis, double _angle )
		{
			double d = axis.length();
			if( d != 0.0 )
			{
				double s = btScalar.btSin( _angle * 0.5 ) / d;
				setValue( axis.x * s, axis.y * s, axis.z * s,
					btScalar.btCos( _angle * 0.5 ) );
			}
			else
				x = y = z = w = 0;
		}

		/*@brief Axis angle Constructor
		  @param axis The axis which the rotation is around
		  @param angle The magnitude of the rotation around the angle (Radians) */
		public btQuaternion( ref btVector3 _axis, float _angle )
		{
			//setRotation( _axis, _angle );
			float d = _axis.length();
			if( d != 0.0 )
			{
				float s = btScalar.btSin( _angle * btScalar.BT_HALF ) / d;
				x = _axis.x * s;
				y = _axis.y * s;
				z = _axis.z * s;
				w = btScalar.btCos( _angle * btScalar.BT_HALF );
			}
			else
				x = y = z = w = 0;
		}

		/*@brief Axis angle Constructor
		  @param axis The axis which the rotation is around
		  @param angle The magnitude of the rotation around the angle (Radians) */
		public btQuaternion( ref btVector3 _axis, double _angle )
		{
			//setRotation( _axis, _angle );
			double d = _axis.length();
			if( d != 0.0 )
			{
				double s = btScalar.btSin( _angle * 0.5 ) / d;
				x = _axis.x * s;
				y = _axis.y * s;
				z = _axis.z * s;
				w = btScalar.btCos( _angle * 0.5 );
			}
			else
				x = y = z = w = 0;
		}

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

		void coneLocalSupport( ref btVector3 v, out btVector3 result )
		{
			btVector3 tmp = btVector3.Zero;

			double halfHeight = m_height * (double)( 0.5 );

			if( v[m_coneIndices[1]] > v.length() * m_sinAngle )
			{

				tmp[m_coneIndices[0]] = btScalar.BT_ZERO;
				tmp[m_coneIndices[1]] = halfHeight;
				tmp[m_coneIndices[2]] = btScalar.BT_ZERO;
				result = tmp;
			}
			else
			{
				double s = btScalar.btSqrt( v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]] );
				if( s > btScalar.SIMD_EPSILON )
				{
					double d = m_radius / s;
					tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
					tmp[m_coneIndices[1]] = -halfHeight;
					tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
					result = tmp;
				}
				else
				{
					tmp[m_coneIndices[0]] = btScalar.BT_ZERO;
					tmp[m_coneIndices[1]] = -halfHeight;
					tmp[m_coneIndices[2]] = btScalar.BT_ZERO;
					result = tmp;
				}
			}

		}

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

			internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess )
			{
				uint iterations = 0;
				double sqdist = 0;
				double alpha = 0;
				btVector3[] lastw = new btVector3[4];
				uint clastw = 0;
				/* Initialize solver		*/
				m_free[0] = new sSV();
				m_free[1] = new sSV();
				m_free[2] = new sSV();
				m_free[3] = new sSV();
				m_nfree = 4;
				m_current = 0;
				m_status = eStatus._.Valid;
				m_shape = shapearg;
				m_distance = 0;
				/* Initialize simplex		*/
				m_simplices0.rank = 0;
				m_ray = guess;
				double sqrl = m_ray.length2();
				btVector3 tmp;
				if( sqrl > 0 )
					m_ray.Invert( out tmp );
				else
					tmp = btVector3.xAxis;
				appendvertice( m_simplices0, ref tmp );
				m_simplices0.p[0] = 1;
				m_ray = m_simplices0.c[0].w;
				sqdist = sqrl;
				lastw[0] =
					lastw[1] =
					lastw[2] =
					lastw[3] = m_ray;
				/* Loop						*/
				do
				{
					uint next = 1 - m_current;
					sSimplex cs = m_current==0?m_simplices0:m_simplices1;
					sSimplex ns = next==0?m_simplices0:m_simplices1;
					/* Check zero							*/
					double rl = m_ray.length();
					if( rl < GJK_MIN_DISTANCE )
					{/* Touching or inside				*/
						m_status = eStatus._.Inside;
						break;
					}
					/* Append new vertice in -'v' direction	*/
					m_ray.Invert( out tmp );
					appendvertice( cs, ref tmp );
					btVector3 w = cs.c[cs.rank - 1].w;
					bool found = false;
					for( uint i = 0; i < 4; ++i )
					{
						w.Sub( ref lastw[i], out tmp );
						if( tmp.length2() < GJK_DUPLICATED_EPS )
						{ found = true; break; }
					}
					if( found )
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					else
					{/* Update lastw					*/
						lastw[clastw = ( clastw + 1 ) & 3] = w;
					}
					/* Check for termination				*/
					double omega = btVector3.btDot( ref m_ray, ref w ) / rl;
					alpha = btScalar.btMax( omega, alpha );
					if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 )
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					/* Reduce simplex						*/
					double[] weights = new double[4];
					uint mask = 0;
					switch( cs.rank )
					{
						case 2:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								weights, out mask ); break;
						case 3:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								ref cs.c[2].w,
								weights, out mask ); break;
						case 4:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								ref cs.c[2].w,
								ref cs.c[3].w,
								weights, out mask ); break;
					}
					if( sqdist >= 0 )
					{/* Valid	*/
						ns.rank = 0;
						m_ray = btVector3.Zero;
//.........这里部分代码省略.........

		/*@brief Set the rotation using axis angle notation 
		  @param axis The axis around which to rotate
		  @param angle The magnitude of the rotation in Radians */
		public void setRotation( ref btVector3 axis, float _angle )
		{
			float d = axis.length();
			if( d != 0.0 )
			{
				float s = btScalar.btSin( _angle * btScalar.BT_HALF ) / d;
				setValue( axis.x * s, axis.y * s, axis.z * s,
					btScalar.btCos( _angle * btScalar.BT_HALF ) );
			}
			else
				x = y = z = w = 0;
		}

		///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
		///result is conservative
		public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep
			, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax )
		{
			//start with static aabb
			getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax );

			double temporalAabbMaxx = temporalAabbMax.x;
			double temporalAabbMaxy = temporalAabbMax.y;
			double temporalAabbMaxz = temporalAabbMax.z;
			double temporalAabbMinx = temporalAabbMin.x;
			double temporalAabbMiny = temporalAabbMin.y;
			double temporalAabbMinz = temporalAabbMin.z;

			// add linear motion
			btVector3 linMotion;
			linvel.Mult( timeStep, out linMotion );
			///@todo: simd would have a vector max/min operation, instead of per-element access
			if( linMotion.x > btScalar.BT_ZERO )
				temporalAabbMaxx += linMotion.x;
			else
				temporalAabbMinx += linMotion.x;
			if( linMotion.y > btScalar.BT_ZERO )
				temporalAabbMaxy += linMotion.y;
			else
				temporalAabbMiny += linMotion.y;
			if( linMotion.z > btScalar.BT_ZERO )
				temporalAabbMaxz += linMotion.z;
			else
				temporalAabbMinz += linMotion.z;

			//add conservative angular motion
			double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
			btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion );
			temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz );
			temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz );
			temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin );
			temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax );
		}

		/*
		void solveConstraintObsolete( btSolverBody bodyA, btSolverBody bodyB, double timeStep )
		{
			if( m_useSolveConstraintObsolete )
			{
				btVector3 pivotAInW = m_rbA.m_worldTransform * m_rbAFrame.m_origin;
				btVector3 pivotBInW = m_rbB.m_worldTransform * m_rbBFrame.m_origin;

				double tau = (double)( 0.3 );

				//linear part
				if( !m_angularOnly )
				{
					btVector3 rel_pos1 = pivotAInW - m_rbA.m_worldTransform.m_origin;
					btVector3 rel_pos2 = pivotBInW - m_rbB.m_worldTransform.m_origin;

					btVector3 vel1;
					bodyA.internalGetVelocityInLocalPointObsolete( rel_pos1, vel1 );
					btVector3 vel2;
					bodyB.internalGetVelocityInLocalPointObsolete( rel_pos2, vel2 );
					btVector3 vel = vel1 - vel2;

					for( int i = 0; i < 3; i++ )
					{		
						btIVector3 normal = m_jac[i].m_linearJointAxis;
						double jacDiagABInv = btScalar.BT_ONE / m_jac[i].getDiagonal();

						double rel_vel;
						rel_vel = normal.dot( vel );
						//positional error (zeroth order error)
						double depth = -( pivotAInW - pivotBInW ).dot( normal ); //this is the error projected on the normal
						double impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
						m_appliedImpulse += impulse;

						btVector3 ftorqueAxis1 = rel_pos1.cross( normal );
						btVector3 ftorqueAxis2 = rel_pos2.cross( normal );
						bodyA.internalApplyImpulse( normal * m_rbA.getInvMass(), m_rbA.m_invInertiaTensorWorld * ftorqueAxis1, impulse );
						bodyB.internalApplyImpulse( normal * m_rbB.getInvMass(), m_rbB.m_invInertiaTensorWorld * ftorqueAxis2, -impulse );

					}
				}

				// apply motor
				if( m_bMotorEnabled )
				{
					// compute current and predicted transforms
					btTransform trACur = m_rbA.m_worldTransform;
					btTransform trBCur = m_rbB.m_worldTransform;
					btVector3 omegaA; bodyA.internalGetAngularVelocity( omegaA );
					btVector3 omegaB; bodyB.internalGetAngularVelocity( omegaB );
					btTransform trAPred; trAPred.setIdentity();
					btVector3 zerovec( 0, 0, 0);
					btTransformUtil::integrateTransform(
						trACur, zerovec, omegaA, timeStep, trAPred );
					btTransform trBPred; trBPred.setIdentity();
					btTransformUtil::integrateTransform(
						trBCur, zerovec, omegaB, timeStep, trBPred );

					// compute desired transforms in world
					btTransform trPose( m_qTarget );
					btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
					btTransform trADes = trBPred * trABDes;
					btTransform trBDes = trAPred * trABDes.inverse();

					// compute desired omegas in world
					btVector3 omegaADes, omegaBDes;

					btTransformUtil::calculateVelocity( trACur, trADes, timeStep, zerovec, omegaADes );
					btTransformUtil::calculateVelocity( trBCur, trBDes, timeStep, zerovec, omegaBDes );

					// compute delta omegas
					btVector3 dOmegaA = omegaADes - omegaA;
					btVector3 dOmegaB = omegaBDes - omegaB;

					// compute weighted avg axis of dOmega (weighting based on inertias)
					btVector3 axisA, axisB;
					double kAxisAInv = 0, kAxisBInv = 0;

					if( dOmegaA.length2() > btScalar.SIMD_EPSILON )
					{
						axisA = dOmegaA.normalized();
						kAxisAInv = m_rbA.computeAngularImpulseDenominator( axisA );
					}

					if( dOmegaB.length2() > btScalar.SIMD_EPSILON )
					{
						axisB = dOmegaB.normalized();
						kAxisBInv = m_rbB.computeAngularImpulseDenominator( axisB );
					}

					btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;

					static bool bDoTorque = true;
					if( bDoTorque & avgAxis.length2() > btScalar.SIMD_EPSILON )
					{
						avgAxis.normalize();
						kAxisAInv = m_rbA.computeAngularImpulseDenominator( avgAxis );
						kAxisBInv = m_rbB.computeAngularImpulseDenominator( avgAxis );
						double kInvCombined = kAxisAInv + kAxisBInv;

//.........这里部分代码省略.........