/// <summary> /// Resolves a set of particle contacts for both penetration /// and velocity. /// /// Contacts that cannot interact with each other should be /// passed to separate calls to resolveContacts, as the /// resolution algorithm takes much longer for lots of contacts /// than it does for the same number of contacts in small sets. /// /// <param name="contacts"> /// Contacts to resolve. /// </param> /// <param name="duration"> /// The duration of the previous integration step. /// This is used to compensate for forces applied. /// </param> public void ResolveContacts(List <ParticleContact> contacts, float duration) { for (int iterationsUsed = 0; iterationsUsed < Iterations; iterationsUsed++) { // Find the contact with the largest closing velocity; var max = float.MaxValue; ParticleContact maxContact = null; foreach (var contact in contacts) { var sepVel = contact.CalculateSeparatingVelocity(); if (sepVel < max && (sepVel < 0 || contact.Penetration > 0)) { max = sepVel; maxContact = contact; } } // Do we have anything worth resolving? if (maxContact == null) { break; } // Resolve this contact maxContact.Resolve(duration); // Update the interpenetrations for all particles foreach (var contact in contacts) { if (contact.Particle0 == maxContact.Particle0) { contact.Penetration -= Vector3.Dot(maxContact.ParticleMovement0, contact.ContactNormal); } else if (contact.Particle0 == maxContact.Particle1) { contact.Penetration -= Vector3.Dot(maxContact.ParticleMovement1, contact.ContactNormal); } if (contact.Particle1 != null) { if (contact.Particle1 == maxContact.Particle0) { contact.Penetration += Vector3.Dot(maxContact.ParticleMovement0, contact.ContactNormal); } else if (contact.Particle1 == maxContact.Particle1) { contact.Penetration += Vector3.Dot(maxContact.ParticleMovement1, contact.ContactNormal); } } } } }
public override void AddContacts(List <ParticleContact> contacts, int maxContacts) { // Find the length of the rod float currentLen = CurrentLength; // Check if we're over-extended if (currentLen == Length) { return; } var contact = new ParticleContact { Particle0 = Particle0, Particle1 = Particle1, Restitution = 0 // no bounciness }; // Calculate the normal var normal = Particle1.Position - Particle0.Position; normal.Normalize(); // The contact normal depends on whether we're extending or compressing if (currentLen > Length) { contact.ContactNormal = normal; contact.Penetration = currentLen - Length; } else { contact.ContactNormal = -normal; contact.Penetration = Length - currentLen; } contacts.Add(contact); }