C# BulletDynamics.RigidBody类代码示例

 public RaycastVehicle(VehicleTuning tuning, RigidBody chassis, IVehicleRaycaster raycaster)
 {
     m_vehicleRaycaster = raycaster;
     m_pitchControl = 0f;
     m_chassisBody = chassis;
     m_indexRightAxis = 0;
     m_indexUpAxis = 1;
     m_indexForwardAxis = 2;
     DefaultInit(ref tuning);
 }

		public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
			: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_frameInA = frameInA;
			m_frameInB = frameInB;
			m_useLinearReferenceFrameA = useLinearReferenceFrameA;
			m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
			m_linearLimits = new TranslationalLimitMotor();
			m_angularLimits[0] = new RotationalLimitMotor();
			m_angularLimits[1] = new RotationalLimitMotor();
			m_angularLimits[2] = new RotationalLimitMotor();
			CalculateTransforms();
		}

		//response  between two dynamic objects without friction, assuming 0 penetration depth
		public static float ResolveSingleCollision(
				RigidBody body1,
				CollisionObject colObj2,
				ref IndexedVector3 contactPositionWorld,
				ref IndexedVector3 contactNormalOnB,
				ContactSolverInfo solverInfo,
				float distance)
		{
			RigidBody body2 = RigidBody.Upcast(colObj2);


			IndexedVector3 normal = contactNormalOnB;

			IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
			IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;

			IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
			IndexedVector3 vel2 = body2 != null ? body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
			IndexedVector3 vel = vel1 - vel2;
			float rel_vel = normal.Dot(ref vel);

			float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
			float restitution = combinedRestitution * -rel_vel;

			float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
			float velocityError = -(1.0f + restitution) * rel_vel;// * damping;
			float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
			float denom1 = body2 != null ? body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
			float relaxation = 1.0f;
			float jacDiagABInv = relaxation / (denom0 + denom1);

			float penetrationImpulse = positionalError * jacDiagABInv;
			float velocityImpulse = velocityError * jacDiagABInv;

			float normalImpulse = penetrationImpulse + velocityImpulse;
			normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;

			body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
			if (body2 != null)
			{
				body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
			}

			return normalImpulse;
		}

        // constructor
    	// anchor, axis1 and axis2 are in world coordinate system
	    // axis1 must be orthogonal to axis2
        public Hinge2Constraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 anchor, ref IndexedVector3 axis1, ref IndexedVector3 axis2) : base(rbA,rbB,IndexedMatrix.Identity,IndexedMatrix.Identity,true)
        {
            m_anchor = anchor;
            m_axis1 = axis1;
            m_axis2 = axis2;
            // build frame basis
            // 6DOF constraint uses Euler angles and to define limits
            // it is assumed that rotational order is :
            // Z - first, allowed limits are (-PI,PI);
            // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
            // used to prevent constraint from instability on poles;
            // new position of X, allowed limits are (-PI,PI);
            // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
            // Build the frame in world coordinate system first
            IndexedVector3 zAxis = IndexedVector3.Normalize(axis1);
            IndexedVector3 xAxis = IndexedVector3.Normalize(axis2);
            IndexedVector3 yAxis = IndexedVector3.Cross(zAxis,xAxis); // we want right coordinate system

            IndexedMatrix frameInW = IndexedMatrix.Identity;
            frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
                                    xAxis.Y, yAxis.Y, zAxis.Y,
                                   xAxis.Z, yAxis.Z, zAxis.Z);
            frameInW._origin = anchor;

            // now get constraint frame in local coordinate systems
            m_frameInA = rbA.GetCenterOfMassTransform().Inverse() * frameInW;
            m_frameInB = rbB.GetCenterOfMassTransform().Inverse() * frameInW;
            // sei limits
            SetLinearLowerLimit(new IndexedVector3(0.0f, 0.0f, -1.0f));
            SetLinearUpperLimit(new IndexedVector3(0.0f, 0.0f, 1.0f));
            // like front wheels of a car
            SetAngularLowerLimit(new IndexedVector3(1.0f, 0.0f, -MathUtil.SIMD_HALF_PI * 0.5f));
            SetAngularUpperLimit(new IndexedVector3(-1.0f, 0.0f, MathUtil.SIMD_HALF_PI * 0.5f));
            // enable suspension
            EnableSpring(2, true);
            SetStiffness(2, MathUtil.SIMD_PI * MathUtil.SIMD_PI * 4.0f); // period 1 sec for 1 kilogramm weel :-)
            SetDamping(2, 0.01f);
            SetEquilibriumPoint();

        }

        // constructor
	    // anchor, axis1 and axis2 are in world coordinate system
	    // axis1 must be orthogonal to axis2
        public UniversalConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 anchor, ref IndexedVector3 axis1, ref IndexedVector3 axis2)
            : base(rbA, rbB, ref BulletGlobals.IdentityMatrix, ref BulletGlobals.IdentityMatrix, true)
        {
            m_anchor = anchor;
            m_axis1 = axis1;
            m_axis2 = axis2;
        
        
        	// build frame basis
	        // 6DOF constraint uses Euler angles and to define limits
	        // it is assumed that rotational order is :
	        // Z - first, allowed limits are (-PI,PI);
	        // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
	        // used to prevent constraint from instability on poles;
	        // new position of X, allowed limits are (-PI,PI);
	        // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
	        // Build the frame in world coordinate system first
	        IndexedVector3 zAxis = IndexedVector3.Normalize(m_axis1);
	        IndexedVector3 yAxis = IndexedVector3.Normalize(m_axis2);
	        IndexedVector3 xAxis = IndexedVector3.Cross(yAxis,zAxis); // we want right coordinate system
	        IndexedMatrix frameInW = IndexedMatrix.Identity;
            frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
                                    xAxis.Y, yAxis.Y, zAxis.Y,
                                    xAxis.Z, yAxis.Z, zAxis.Z);
            frameInW._origin = anchor;
	        // now get constraint frame in local coordinate systems
			//m_frameInA = MathUtil.inverseTimes(rbA.getCenterOfMassTransform(),frameInW);
			//m_frameInB = MathUtil.inverseTimes(rbB.getCenterOfMassTransform(),frameInW);
			m_frameInA = rbA.GetCenterOfMassTransform().Inverse() * frameInW;
			m_frameInB = rbB.GetCenterOfMassTransform().Inverse() * frameInW;

	        // sei limits
	        SetLinearLowerLimit(IndexedVector3.Zero);
	        SetLinearUpperLimit(IndexedVector3.Zero);
	        SetAngularLowerLimit(new IndexedVector3(0.0f, -MathUtil.SIMD_HALF_PI + UNIV_EPS, -MathUtil.SIMD_PI + UNIV_EPS));
	        SetAngularUpperLimit(new IndexedVector3(0.0f,  MathUtil.SIMD_HALF_PI - UNIV_EPS,  MathUtil.SIMD_PI - UNIV_EPS));
        }

		public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA)
			: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_angularOnly = false;
			m_enableAngularMotor = false;
			m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

			m_useReferenceFrameA = useReferenceFrameA;

			m_rbAFrame._origin = pivotInA;
