C# btVector3.length2方法代码示例

		///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;
//.........这里部分代码省略.........

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

		internal static void calculateDiffAxisAngleQuaternion( ref btQuaternion orn0, ref btQuaternion orn1a, out btVector3 axis, out double angle )
		{
			btQuaternion orn1;
			orn0.nearest( ref orn1a, out orn1 );
			btQuaternion dorn;
			btQuaternion tmp;
			orn0.inverse( out tmp );
			orn1.Mult( ref tmp, out dorn );
			angle = dorn.getAngle();
			axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
			//check for axis length
			double len = axis.length2();
			if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
				axis = btVector3.xAxis;
			else
				axis.Div( btScalar.btSqrt( len ), out axis );
		}

		internal static void calculateDiffAxisAngle( ref btTransform transform0, ref btTransform transform1, out btVector3 axis, out double angle )
		{
			btMatrix3x3 tmp_m;
			transform0.m_basis.inverse( out tmp_m );
			btMatrix3x3 dmat;
			transform1.m_basis.Mult( ref tmp_m, out dmat );
			btQuaternion dorn;
			dmat.getRotation( out dorn );

			///floating point inaccuracy can lead to w component > 1..., which breaks 
			dorn.normalize();

			angle = dorn.getAngle();
			axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
			//check for axis length
			double len = axis.length2();
			if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
				axis = btVector3.xAxis;
			else
				axis.Div( btScalar.btSqrt( len ), out axis );
		}

//.........这里部分代码省略.........
						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;
					}

		/*@brief Return the angle between this and another vector
          @param v The other vector */
		public double angle( ref btVector3 v )
		{
			double s = btScalar.btSqrt( ( length2() * v.length2() ) );
#if PARANOID_ASSERTS
			Debug.Assert( s != (double)(0.0));
#endif
			return btScalar.btAcos( ( dot( ref v ) / s ) );
		}

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

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