Beispiel #1
0
        //velocity + friction
        //response  between two dynamic objects with friction
        public virtual float ResolveSingleCollisionCombinedCacheFriendly(
            SolverBody bodyA,
            SolverBody bodyB,
            SolverConstraint contactConstraint,
            ContactSolverInfo solverInfo)
        {
            float normalImpulse = 0;

            if (contactConstraint.Penetration < 0)
            {
                return(0);
            }

            float relVel;
            float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
                             + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
            float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
                             + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);

            relVel = velADotn - velBDotn;

            float positionalError = contactConstraint.Penetration;
            float velocityError   = contactConstraint.Restitution - relVel;          // * damping;

            float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
            float velocityImpulse    = velocityError * contactConstraint.JacDiagABInv;

            normalImpulse = penetrationImpulse + velocityImpulse;

            // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
            float oldNormalImpulse = contactConstraint.AppliedImpulse;
            float sum = oldNormalImpulse + normalImpulse;

            contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;

            float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
            float velocitySum        = oldVelocityImpulse + velocityImpulse;

            contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;

            normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;

            if (bodyA.InvMass != 0)
            {
                bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
                                   contactConstraint.AngularComponentA, normalImpulse);
            }
            if (bodyB.InvMass != 0)
            {
                bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
                                   contactConstraint.AngularComponentB, -normalImpulse);
            }

            return(normalImpulse);
        }
Beispiel #2
0
 private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
 {
     solverBody = new SolverBody();
     solverBody.AngularVelocity      = rigidBody.AngularVelocity;
     solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
     solverBody.Friction             = rigidBody.Friction;
     solverBody.InvMass        = rigidBody.InverseMass;
     solverBody.LinearVelocity = rigidBody.LinearVelocity;
     solverBody.OriginalBody   = rigidBody;
     solverBody.AngularFactor  = rigidBody.AngularFactor;
 }
Beispiel #3
0
        public virtual float ResolveSingleFrictionCacheFriendly(
            SolverBody bodyA,
            SolverBody bodyB,
            SolverConstraint contactConstraint,
            ContactSolverInfo solverInfo,
            float appliedNormalImpulse)
        {
            float combinedFriction = contactConstraint.Friction;
            float limit            = appliedNormalImpulse * combinedFriction;

            if (appliedNormalImpulse > 0)
            //friction
            {
                float j1;
                {
                    float relVel;
                    float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
                                     + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
                    float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
                                     + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
                    relVel = velADotn - velBDotn;

                    // calculate j that moves us to zero relative velocity
                    j1 = -relVel * contactConstraint.JacDiagABInv;
                    float oldTangentImpulse = contactConstraint.AppliedImpulse;
                    contactConstraint.AppliedImpulse = oldTangentImpulse + j1;

                    float test = contactConstraint.AppliedImpulse;
                    MathHelper.SetMin(ref test, limit);
                    MathHelper.SetMax(ref test, -limit);
                    contactConstraint.AppliedImpulse = test;

                    j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
                }

                if (bodyA.InvMass != 0)
                {
                    bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
                }
                if (bodyB.InvMass != 0)
                {
                    bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
                }
            }
            return(0);
        }
		private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
		{
			solverBody = new SolverBody();
			solverBody.AngularVelocity = rigidBody.AngularVelocity;
			solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
			solverBody.Friction = rigidBody.Friction;
			solverBody.InvMass = rigidBody.InverseMass;
			solverBody.LinearVelocity = rigidBody.LinearVelocity;
			solverBody.OriginalBody = rigidBody;
			solverBody.AngularFactor = rigidBody.AngularFactor;
		}
		public virtual float ResolveSingleFrictionCacheFriendly(
			SolverBody bodyA,
			SolverBody bodyB,
			SolverConstraint contactConstraint,
			ContactSolverInfo solverInfo,
			float appliedNormalImpulse)
		{
			float combinedFriction = contactConstraint.Friction;
			float limit = appliedNormalImpulse * combinedFriction;

			if (appliedNormalImpulse > 0)
			//friction
			{
				float j1;
				{
					float relVel;
					float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
								+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
					float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
						+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
					relVel = velADotn - velBDotn;

					// calculate j that moves us to zero relative velocity
					j1 = -relVel * contactConstraint.JacDiagABInv;
					float oldTangentImpulse = contactConstraint.AppliedImpulse;
					contactConstraint.AppliedImpulse = oldTangentImpulse + j1;

					float test = contactConstraint.AppliedImpulse;
					MathHelper.SetMin(ref test, limit);
					MathHelper.SetMax(ref test, -limit);
					contactConstraint.AppliedImpulse = test;

					j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
				}

				if (bodyA.InvMass != 0)
				{
					bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
				}
				if (bodyB.InvMass != 0)
				{
					bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
				}
			}
			return 0;
		}
		//velocity + friction
		//response  between two dynamic objects with friction
		public virtual float ResolveSingleCollisionCombinedCacheFriendly(
			SolverBody bodyA,
			SolverBody bodyB,
			SolverConstraint contactConstraint,
			ContactSolverInfo solverInfo)
		{
			float normalImpulse = 0;

			if (contactConstraint.Penetration < 0)
				return 0;

			float relVel;
			float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
						+ Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
			float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
						+ Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);

			relVel = velADotn - velBDotn;

			float positionalError = contactConstraint.Penetration;
			float velocityError = contactConstraint.Restitution - relVel;// * damping;

			float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
			float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
			normalImpulse = penetrationImpulse + velocityImpulse;

			// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
			float oldNormalImpulse = contactConstraint.AppliedImpulse;
			float sum = oldNormalImpulse + normalImpulse;
			contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;

			float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
			float velocitySum = oldVelocityImpulse + velocityImpulse;
			contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;

			normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;

			if (bodyA.InvMass != 0)
			{
				bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
					contactConstraint.AngularComponentA, normalImpulse);
			}
			if (bodyB.InvMass != 0)
			{
				bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
					contactConstraint.AngularComponentB, -normalImpulse);
			}

			return normalImpulse;
		}