C# btVector3.cross方法代码示例

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

		public virtual void drawArc( ref btVector3 center, ref btVector3 normal, ref btVector3 axis
			, double radiusA, double radiusB, double minAngle, double maxAngle,
					ref btVector3 color, bool drawSect, double stepDegrees = btScalar.BT_TEN )
		{
			btVector3 vx = axis;
			btVector3 vy; normal.cross( ref axis, out vy );
			double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG;
			int nSteps = (int)btScalar.btFabs( ( maxAngle - minAngle ) / step );
			if( nSteps == 0 ) nSteps = 1;
			btVector3 prev;
			center.AddScale( ref vx, radiusA * btScalar.btCos( minAngle ), out prev );
			prev.AddScale( ref vy, radiusB * btScalar.btSin( minAngle ), out prev );
			if( drawSect )
			{
				drawLine( ref center, ref prev, ref color );
			}
			for( int i = 1; i <= nSteps; i++ )
			{
				double angle = minAngle + ( maxAngle - minAngle ) * i / (double)nSteps;
				btVector3 next;
				center.AddScale( ref vx, radiusA * btScalar.btCos( angle ), out next );
				next.AddScale( ref vy, radiusB * btScalar.btSin( angle ), out next );
				drawLine( ref prev, ref next, ref color );
				prev = next;
			}
			if( drawSect )
			{
				drawLine( ref center, ref prev, ref color );
			}
		}

		public btHingeConstraint( btRigidBody rbA, btRigidBody rbB, ref btVector3 pivotInA, ref btVector3 pivotInB,
												  ref btVector3 axisInA, ref btVector3 axisInB, bool useReferenceFrameA = false )
										 : base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA, rbB )
		{
			Init();
#if _BT_USE_CENTER_LIMIT_
			m_limit = new btAngularLimit();
#endif
			m_useReferenceFrameA = ( useReferenceFrameA );
			m_rbAFrame.m_origin = pivotInA;

			// since no frame is given, assume this to be zero angle and just pick rb transform axis
			btVector3 rbAxisA1; rbA.m_worldTransform.m_basis.getColumn( 0, out rbAxisA1 );

			btVector3 rbAxisA2;
			double projection = axisInA.dot( rbAxisA1 );
			if( projection >= 1.0f - btScalar.SIMD_EPSILON )
			{
				btVector3 tmp;
				rbA.m_worldTransform.m_basis.getColumn( 2, out tmp );
				tmp.Invert( out rbAxisA1 );
				rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
			}
			else if( projection <= -1.0f + btScalar.SIMD_EPSILON )
			{
				rbA.m_worldTransform.m_basis.getColumn( 2, out rbAxisA1 );
				rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
			}
			else
			{
				axisInA.cross( ref rbAxisA1, out rbAxisA2 );
				rbAxisA2.cross( ref axisInA, out rbAxisA1 );
			}

			btMatrix3x3.setValue( out m_rbAFrame.m_basis, rbAxisA1.x, rbAxisA2.x, axisInA.x,
											rbAxisA1.y, rbAxisA2.y, axisInA.y,
											rbAxisA1.z, rbAxisA2.z, axisInA.z );

			btQuaternion rotationArc; btQuaternion.shortestArcQuat( ref axisInA, ref axisInB, out rotationArc );
			btVector3 rbAxisB1; btQuaternion.quatRotate( ref rotationArc, ref rbAxisA1, out rbAxisB1 );
			btVector3 rbAxisB2; axisInB.cross( ref rbAxisB1, out rbAxisB2 );

			m_rbBFrame.m_origin = pivotInB;
			m_rbBFrame.m_basis.setValue( ref rbAxisB1, ref rbAxisB2, ref axisInB );
			btMatrix3x3.setValue( out m_rbBFrame.m_basis, ref rbAxisB1, ref rbAxisB2, ref axisInB );

			m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
		}

		public static void shortestArcQuat( ref btVector3 v0, ref btVector3 v1, out btQuaternion result ) // Game Programming Gems 2.10 make sure v0,v1 are normalized
		{
			btVector3 c;
			v0.cross( ref v1, out c );
			double d = v0.dot( ref v1 );

			if( d < -1.0 + btScalar.SIMD_EPSILON )
			{
				btVector3 n, unused;
				btVector3.btPlaneSpace1( ref v0, out n, out unused );
				result.x = n.x; result.y = n.y; result.z = n.z; result.w = 0.0f; // just pick any vector that is orthogonal to v0
				return;
			}

			double s = btScalar.btSqrt( ( 1.0f + d ) * 2.0f );
			double rs = 1.0f / s;
			result.x = c.x * rs;
			result.y = c.y * rs;
			result.z = c.z * rs;
			result.w = s * 0.5f;
		}

		internal void setupContactConstraint( btSolverConstraint solverConstraint,
											btSolverBody bodyA, btSolverBody bodyB,
											btManifoldPoint cp, btContactSolverInfo infoGlobal,
											out double relaxation,
											ref btVector3 rel_pos1, ref btVector3 rel_pos2 )
		{

			//	ref btVector3 pos1 = cp.getPositionWorldOnA();
			//	ref btVector3 pos2 = cp.getPositionWorldOnB();

			btRigidBody rb0 = bodyA.m_originalBody;
			btRigidBody rb1 = bodyB.m_originalBody;

			//			btVector3 rel_pos1 = pos1 - colObj0.getWorldTransform().getOrigin();
			//			btVector3 rel_pos2 = pos2 - colObj1.getWorldTransform().getOrigin();
			//rel_pos1 = pos1 - bodyA.getWorldTransform().getOrigin();
			//rel_pos2 = pos2 - bodyB.getWorldTransform().getOrigin();

			relaxation = 1;

			btVector3 torqueAxis0; rel_pos1.cross( ref cp.m_normalWorldOnB, out torqueAxis0 );
			btVector3 tmp;
			//solverConstraint.m_angularComponentA = rb0 != null ? rb0.m_invInertiaTensorWorld * torqueAxis0 * rb0.getAngularFactor() : btVector3.Zero;
			if( rb0 != null )
			{
				rb0.m_invInertiaTensorWorld.Apply( ref torqueAxis0, out tmp );
				tmp.Mult( ref rb0.m_angularFactor, out solverConstraint.m_angularComponentA );
			}
			else
				solverConstraint.m_angularComponentA = btVector3.Zero;

			btVector3 torqueAxis1; rel_pos2.cross( ref cp.m_normalWorldOnB, out torqueAxis1 );
			torqueAxis1.Invert( out torqueAxis1 );
			//solverConstraint.m_angularComponentB = rb1 != null ? rb1.m_invInertiaTensorWorld * -torqueAxis1 * rb1.getAngularFactor() : btVector3.Zero;
			if( rb1 != null )
			{
				rb1.m_invInertiaTensorWorld.Apply( ref torqueAxis1, out tmp );
				tmp.Mult( ref rb1.m_angularFactor, out solverConstraint.m_angularComponentB );
			}
			else
				solverConstraint.m_angularComponentB = btVector3.Zero;

			{
#if COMPUTE_IMPULSE_DENOM
					double denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
					double denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
				btVector3 vec;
				double denom0 = 0;
				double denom1 = 0;
				if( rb0 != null )
				{
					( solverConstraint.m_angularComponentA ).cross( ref rel_pos1, out vec );
					denom0 = rb0.getInvMass() + cp.m_normalWorldOnB.dot( vec );
				}
				if( rb1 != null )
				{
					solverConstraint.m_angularComponentB.Invert( out tmp );
					tmp.cross( ref rel_pos2, out vec );
					denom1 = rb1.getInvMass() + cp.m_normalWorldOnB.dot( vec );
				}
#endif //COMPUTE_IMPULSE_DENOM

				double denom = relaxation / ( denom0 + denom1 );
				btScalar.Dbg( "m_jacDiagABInv 3 set to " + denom.ToString( "g17" ) );
				solverConstraint.m_jacDiagABInv = denom;
			}

			if( rb0 != null )
			{
				solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
				solverConstraint.m_relpos1CrossNormal = torqueAxis0;
				btScalar.Dbg( "Torque Axis to relpos1 " + torqueAxis0 );
			}
			else
			{
				solverConstraint.m_contactNormal1 = btVector3.Zero;
				solverConstraint.m_relpos1CrossNormal = btVector3.Zero;
			}
			if( rb1 != null )
			{
				cp.m_normalWorldOnB.Invert( out solverConstraint.m_contactNormal2 );
				solverConstraint.m_relpos2CrossNormal = torqueAxis1;
				btScalar.Dbg( "Torque Axis to relpos2 " + torqueAxis1 );
			}
			else
			{
				solverConstraint.m_contactNormal2 = btVector3.Zero;
				solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
			}

			double restitution = 0;
			double penetration = cp.getDistance() + infoGlobal.m_linearSlop;

			{
				btVector3 vel1, vel2;

				vel1 = rb0 != null ? rb0.getVelocityInLocalPoint( ref rel_pos1 ) : btVector3.Zero;
				vel2 = rb1 != null ? rb1.getVelocityInLocalPoint( ref rel_pos2 ) : btVector3.Zero;

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

		/// Solve A * x = b, where b is a column vector. This is more efficient
		/// than computing the inverse in one-shot cases.
		///Solve33 is from Box2d, thanks to Erin Catto,
		public void solve33( ref btVector3 b, out btVector3 result )
		{
			btVector3 col1 = getColumn( 0 );
			btVector3 col2 = getColumn( 1 );
			btVector3 col3 = getColumn( 2 );
			btVector3 tmp;
			col2.cross( ref col3, out tmp );
			double det = col1.dot( ref tmp );
			if( btScalar.btFabs( det ) > btScalar.SIMD_EPSILON )
			{
				det = 1.0f / det;
			}
			col2.cross( ref col3, out tmp );
			result.x = det * b.dot( ref tmp );
			b.cross( ref col3, out tmp );
			result.y = det * col1.dot( ref tmp );
			col2.cross( ref b, out tmp );
			result.z = det * col1.dot( ref tmp );
			result.w = 0;
		}

		internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis
			//, int solverBodyIdA, int solverBodyIdB
			, btSolverBody solverBodyA, btSolverBody solverBodyB
			, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 )
		{
			//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
			//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];

			btRigidBody body0 = solverBodyA.m_originalBody;
			btRigidBody body1 = solverBodyB.m_originalBody;

			solverConstraint.m_solverBodyA = solverBodyA;
			solverConstraint.m_solverBodyB = solverBodyB;

			solverConstraint.m_friction = cp.m_combinedFriction;
			solverConstraint.m_originalContactPoint = null;

			solverConstraint.m_appliedImpulse = 0;
			solverConstraint.m_appliedPushImpulse = 0;

			if( body0 != null )
			{
				solverConstraint.m_contactNormal1 = normalAxis;
				rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal );
				btVector3 tmp;
				body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
				tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
			}
			else
			{
				solverConstraint.m_contactNormal1.setZero();
				solverConstraint.m_relpos1CrossNormal.setZero();
				solverConstraint.m_angularComponentA.setZero();
			}

			if( body1 != null )
			{
				normalAxis.Invert( out solverConstraint.m_contactNormal2 );
				rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal );
				btVector3 tmp;
				body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
				tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
			}
			else
			{
				solverConstraint.m_contactNormal2 = btVector3.Zero;
				solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
				solverConstraint.m_angularComponentB = btVector3.Zero;
			}

			{
				btVector3 vec;
				double denom0 = 0;
				double denom1 = 0;
				if( body0 != null )
				{
					solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec );
					denom0 = body0.getInvMass() + normalAxis.dot( ref vec );
				}
				if( body1 != null )
				{
					btVector3 tmp;
					solverConstraint.m_angularComponentB.Invert( out tmp );
					tmp.cross( ref rel_pos2, out vec );
					denom1 = body1.getInvMass() + normalAxis.dot( ref vec );
				}
				double denom = relaxation / ( denom0 + denom1 );
				btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) );
				solverConstraint.m_jacDiagABInv = denom;
			}

			{


				double rel_vel;
				double vel1Dotn;
				double vel2Dotn;
				//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
				//	+ solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
				if( body0 != null )
					vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
						+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
				else
					vel1Dotn = 0;

				//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
				//	+ solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
				if( body1 != null )
					vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
						+ solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity );
				else
					vel2Dotn = 0;


				rel_vel = vel1Dotn + vel2Dotn;

				//		double positionalError = 0;

				double velocityError = desiredVelocity - rel_vel;
				double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
