//This works in the specific case of 4 contacts and 1 contact candidate.
///<summary>
/// Reduces a 4-contact manifold and contact candidate to 4 total contacts.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidate">Contact candidate to include in the reduction process.</param>
///<param name="toRemove">Contacts that need to be removed to reduce the manifold.</param>
///<param name="addCandidate">Whether or not to add the contact candidate to reach the reduced manifold.</param>
///<exception cref="ArgumentException">Thrown when the contact manifold being reduced doesn't have 4 contacts.</exception>
public static void ReduceContacts(RawList <Contact> contacts, ref ContactData contactCandidate, RawList <int> toRemove, out bool addCandidate)
{
if (contacts.Count != 4)
{
throw new ArgumentException("Can only use this method to reduce contact lists with four contacts and a contact candidate.");
}
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
for (int i = 0; i < 4; i++)
{
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
if (contactCandidate.PenetrationDepth > maximumDepth)
{
deepestIndex = 4;
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < 4)
{
deepestPosition = contacts.Elements[deepestIndex].Position;
}
else
{
deepestPosition = contactCandidate.Position;
}
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < 4; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
Vector3.DistanceSquared(ref contactCandidate.Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestIndex = 4;
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidate.Position;
}
Vector3 xAxis;
Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
Vector3 yAxis;
Vector3.Cross(ref xAxis, ref contacts.Elements[0].Normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
float dot;
for (int i = 0; i < 4; i++)
{
Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
Vector3.Dot(ref contactCandidate.Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = 4;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = 4;
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
if (4 == deepestIndex || 4 == furthestIndex || 4 == minYAxisIndex || 4 == maxYAxisIndex)
{
addCandidate = true;
//Only reduce when we are going to add a new contact, and only get rid of one.
for (int i = 0; i < 4; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
toRemove.Add(i);
break;
}
}
}
else
{
addCandidate = false;
}
}
private void Events_ContactCreated(EntityCollidable sender, Collidable other, BEPUphysics.NarrowPhaseSystems.Pairs.CollidablePairHandler pair, BEPUphysics.CollisionTests.ContactData contact)
{
this.Collided.Execute(other, pair.Contacts);
}
