/// <summary>
/// Adds a contact to the arbiter (threadsafe). No more than four contacts
/// are stored in the contactList. When adding a new contact
/// to the arbiter the existing are checked and the best are kept.
/// </summary>
/// <param name="point1">Point on body1. In world space.</param>
/// <param name="point2">Point on body2. In world space.</param>
/// <param name="normal">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public Contact AddContact(Vector3 point1, Vector3 point2, Vector3 normal, float penetration,
ContactSettings contactSettings)
{
Vector3 relPos1;
Vector3.Subtract(ref point1, ref body1.position, out relPos1);
int index;
lock (contactList)
{
if (this.contactList.Count == 4)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
else
{
Contact contact = Contact.Pool.GetNew();
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
private void ReplaceContact(ref Vector3 point1, ref Vector3 point2, ref Vector3 n, float p, int index,
ContactSettings contactSettings)
{
Contact contact = contactList[index];
Debug.Assert(body1 == contact.body1, "Body1 and Body2 not consistent.");
contact.Initialize(body1, body2, ref point1, ref point2, ref n, p, false, contactSettings);
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref Vector3 point1, ref Vector3 point2, ref Vector3 n,
float penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
Vector3.Subtract(ref p1, ref body1.position, out relativePos1);
Vector3.Subtract(ref p2, ref body2.position, out relativePos2);
Vector3.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
Vector3.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
accumulatedNormalImpulse = 0.0f;
accumulatedTangentImpulse = 0.0f;
lostSpeculativeBounce = 0.0f;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = JMath.Max(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = JMath.Min(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / 2.0f;
dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / 2.0f;
restitution = (body1.material.restitution + body2.material.restitution) / 2.0f;
break;
}
}
this.settings = settings;
}