C# btVector3.normalize方法代码示例

		public void calcNormal( out btVector3 normal )
		{
			btVector3 tmp;
			btVector3 tmp2;
			m_vertices2.Sub( ref m_vertices1, out tmp );
            m_vertices3.Sub( ref m_vertices1, out tmp2 );
            tmp.cross( ref tmp2, out normal );
			normal.normalize();
		}

		bool collide( ref btVector3 sphereCenter, out btVector3 point, out btVector3 resultNormal, out double depth, double timeOfImpact, double contactBreakingThreshold )
		{

			//btVector3[] vertices = m_triangle.getVertexPtr( 0 );

			double radius = m_sphere.getRadius();
			double radiusWithThreshold = radius + contactBreakingThreshold;


			btVector3 normal;
			btVector3.btCross2Del( ref m_triangle.m_vertices2, ref m_triangle.m_vertices1
				, ref m_triangle.m_vertices3, ref m_triangle.m_vertices1
				, out normal );
			normal.normalize();

			btVector3 p1ToCentre; sphereCenter.Sub( ref m_triangle.m_vertices1, out p1ToCentre );
			double distanceFromPlane = p1ToCentre.dot( ref normal );

			if( distanceFromPlane < btScalar.BT_ZERO )
			{
				//triangle facing the other way
				distanceFromPlane *= btScalar.BT_NEG_ONE;
				normal *= btScalar.BT_NEG_ONE;
			}

			bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;

			// Check for contact / intersection
			bool hasContact = false;
			btVector3 contactPoint = btVector3.Zero;
			if( isInsideContactPlane )
			{
				if( pointInTriangle(
					ref m_triangle.m_vertices1
					, ref m_triangle.m_vertices2
					, ref m_triangle.m_vertices3
					, ref normal
					, ref sphereCenter
					) )
				{
					// Inside the contact wedge - touches a point on the shell plane
					hasContact = true;
					contactPoint = sphereCenter - normal * distanceFromPlane;
				}
				else
				{
					// Could be inside one of the contact capsules
					double contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
					btVector3 nearestOnEdge;
					for( int i = 0; i < m_triangle.getNumEdges(); i++ )
					{

						btVector3 pa;
						btVector3 pb;

						m_triangle.getEdge( i, out pa, out pb );

						double distanceSqr = SegmentSqrDistance( ref pa, ref pb, ref sphereCenter, out nearestOnEdge );
						if( distanceSqr < contactCapsuleRadiusSqr )
						{
							// Yep, we're inside a capsule
							hasContact = true;
							contactPoint = nearestOnEdge;
						}

					}
				}
			}

			if( hasContact )
			{
				btVector3 contactToCentre; sphereCenter.Sub( ref contactPoint, out contactToCentre );
				double distanceSqr = contactToCentre.length2();

				if( distanceSqr < radiusWithThreshold * radiusWithThreshold )
				{
					if( distanceSqr > btScalar.SIMD_EPSILON )
					{
						double distance = btScalar.btSqrt( distanceSqr );
						resultNormal = contactToCentre;
						resultNormal.normalize();
						point = contactPoint;
						depth = -( radius - distance );
					}
					else
					{
						resultNormal = normal;
						point = contactPoint;
						depth = -radius;
					}
					return true;
				}
			}
			depth = 0;
			point = btVector3.Zero;
			resultNormal = btVector3.Zero;
			return false;
		}

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

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

		void adjustSwingAxisToUseEllipseNormal( ref btVector3 vSwingAxis )
		{
			// the swing axis is computed as the "twist-free" cone rotation,
			// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
			// so, if we're outside the limits, the closest way back inside the cone isn't 
			// along the vector back to the center. better (and more stable) to use the ellipse normal.

			// convert swing axis to direction from center to surface of ellipse
			// (ie. rotate 2D vector by PI/2)
			double y = -vSwingAxis.z;
			double z = vSwingAxis.y;

			// do the math...
			if( Math.Abs( z ) > btScalar.SIMD_EPSILON ) // avoid division by 0. and we don't need an update if z == 0.
			{
				// compute gradient/normal of ellipse surface at current "point"
				double grad = y / z;
				grad *= m_swingSpan2 / m_swingSpan1;

				// adjust y/z to represent normal at point (instead of vector to point)
				if( y > 0 )
					y = Math.Abs( grad * z );
				else
					y = -Math.Abs( grad * z );

				// convert ellipse direction back to swing axis
				vSwingAxis.z = ( -y );
				vSwingAxis.y = ( z );
				vSwingAxis.normalize();
			}
		}

		// given a twist rotation in constraint space, (pre: cone must already be removed)
		// this method computes its corresponding angle and axis.
		void computeTwistLimitInfo( ref btQuaternion qTwist,
														  out double twistAngle, // out
														  out btVector3 vTwistAxis ) // out
		{
			btQuaternion qMinTwist = qTwist;
			twistAngle = qTwist.getAngle();

			if( twistAngle > btScalar.SIMD_PI ) // long way around. flip quat and recalculate.
			{
				qTwist.inverse( out qMinTwist );
				twistAngle = qMinTwist.getAngle();
			}
			if( twistAngle < 0 )
			{
				// this should never happen
#if false
        Debug.Assert(false);
#endif
			}

			vTwistAxis = new btVector3( qMinTwist.x, qMinTwist.y, qMinTwist.z );
			if( twistAngle > btScalar.SIMD_EPSILON )
				vTwistAxis.normalize();
		}

		// given a cone rotation in constraint space, (pre: twist must already be removed)
		// this method computes its corresponding swing angle and axis.
		// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
		void computeConeLimitInfo( ref btQuaternion qCone,
														 out double swingAngle, // out
														 out btVector3 vSwingAxis, // out
														 out double swingLimit ) // out
		{
			swingAngle = qCone.getAngle();
			if( swingAngle > btScalar.SIMD_EPSILON )
			{
				vSwingAxis = new btVector3( qCone.x, qCone.y, qCone.z );
				vSwingAxis.normalize();
#if false
        // non-zero twist?! this should never happen.
       Debug.Assert(Math.Abs(vSwingAxis.x) <= btScalar.SIMD_EPSILON));
#endif

				// Compute limit for given swing. tricky:
				// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
				// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)

				// For starters, compute the direction from center to surface of ellipse.
				// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
				// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
				double xEllipse = vSwingAxis.y;
				double yEllipse = -vSwingAxis.z;

				// Now, we use the slope of the vector (using x/yEllipse) and find the length
				// of the line that intersects the ellipse:
				//  x^2   y^2
				//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
				//  a^2   b^2
				// Do the math and it should be clear.

				swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
				if( Math.Abs( xEllipse ) > btScalar.SIMD_EPSILON )
				{
					double surfaceSlope2 = ( yEllipse * yEllipse ) / ( xEllipse * xEllipse );
					double norm = 1 / ( m_swingSpan2 * m_swingSpan2 );
					norm += surfaceSlope2 / ( m_swingSpan1 * m_swingSpan1 );
					double swingLimit2 = ( 1 + surfaceSlope2 ) / norm;
					swingLimit = btScalar.btSqrt( swingLimit2 );
				}
				// test!
				/*swingLimit = m_swingSpan2;
				if (Math.Abs(vSwingAxis.z) > btScalar.SIMD_EPSILON)
				{
				double mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
				double sinphi = m_swingSpan2 / sqrt(mag_2);
				double phi = asin(sinphi);
				double theta = atan2(Math.Abs(vSwingAxis.y),Math.Abs(vSwingAxis.z));
				double alpha = 3.14159f - theta - phi;
				double sinalpha = sin(alpha);
				swingLimit = m_swingSpan1 * sinphi/sinalpha;
				}*/
			}
			else //if( swingAngle < 0 )
			{
				vSwingAxis = btVector3.xAxis;
				swingLimit = 0;
				// this should never happen!
#if false
        Debug.Assert(false);
#endif
			}
		}