コード例 #1
0
		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);
			}
		}