/// <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);
}