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.");
//addCandidate = true;
//float min = float.MaxValue;
//int minIndex = 3;
//for (int i = 0; i < 4; i++)
//{
// if (contacts.Elements[i].PenetrationDepth < min)
// {
// min = contacts.Elements[i].PenetrationDepth;
// minIndex = i;
// }
//}
//toRemove.Add(minIndex);
//return;
//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;
}
}
