C# btVector3.normalized方法代码示例

		btStaticPlaneShape( ref btVector3 planeOrigin, ref btVector3 planeNormal ) : base()
		{
			planeNormal.normalized( out m_planeNormal );
			m_planeConstant = planeOrigin.dot( ref planeNormal );
			m_localScaling = btVector3.Zero;
			m_shapeType = BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE;
			//	Debug.Assert( btFuzzyZero(m_planeNormal.length() - btScalar.BT_ONE) );
		}

		void setAxis( ref btVector3 axis1, ref btVector3 axis2 )
		{
			btVector3 zAxis; axis1.normalized( out zAxis );
			btVector3 yAxis; axis2.normalized( out yAxis );
			btVector3 xAxis; yAxis.cross( ref zAxis, out xAxis ); // we want right coordinate system

			btTransform frameInW = btTransform.Identity;
			frameInW.m_basis.setValue( ref xAxis, ref yAxis, ref zAxis );

			// now get constraint frame in local coordinate systems
			btTransform tmp;
			m_rbA.m_worldTransform.inverse( out tmp );

			tmp.Apply( ref frameInW, out m_frameInA );
			m_rbB.m_worldTransform.inverse( out tmp );
			tmp.Apply( ref frameInW, out m_frameInB );

			calculateTransforms();
		}

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

		void setAxis( ref btVector3 axis1, ref btVector3 axis2 )
		{
			btVector3 zAxis; axis1.normalized( out zAxis );
			btVector3 yAxis; axis2.normalized( out yAxis );
			btVector3 xAxis; yAxis.cross( ref zAxis, out xAxis ); // we want right coordinate system

			btTransform frameInW = btTransform.Identity;
			//frameInW.setIdentity();
			frameInW.m_basis.setValue( xAxis[0], yAxis[0], zAxis[0],
											xAxis[1], yAxis[1], zAxis[1],
										   xAxis[2], yAxis[2], zAxis[2] );

			// now get constraint frame in local coordinate systems
			btTransform inv;
			m_rbA.m_worldTransform.inverse( out inv );
			inv.Apply( ref frameInW, out m_frameInA );
			m_rbB.m_worldTransform.inverse( out inv );
			inv.Apply( ref frameInW, out m_frameInB );
			//m_frameInB = m_rbB.m_worldTransform.inverse() * frameInW;

			calculateTransforms();
		}

		public void shortestArcQuatNormalize2( ref btVector3 v0, ref btVector3 v1, out btQuaternion result )
		{
			btVector3 _v0;
			btVector3 _v1;
			v0.normalized( out _v0 );
			v1.normalized( out _v1 );
			shortestArcQuat( ref _v0, ref _v1, out result );
		}

			/* Internals	*/
			internal void getsupport( ref btVector3 d, sSV sv )
			{
				d.normalized( out sv.d );
				//sv.d = d / d.length();
				m_shape.Support( ref sv.d, out sv.w );
				//sv.w = ;
			}

			int4 FindSimplex( btVector3[] verts, int verts_count, int[] allow)
			{
				btVector3[] basis = new btVector3[3];
				basis[0] = new btVector3( 0.01, 0.02, 1.0 );
				int p0 = maxdirsterid( verts, verts_count, ref basis[0], allow );
				btVector3 tmp; basis[0].Invert( out tmp );
				int p1 = maxdirsterid( verts, verts_count, ref tmp, allow );
				verts[p0].Sub( ref  verts[p1], out basis[0] );
				//basis[0] = verts[p0] - verts[p1];
				if( p0 == p1 || basis[0].isZero() )
					return new int4( -1, -1, -1, -1 );
				tmp = new btVector3( (double)( 1 ), 0.02, (double)( 0 ) );
                btVector3.btCross( ref tmp, ref basis[0], out basis[1] );
				tmp = new btVector3( (double)( -0.02 ), (double)( 1 ), (double)( 0 ) );
                btVector3.btCross( ref tmp, ref basis[0], out basis[2] );
				if( basis[1].length() > basis[2].length() )
				{
					basis[1].normalize();
				}
				else
				{
					basis[1] = basis[2];
					basis[1].normalize();
				}
				int p2 = maxdirsterid( verts, verts_count, ref basis[1], allow );
				if( p2 == p0 || p2 == p1 )
				{
					basis[1].Invert( out tmp );
					p2 = maxdirsterid( verts, verts_count, ref tmp, allow );
				}
				if( p2 == p0 || p2 == p1 )
					return new int4( -1, -1, -1, -1 );
				verts[p2].Sub( ref verts[p0], out basis[1] );
				btVector3.btCross( ref basis[1], ref basis[0], out tmp );
                tmp.normalized( out basis[2] );
				int p3 = maxdirsterid( verts, verts_count, ref basis[2], allow );
				basis[2].Invert( out tmp );
				if( p3 == p0 || p3 == p1 || p3 == p2 ) p3 = maxdirsterid( verts, verts_count, ref tmp, allow );
				if( p3 == p0 || p3 == p1 || p3 == p2 )
					return new int4( -1, -1, -1, -1 );
				Debug.Assert( !( p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3 ) );
				btVector3 tmp2;
				verts[p1].Sub( ref verts[p0], out tmp );
				verts[p2].Sub( ref verts[p0], out tmp2 );
				btVector3 tmp3;
				btVector3.btCross( ref tmp, ref tmp2, out tmp3 );
				verts[p3].Sub( ref verts[p0], out tmp );
                if( btVector3.btDot( ref tmp, ref tmp3  ) < 0 ) { btScalar.btSwap( ref p2, ref p3 ); }
				return new int4( p0, p1, p2, p3 );
			}