/// <summary>
/// Gets the error tolerance for the simplex.
/// </summary>
/// <param name="rayOrigin">Origin of the ray.</param>
/// <returns>Error tolerance of the simplex.</returns>
public float GetErrorTolerance(ref System.Numerics.Vector3 rayOrigin)
{
switch (State)
{
case SimplexState.Point:
float distanceA;
Vector3Ex.DistanceSquared(ref A, ref rayOrigin, out distanceA);
return(distanceA);
case SimplexState.Segment:
float distanceB;
Vector3Ex.DistanceSquared(ref A, ref rayOrigin, out distanceA);
Vector3Ex.DistanceSquared(ref B, ref rayOrigin, out distanceB);
return(MathHelper.Max(distanceA, distanceB));
case SimplexState.Triangle:
float distanceC;
Vector3Ex.DistanceSquared(ref A, ref rayOrigin, out distanceA);
Vector3Ex.DistanceSquared(ref B, ref rayOrigin, out distanceB);
Vector3Ex.DistanceSquared(ref C, ref rayOrigin, out distanceC);
return(MathHelper.Max(distanceA, MathHelper.Max(distanceB, distanceC)));
case SimplexState.Tetrahedron:
float distanceD;
Vector3Ex.DistanceSquared(ref A, ref rayOrigin, out distanceA);
Vector3Ex.DistanceSquared(ref B, ref rayOrigin, out distanceB);
Vector3Ex.DistanceSquared(ref C, ref rayOrigin, out distanceC);
Vector3Ex.DistanceSquared(ref D, ref rayOrigin, out distanceD);
return(MathHelper.Max(distanceA, MathHelper.Max(distanceB, MathHelper.Max(distanceC, distanceD))));
}
return(0);
}
private bool IsContactUnique(ref ContactData contactCandidate, ref QuickList <ContactData> candidatesToAdd)
{
contactCandidate.Validate();
float distanceSquared;
RigidTransform meshTransform = MeshTransform;
for (int i = 0; i < contacts.Count; i++)
{
Vector3Ex.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3Ex.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
}
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Vector3Ex.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3Ex.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
return(false);
}
}
}
//for (int i = 0; i < edgeContacts.count; i++)
//{
// Vector3Ex.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
//for (int i = 0; i < vertexContacts.count; i++)
//{
// Vector3Ex.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
return(true);
}
private bool IsContactUnique(ref ContactData contactCandidate)
{
contactCandidate.Validate();
for (int i = 0; i < contacts.Count; i++)
{
float distanceSquared;
Vector3Ex.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableA.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableB.worldTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
return(true);
}
//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;
System.Numerics.Vector3 normal = Toolbox.ZeroVector;
for (int i = 0; i < contacts.Count; i++)
{
Vector3Ex.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++)
{
Vector3Ex.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).
System.Numerics.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++)
{
Vector3Ex.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++)
{
Vector3Ex.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.");
}
System.Numerics.Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].Position;
}
System.Numerics.Vector3 xAxis;
Vector3Ex.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
System.Numerics.Vector3 yAxis;
Vector3Ex.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;
Vector3Ex.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;
Vector3Ex.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);
}
}
}
//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).
System.Numerics.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++)
{
Vector3Ex.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
Vector3Ex.DistanceSquared(ref contactCandidate.Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestIndex = 4;
}
System.Numerics.Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidate.Position;
}
System.Numerics.Vector3 xAxis;
Vector3Ex.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
System.Numerics.Vector3 yAxis;
Vector3Ex.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++)
{
Vector3Ex.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
Vector3Ex.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;
}
}