//.........这里部分代码省略.........

		static void orth( ref btVector3 v, out btVector3 result )
		{
			btVector3 a; v.cross( ref btVector3.Forward, out a );
			btVector3 b; v.cross( ref btVector3.yAxis, out b );
			if( a.length() > b.length() )
			{
				a.normalized( out result );
			}
			else
			{
				b.normalized( out result );
			}
		}

		public static double DistanceBetweenLines( ref btVector3 ustart, ref btVector3 udir
			, ref btVector3 vstart, ref btVector3 vdir
			, out btVector3 upoint, out btVector3 vpoint )
		{
			btVector3 cp;
			udir.cross( ref vdir, out cp );
			cp.normalize();

			double distu = -cp.dot( ref ustart );
			double distv = -cp.dot( ref vstart );
			double dist = btScalar.btFabs( distu - distv );
			btPlane plane;
			btVector3 tmp;
			//if( upoint )
			{
				vdir.cross( ref cp, out plane.normal );
				plane.normal.normalize();
				plane.dist = -plane.normal.dot( ref vstart );
				ustart.Add( ref udir, out tmp );
				plane.PlaneLineIntersection( ref ustart, ref tmp, out upoint );
			}
			//if( vpoint )
			{
				udir.cross( ref cp, out plane.normal );
				plane.normal.normalize();
				plane.dist = -plane.normal.dot( ref ustart );
				ustart.Add( ref vdir, out tmp );
				plane.PlaneLineIntersection( ref vstart, ref tmp, out vpoint );
			}
			return dist;
		}

		/*@brief Return the cross product of two vectors */
		public static void btCross( ref btVector3 v1, ref btVector3 v2, out btVector3 result )
		{
			v1.cross( ref v2, out result );
		}

		/*@brief Return a rotated version of this vector
          @param wAxis The axis to rotate about 
          @param angle The angle to rotate by */
		public void rotate( ref btVector3 wAxis, double angle, out btVector3 result )
		{
			btVector3 o;
			wAxis.Mult( wAxis.dot( ref this ), out o );
			btVector3 _x;
			this.Sub( ref o, out _x );
			btVector3 _y;

			wAxis.cross( ref this, out _y );
			btVector3 tmp;
			btVector3 tmp2;
			_x.Mult( btScalar.btCos( angle ), out tmp );
			o.Add( ref tmp, out tmp2 );
			_y.Mult( btScalar.btSin( angle ), out tmp );
			tmp2.Add( ref tmp, out result );
		}