#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#endif


			m_flags = 0;

			// since no frame is given, assume this to be zero angle and just pick rb transform axis
            IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0);

			IndexedVector3 rbAxisA2 = IndexedVector3.Zero;
			float projection = IndexedVector3.Dot(axisInA, rbAxisA1);
			if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
			{
                rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
			else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
			{
                rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
			else
			{
				rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1);
				rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA);
			}

            
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                    rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                    rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

			IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
			IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
			IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1);

			m_rbBFrame._origin = pivotInB;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                    rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                    rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

#if!	_BT_USE_CENTER_LIMIT_
	//start with free
	m_lowerLimit = float(1.0f);
	m_upperLimit = float(-1.0f);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
		}

		//------------------------    

		// constructors
		public SliderConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
			: base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_frameInA = frameInA;
			m_frameInB = frameInB;
			m_useLinearReferenceFrameA = useLinearReferenceFrameA;
			InitParams();

		}

		protected void ResolveSplitPenetrationImpulseCacheFriendly(
			RigidBody body1,
			RigidBody body2,
			ref SolverConstraint c)
		{
			if (c.m_rhsPenetration != 0f)
			{
				gNumSplitImpulseRecoveries++;
				float deltaImpulse = c.m_rhsPenetration - (c.m_appliedPushImpulse * c.m_cfm);
				float deltaVel1Dotn = IndexedVector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
				float deltaVel2Dotn = -IndexedVector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetTurnVelocity());

				deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
				deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
				float sum = c.m_appliedPushImpulse + deltaImpulse;
				if (sum < c.m_lowerLimit)
				{
					deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
					c.m_appliedPushImpulse = c.m_lowerLimit;
				}
				else
				{
					c.m_appliedPushImpulse = sum;
				}
				body1.InternalApplyPushImpulse(c.m_contactNormal * body1.InternalGetInvMass(), c.m_angularComponentA, deltaImpulse);
				body2.InternalApplyPushImpulse(-c.m_contactNormal * body2.InternalGetInvMass(), c.m_angularComponentB, deltaImpulse);
			}
		}

    	///internal method used by the constraint solver, don't use them directly
	    public virtual	void SolveConstraintObsolete(RigidBody bodyA,RigidBody bodyB,float timeStep) 
        {

        }

    public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
    {

        IndexedMatrix mat =
            IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
                                                                     pRawOrientation.Z, pRawOrientation.W));
        mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);

        CollisionShape shape = (pShape as BulletShapeXNA).shape;

        // TODO: Feed Update array into null
        RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
        body.SetWorldTransform(mat);
        body.SetUserPointer(pLocalID);
        return new BulletBodyXNA(pLocalID, body);
    }

		public override void RemoveRigidBody(RigidBody body)
		{
			base.RemoveCollisionObject(body);
		}

		public TypedConstraint(TypedConstraintType type, RigidBody rbA, RigidBody rbB)
			: base((int)type)
		{
			m_userConstraintType = -1;
			m_userConstraintId = -1;
			m_constraintType = type;
			m_rbA = rbA;
			m_rbB = rbB;
			m_appliedImpulse = 0f;
			m_breakingImpulseThreshold = MathUtil.SIMD_INFINITY;
			m_isEnabled = true;
			m_dbgDrawSize = DEFAULT_DEBUGDRAW_SIZE;
			{
				GetFixedBody().SetMassProps(0f, IndexedVector3.Zero);
			}

		}

		public override void AddRigidBody(RigidBody body)
		{
			body.SetGravity(ref m_gravity);

			if (body.GetCollisionShape() != null)
			{
				AddCollisionObject(body);
			}

		}

		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
								ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
								CollisionObject colObj0, CollisionObject colObj1, float relaxation,
								float desiredVelocity, float cfmSlip)
		{
			RigidBody body0 = RigidBody.Upcast(colObj0);
			RigidBody body1 = RigidBody.Upcast(colObj1);

			solverConstraint.m_contactNormal = normalAxis;

			solverConstraint.m_solverBodyA = body0 != null ? body0 : GetFixedBody();
			solverConstraint.m_solverBodyB = body1 != null ? body1 : GetFixedBody();

			solverConstraint.m_friction = cp.GetCombinedFriction();
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && (body0 != null  || body1 != null) && BulletGlobals.debugSolver)
            {
                BulletGlobals.g_streamWriter.WriteLine("SetupFrictionConstraint[{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
            }
#endif

			solverConstraint.m_originalContactPoint = null;
            //solverConstraint.m_originalContactPointConstraint = null;
			solverConstraint.m_appliedImpulse = 0f;
			solverConstraint.m_appliedPushImpulse = 0f;




			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
				solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentA = body0 != null ? body0.GetInvInertiaTensorWorld() * ftorqueAxis1 * body0.GetAngularFactor() : IndexedVector3.Zero;
            }
			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
				solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentB = body1 != null ? body1.GetInvInertiaTensorWorld() * ftorqueAxis1 * body1.GetAngularFactor() : IndexedVector3.Zero;
            }

