void AddLocalContact(ref ContactData contact, ref Matrix3x3 orientation, ref QuickList <ContactData> candidatesToAdd)
{
//Put the contact into world space.
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check to see if the contact is unique before proceeding.
if (IsContactUnique(ref contact, ref candidatesToAdd))
{
candidatesToAdd.Add(ref contact);
}
}
protected override void ProcessCandidates(ref QuickList <ContactData> candidates)
{
//If the candidates list is empty, then let's see if the convex is in the 'thickness' of the terrain.
if (candidates.Count == 0 & terrain.thickness > 0)
{
RayHit rayHit;
Ray ray = new Ray {
Position = convex.worldTransform.Position, Direction = terrain.worldTransform.LinearTransform.Up
};
ray.Direction.Normalize();
//The raycast has to use doublesidedness, since we're casting from the bottom up.
if (terrain.Shape.RayCast(ref ray, terrain.thickness, ref terrain.worldTransform, TriangleSidedness.DoubleSided, out rayHit))
{
//Found a hit!
rayHit.Normal.Normalize();
float dot;
Vector3.Dot(ref ray.Direction, ref rayHit.Normal, out dot);
var newContact = new ContactData
{
Normal = rayHit.Normal,
Position = convex.worldTransform.Position,
Id = 2,
PenetrationDepth = -rayHit.T * dot + convex.Shape.MinimumRadius
};
newContact.Validate();
bool found = false;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
//As set above, an id of 2 corresponds to a contact created from this raycast process.
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
found = true;
break;
}
}
if (!found)
{
candidates.Add(ref newContact);
}
}
}
}
//This works in the general case where there can be any number of contacts and candidates. Could specialize it as an optimization to single-contact added incremental manifolds.
///<summary>
/// Reduces the contact manifold to a good subset.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidates">Contact candidates to include in the reduction process.</param>
///<param name="contactsToRemove">Contacts that need to removed to reach the reduced state.</param>
///<param name="toAdd">Contact candidates that should be added to reach the reduced state.</param>
///<exception cref="InvalidOperationException">Thrown when the set being reduced is empty.</exception>
public static void ReduceContacts(RawList <Contact> contacts, ref QuickList <ContactData> contactCandidates, RawList <int> contactsToRemove, ref QuickList <ContactData> toAdd)
{
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
Vector3 normal = Toolbox.ZeroVector;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.Add(ref normal, ref contacts.Elements[i].Normal, out normal);
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3.Add(ref normal, ref contactCandidates.Elements[i].Normal, out normal);
if (contactCandidates.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = contacts.Count + i;
maximumDepth = contactCandidates.Elements[i].PenetrationDepth;
}
}
//If the normals oppose each other, this can happen. It doesn't need to be normalized, but having SOME normal is necessary.
if (normal.LengthSquared() < Toolbox.Epsilon)
{
if (contacts.Count > 0)
{
normal = contacts.Elements[0].Normal;
}
else if (contactCandidates.Count > 0)
{
normal = contactCandidates.Elements[0].Normal; //This method is only called when there's too many contacts, so if contacts is empty, the candidates must NOT be empty.
}
else //This method should not have been called at all if it gets here.
{
throw new ArgumentException("Cannot reduce an empty contact set.");
}
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < contacts.Count)
{
deepestPosition = contacts.Elements[deepestIndex].Position;
}
else
{
deepestPosition = contactCandidates.Elements[deepestIndex - contacts.Count].Position;
}
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3.DistanceSquared(ref contactCandidates.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = contacts.Count + i;
}
}
if (furthestIndex == -1)
{
//Either this method was called when it shouldn't have been, or all contacts and contact candidates are at the same location.
if (contacts.Count > 0)
{
for (int i = 1; i < contacts.Count; i++)
{
contactsToRemove.Add(i);
}
return;
}
if (contactCandidates.Count > 0)
{
toAdd.Add(ref contactCandidates.Elements[0]);
return;
}
throw new ArgumentException("Cannot reduce an empty contact set.");
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].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 normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
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;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
float dot;
Vector3.Dot(ref contactCandidates.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i + contacts.Count;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i + contacts.Count;
maxYAxisDot = dot;
}
}
//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.
for (int i = 0; i < contactCandidates.Count; i++)
{
int totalIndex = i + contacts.Count;
if (totalIndex == deepestIndex || totalIndex == furthestIndex || totalIndex == minYAxisIndex || totalIndex == maxYAxisIndex)
{
//This contact is present in the new manifold. Add it.
toAdd.Add(ref contactCandidates.Elements[i]);
}
}
for (int i = 0; i < contacts.Count; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
contactsToRemove.Add(i);
}
}
}