C# RigidBody.InternalApplyImpulse方法代码示例

		protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
		{
            m_lowerLimitCount++;
			//check magniture of applied impulse from SolverConstraint 
			float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;

            if (deltaImpulse > 200)
            {
                int ibreak = 0;
            }

            float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
            (c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);

            float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
            (c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);

			deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
			deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;

			float sum = c.m_appliedImpulse + deltaImpulse;

			if (sum < c.m_lowerLimit)
			{
				deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
				c.m_appliedImpulse = c.m_lowerLimit;
			}
			else
			{
				c.m_appliedImpulse = sum;
			}

            IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X,c.m_contactNormal.Y * body1.m_invMass.Y,c.m_contactNormal.Z * body1.m_invMass.Z);
            body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body1");

            temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
            body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body2");

		}

		protected void ResolveSingleConstraintRowGeneric(RigidBody body1, RigidBody body2, ref SolverConstraint c)
		{
            m_genericCount++;
                float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;

                float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
                (c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);

                float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
                (c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);


                float originalDeltaImpulse = deltaImpulse;

                deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
                deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;

#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
                {
                    BulletGlobals.g_streamWriter.WriteLine("ResolveSingleConstraintRowGeneric start [{0}][{1}][{2}][{3}].", originalDeltaImpulse, deltaVel1Dotn, deltaVel2Dotn, c.m_jacDiagABInv);
                }
#endif
                float sum = c.m_appliedImpulse + deltaImpulse;
                if (sum < c.m_lowerLimit)
                {
                    deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
                    c.m_appliedImpulse = c.m_lowerLimit;
                }
                else if (sum > c.m_upperLimit)
                {
                    deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
                    c.m_appliedImpulse = c.m_upperLimit;
                }
                else
                {
                    c.m_appliedImpulse = sum;
                }

                IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X, c.m_contactNormal.Y * body1.m_invMass.Y, c.m_contactNormal.Z * body1.m_invMass.Z);

                body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintGeneric-body1");

                temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);

                body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintGeneric-body2");
		}

		protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1,
										 ManifoldPoint cp, ContactSolverInfo infoGlobal)
		{
			if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING))
			{
				{
					SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
					if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
					{
						frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
						if (rb0 != null)
						{
                            rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse,"SetupFriction-rb0");
						}
						if (rb1 != null)
						{
                            rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse, "SetupFriction-rb1");
						}
					}
					else
					{
						frictionConstraint1.m_appliedImpulse = 0f;
					}
					m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;

				}

				if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
				{
					SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
					if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
					{
						frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
						if (rb0 != null)
						{
							rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse,"SetFriction-rb0");
						}
						if (rb1 != null)
						{
							rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse,"SetFriction-rb1");
						}
					}
					else
					{
						frictionConstraint2.m_appliedImpulse = 0f;
					}
					m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
				}
			}
			else
			{
				SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
				frictionConstraint1.m_appliedImpulse = 0f;
				if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
				{
					SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
					frictionConstraint2.m_appliedImpulse = 0f;
					m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
				}
				m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;

			}
		}

		//! apply the correction impulses for two bodies
		public float SolveAngularLimits(float timeStep, ref IndexedVector3 axis, float jacDiagABInv, RigidBody body0, RigidBody body1)
		{
			if (NeedApplyTorques() == false)
			{
				return 0.0f;
			}

			float target_velocity = m_targetVelocity;
			float maxMotorForce = m_maxMotorForce;

			//current error correction
			if (m_currentLimit != 0)
			{
				target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
				maxMotorForce = m_maxLimitForce;
			}

			maxMotorForce *= timeStep;

			// current velocity difference

			IndexedVector3 angVelA = IndexedVector3.Zero;
			body0.InternalGetAngularVelocity(ref angVelA);
			IndexedVector3 angVelB = IndexedVector3.Zero;
			body1.InternalGetAngularVelocity(ref angVelB);

			IndexedVector3 vel_diff = angVelA - angVelB;

			float rel_vel = IndexedVector3.Dot(axis, vel_diff);

			// correction velocity
			float motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);

			if (motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON)
			{
				return 0.0f;//no need for applying force
			}


			// correction impulse
			float unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;

			// clip correction impulse
			float clippedMotorImpulse;

			///@todo: should clip against accumulated impulse
			if (unclippedMotorImpulse > 0.0f)
			{
				clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
			}
			else
			{
				clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
			}


			// sort with accumulated impulses
			float lo = float.MinValue;
			float hi = float.MaxValue;

			float oldaccumImpulse = m_accumulatedImpulse;
			float sum = oldaccumImpulse + clippedMotorImpulse;
			m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;

			clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;

			IndexedVector3 motorImp = clippedMotorImpulse * axis;

			//body0.applyTorqueImpulse(motorImp);
			//body1.applyTorqueImpulse(-motorImp);

            body0.InternalApplyImpulse(IndexedVector3.Zero, body0.GetInvInertiaTensorWorld() * axis, clippedMotorImpulse, "Generic6DoF body0");
            body1.InternalApplyImpulse(IndexedVector3.Zero, body1.GetInvInertiaTensorWorld() * axis, -clippedMotorImpulse, "Generic6DoF body1");

			return clippedMotorImpulse;
		}

		public float SolveLinearAxis(
			float timeStep,
			float jacDiagABInv,
			RigidBody body1, ref IndexedVector3 pointInA,
			RigidBody body2, ref IndexedVector3 pointInB,
			int limit_index,
			ref IndexedVector3 axis_normal_on_a,
			ref IndexedVector3 anchorPos)
		{
			///find relative velocity
			//    IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
			//    IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
			IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
			IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();

			IndexedVector3 vel1 = IndexedVector3.Zero;
			body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1);
			IndexedVector3 vel2 = IndexedVector3.Zero; ;
			body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2);
			IndexedVector3 vel = vel1 - vel2;

			float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel);

			/// apply displacement correction

			//positional error (zeroth order error)
			float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a);
			float lo = float.MinValue;
			float hi = float.MaxValue;

			float minLimit = m_lowerLimit[limit_index];
			float maxLimit = m_upperLimit[limit_index];

			//handle the limits
			if (minLimit < maxLimit)
			{
				{
					if (depth > maxLimit)
					{
						depth -= maxLimit;
						lo = 0f;

					}
					else
					{
						if (depth < minLimit)
						{
							depth -= minLimit;
							hi = 0f;
						}
						else
						{
							return 0.0f;
						}
					}
				}
			}

			float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;

			float oldNormalImpulse = m_accumulatedImpulse[limit_index];
			float sum = oldNormalImpulse + normalImpulse;
			m_accumulatedImpulse[limit_index] =  (sum > hi ? 0f : sum < lo ? 0f : sum);
			normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;

			IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse;
			//body1.applyImpulse( impulse_vector, rel_pos1);
			//body2.applyImpulse(-impulse_vector, rel_pos2);

			IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a);
			IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a);
            body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1");
            body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2");

			return normalImpulse;

		}

        public void solveConstraintObsolete(RigidBody bodyA, RigidBody bodyB, float timeStep)
{
	if (m_useSolveConstraintObsolete)
	{
		IndexedVector3 pivotAInW = m_rbA.GetCenterOfMassTransform()*m_rbAFrame._origin;
		IndexedVector3 pivotBInW = m_rbB.GetCenterOfMassTransform()*m_rbBFrame._origin;

		float tau = 0.3f;

		//linear part
		if (!m_angularOnly)
		{
			IndexedVector3 rel_pos1 = pivotAInW - m_rbA.GetCenterOfMassPosition(); 
			IndexedVector3 rel_pos2 = pivotBInW - m_rbB.GetCenterOfMassPosition();

            IndexedVector3 vel1 = IndexedVector3.Zero;
			bodyA.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1);
            IndexedVector3 vel2 = IndexedVector3.Zero;
			bodyB.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2);
			IndexedVector3 vel = vel1 - vel2;

			for (int i=0;i<3;i++)
			{		
				IndexedVector3 normal = m_jac[i].m_linearJointAxis;
				float jacDiagABInv = 1.0f / m_jac[i].GetDiagonal();

				float rel_vel = normal.Dot(ref vel);
				//positional error (zeroth order error)
				float depth = -(pivotAInW - pivotBInW).Dot(ref normal); //this is the error projected on the normal
				float impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
				m_appliedImpulse += impulse;
				
				IndexedVector3 ftorqueAxis1 = rel_pos1.Cross(ref normal);
				IndexedVector3 ftorqueAxis2 = rel_pos2.Cross(ref normal);
				bodyA.InternalApplyImpulse(normal*m_rbA.GetInvMass(), m_rbA.GetInvInertiaTensorWorld()*ftorqueAxis1,impulse,null);
				bodyB.InternalApplyImpulse(normal*m_rbB.GetInvMass(), m_rbB.GetInvInertiaTensorWorld()*ftorqueAxis2,-impulse,null);
		
			}
		}

		// apply motor
		if (m_bMotorEnabled)
		{
			// compute current and predicted transforms
			IndexedMatrix trACur = m_rbA.GetCenterOfMassTransform();
			IndexedMatrix trBCur = m_rbB.GetCenterOfMassTransform();
			IndexedVector3 omegaA = IndexedVector3.Zero; bodyA.InternalGetAngularVelocity(ref omegaA);
            IndexedVector3 omegaB = IndexedVector3.Zero; bodyB.InternalGetAngularVelocity(ref omegaB);
			IndexedMatrix trAPred;
			IndexedVector3 zerovec = new IndexedVector3(0,0,0);
			TransformUtil.IntegrateTransform(ref trACur, ref zerovec, ref omegaA, timeStep, out trAPred);
			IndexedMatrix trBPred;
			TransformUtil.IntegrateTransform(ref trBCur, ref zerovec, ref omegaB, timeStep, out trBPred);

			// compute desired transforms in world
			IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
			IndexedMatrix trABDes = m_rbBFrame * trPose * m_rbAFrame.Inverse();
			IndexedMatrix trADes = trBPred * trABDes;
			IndexedMatrix trBDes = trAPred * trABDes.Inverse();

			// compute desired omegas in world
			IndexedVector3 omegaADes, omegaBDes;
			
			TransformUtil.CalculateVelocity(ref trACur, ref trADes, timeStep, out zerovec, out omegaADes);
			TransformUtil.CalculateVelocity(ref trBCur, ref trBDes, timeStep, out zerovec, out omegaBDes);

			// compute delta omegas
			IndexedVector3 dOmegaA = omegaADes - omegaA;
			IndexedVector3 dOmegaB = omegaBDes - omegaB;

			// compute weighted avg axis of dOmega (weighting based on inertias)
            IndexedVector3 axisA = IndexedVector3.Zero, axisB = IndexedVector3.Zero;
			float kAxisAInv = 0, kAxisBInv = 0;

			if (dOmegaA.LengthSquared() > MathUtil.SIMD_EPSILON)
			{
				axisA = dOmegaA.Normalized();
				kAxisAInv = GetRigidBodyA().ComputeAngularImpulseDenominator(ref axisA);
			}

			if (dOmegaB.LengthSquared() > MathUtil.SIMD_EPSILON)
			{
				axisB = dOmegaB.Normalized();
				kAxisBInv = GetRigidBodyB().ComputeAngularImpulseDenominator(ref axisB);
			}

			IndexedVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;

			if (bDoTorque && avgAxis.LengthSquared() > MathUtil.SIMD_EPSILON)
			{
				avgAxis.Normalize();
				kAxisAInv = GetRigidBodyA().ComputeAngularImpulseDenominator(ref avgAxis);
				kAxisBInv = GetRigidBodyB().ComputeAngularImpulseDenominator(ref avgAxis);
				float kInvCombined = kAxisAInv + kAxisBInv;

				IndexedVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
									(kInvCombined * kInvCombined);

				if (m_maxMotorImpulse >= 0)
				{
//.........这里部分代码省略.........