#if COMPUTE_IMPULSE_DENOM
	        float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
	        float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
			IndexedVector3 vec;
			float denom0 = 0f;
			float denom1 = 0f;
			if (body0 != null)
			{
				vec = IndexedVector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
				denom0 = body0.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}
			if (body1 != null)
			{
				vec = IndexedVector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
				denom1 = body1.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}


#endif //COMPUTE_IMPULSE_DENOM
			float denom = relaxation / (denom0 + denom1);
			solverConstraint.m_jacDiagABInv = denom;
			MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
#if _USE_JACOBIAN
	        solverConstraint.m_jac =  new JacobianEntry (
		        ref rel_pos1,ref rel_pos2,ref solverConstraint.m_contactNormal,
		        body0.getInvInertiaDiagLocal(),
		        body0.getInvMass(),
		        body1.getInvInertiaDiagLocal(),
		        body1.getInvMass());
#endif //_USE_JACOBIAN


			{
				float rel_vel;
				float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : IndexedVector3.Zero);
				float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : IndexedVector3.Zero);

				rel_vel = vel1Dotn + vel2Dotn;

				//float positionalError = 0f;

				float velocityError = desiredVelocity - rel_vel;
				float damper = 1f;
				float velocityImpulse = (velocityError * solverConstraint.m_jacDiagABInv) * damper;
				solverConstraint.m_rhs = velocityImpulse;
				solverConstraint.m_cfm = cfmSlip;
				solverConstraint.m_lowerLimit = 0;
				solverConstraint.m_upperLimit = 1e10f;
			}
		}

		public static RigidBody GetFixedBody()
		{
			if (s_fixed == null)
			{
				s_fixed = new RigidBody(0f, null, null, IndexedVector3.Zero);
                s_fixed.SetUserPointer("SICS:Fixed");
			}
			s_fixed.SetMassProps(0f, IndexedVector3.Zero);
			return s_fixed;
		}