///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
var transform = MeshTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
CleanUpOverlappingTriangles();
//Get all the overlapped triangle indices.
int triangleCount = FindOverlappingTriangles(dt);
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
var guaranteedContacts = 0;
for (int i = 0; i < triangleCount; i++)
{
//Initialize the local triangle.
TriangleIndices indices;
if (ConfigureTriangle(i, out indices))
{
//Find a pairtester for the triangle.
TrianglePairTester pairTester;
if (!activePairTesters.TryGetValue(indices, out pairTester))
{
pairTester = GetTester();
pairTester.Initialize(convex.Shape, localTriangleShape);
activePairTesters.Add(indices, pairTester);
}
pairTester.Updated = true;
//Put the triangle into the local space of the convex.
Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);
//Now, generate a contact between the two shapes.
ContactData contact;
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidate(out contactList))
{
for (int j = 0; j < contactList.Count; j++)
{
contactList.Get(j, out contact);
if (UseImprovedBoundaryHandling)
{
if (AnalyzeCandidate(ref indices, pairTester, ref contact))
{
//This is let through if there's a face contact. Face contacts cannot be blocked.
guaranteedContacts++;
AddLocalContact(ref contact, ref orientation);
}
}
else
{
AddLocalContact(ref contact, ref orientation);
}
}
}
//Get the voronoi region from the contact candidate generation. Possibly just recalculate, since most of the systems don't calculate it.
//Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region. For face contacts, don't bother- just add it if unique.
//For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
//For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
//If the edge/vertex is already present in the set, then DO NOT add the contact.
//When adding a contact, add ALL other voronoi regions to the blocked sets.
}
}
if (UseImprovedBoundaryHandling)
{
//If there were no face contacts that absolutely must be included, we may get into a very rare situation
//where absolutely no contacts get created. For example, a sphere falling directly on top of a vertex in a flat terrain.
//It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices).
//In some cases, the contacts will be ignored instead of corrected (e.g. spheres).
//To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires.
//Sure, it might not be necessary under normal circumstances, but it's a better option than having no contacts.
//TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge,
//and an edge only restricts the far vertex and other two edges. This introduces an occasional bump though...
//It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles.
//For this state to continue beyond a brief instant generally requires the object be orientation locked and slender.
//However, some characters fit this description, so it can't be ignored!
//Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only).
//However, an object stuck between multiple triangles is not in a stable state. In the edge case, the object gets shoved to one side
//as one contact 'wins' the solver war. That's not enough to escape, unfortunately.
//The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability.
//So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :)
//This isn't a completely free operation, but it's guarded behind pretty rare conditions.
//Essentially, we will check to see if there's just edge contacts fighting against each other.
//If they are, then we will correct any stuck-contributing normals to the triangle normal.
if (vertexContacts.Count == 0 && guaranteedContacts == 0 && edgeContacts.Count > 1)
{
//There are only edge contacts, check to see if:
//all normals are coplanar, and
//at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner).
bool allNormalsInSamePlane = true;
bool atLeastOneNormalAgainst = false;
var firstNormal = edgeContacts.Elements[0].ContactData.Normal;
edgeContacts.Elements[0].CorrectedNormal.Normalize();
float dot;
Vector3.Dot(ref firstNormal, ref edgeContacts.Elements[0].CorrectedNormal, out dot);
if (Math.Abs(dot) > .01f)
{
//Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity.
allNormalsInSamePlane = false;
}
else
{
//TODO: Note that we're only checking the new edge contacts, not the existing contacts.
//It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity
//we'll ignore that possibility for now.
for (int i = 1; i < edgeContacts.Count; i++)
{
Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot);
if (dot < 0)
{
atLeastOneNormalAgainst = true;
}
//Check to see if the normal is outside the plane.
Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref edgeContacts.Elements[0].CorrectedNormal, out dot);
if (Math.Abs(dot) > .01f)
{
//We are not stuck!
allNormalsInSamePlane = false;
break;
}
}
}
if (allNormalsInSamePlane && atLeastOneNormalAgainst)
{
//Uh oh! all the normals are parallel... The object is probably in a weird situation.
//Let's correct the normals!
//Already normalized the first contact above.
//We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already.
edgeContacts.Elements[0].ContactData.Normal = edgeContacts.Elements[0].CorrectedNormal;
edgeContacts.Elements[0].ShouldCorrect = true;
for (int i = 1; i < edgeContacts.Count; i++)
{
//Must normalize the corrected normal before using it.
edgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
if (dot < .01)
{
//Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness!
//If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal.
//Make them the same thing :)
edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal;
edgeContacts.Elements[i].ShouldCorrect = true;
//Note that the penetration depth is NOT corrected. The contact's depth no longer represents the true depth.
//However, we only need to have some penetration depth to get the object to escape the rut.
//Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction.
//If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system
//would have picked a different depth, as it finds a reasonable approximation of the minimum penetration!
//As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it).
}
}
}
}
for (int i = 0; i < edgeContacts.Count; i++)
{
//Only correct if it's allowed AND it's blocked.
//If it's not blocked, the contact being created is necessary!
//The normal generated by the triangle-convex tester is already known not to
//violate the triangle sidedness.
if (!blockedEdgeRegions.Contains(edgeContacts.Elements[i].Edge))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
}
else if (edgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
edgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal;
edgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
for (int i = 0; i < vertexContacts.Count; i++)
{
if (!blockedVertexRegions.Contains(vertexContacts.Elements[i].Vertex))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
}
else if (vertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
vertexContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref vertexContacts.Elements[i].CorrectedNormal, ref vertexContacts.Elements[i].ContactData.Normal, out dot);
vertexContacts.Elements[i].ContactData.Normal = vertexContacts.Elements[i].CorrectedNormal;
vertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
blockedEdgeRegions.Clear();
blockedVertexRegions.Clear();
vertexContacts.Clear();
edgeContacts.Clear();
}
//Remove stale pair testers.
//This will only remove 8 stale ones per frame, but it doesn't really matter.
//VERY rarely will there be more than 8 in a single frame, and they will be immediately taken care of in the subsequent frame.
var toRemove = new TinyList <TriangleIndices>();
foreach (KeyValuePair <TriangleIndices, TrianglePairTester> pair in activePairTesters)
{
if (!pair.Value.Updated)
{
if (!toRemove.Add(pair.Key))
{
break;
}
}
else
{
pair.Value.Updated = false;
}
}
for (int i = toRemove.Count - 1; i >= 0; i--)
{
var pairTester = activePairTesters[toRemove[i]];
pairTester.CleanUp();
GiveBackTester(pairTester);
activePairTesters.Remove(toRemove[i]);
}
//Some child types will want to do some extra post processing on the manifold.
ProcessCandidates(candidatesToAdd);
//Check if adding the new contacts would overflow the manifold.
if (contacts.Count + candidatesToAdd.Count > 4)
{
//Adding all the contacts would overflow the manifold. Reduce to the best subset.
ContactReducer.ReduceContacts(contacts, candidatesToAdd, contactIndicesToRemove, reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.Count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
}
else if (candidatesToAdd.Count > 0)
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
candidatesToAdd.Clear();
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
BoxContactDataCache manifold;
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
unsafe
{
BoxContactData *manifoldPointer = &manifold.D1;
Vector3.Negate(ref axis, out axis);
var toRemove = new TinyList <int>();
for (int i = 0; i < contacts.Count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
if (contacts.Elements[i].Id == manifoldPointer[j].Id)
{
found = true;
contacts.Elements[i].Validate();
//Update contact...
contacts.Elements[i].Position = manifoldPointer[j].Position;
contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
contacts.Elements[i].Normal = axis;
//Remove manifold entry
contacts.Elements[i].Validate();
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
{
Remove(toRemove[i]);
}
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
var newContact = new ContactData
{
Position = manifoldPointer[i].Position,
PenetrationDepth = -manifoldPointer[i].Depth,
Normal = axis,
Id = manifoldPointer[i].Id
};
Add(ref newContact);
}
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
var manifold = new TinyStructList <BoxContactData>();
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
Vector3.Negate(ref axis, out axis);
TinyList <int> toRemove = new TinyList <int>();
BoxContactData data;
for (int i = 0; i < contacts.Count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
manifold.Get(j, out data);
if (contacts.Elements[i].Id == data.Id)
{
found = true;
//Update contact...
contacts.Elements[i].Position = data.Position;
contacts.Elements[i].PenetrationDepth = -data.Depth;
contacts.Elements[i].Normal = axis;
contacts.Elements[i].Validate();
//Remove manifold entry
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
////Go through the indices to remove.
////For each one, replace the removal index with a contact in the new manifold.
//int removalIndex;
//for (removalIndex = toRemove.count - 1; removalIndex >= 0 && manifold.count > 0; removalIndex--)
//{
// int indexToReplace = toRemove[removalIndex];
// toRemove.RemoveAt(removalIndex);
// manifold.Get(manifold.count - 1, out data);
// //Update contact...
// contacts.Elements[indexToReplace].Position = data.Position;
// contacts.Elements[indexToReplace].PenetrationDepth = -data.Depth;
// contacts.Elements[indexToReplace].Normal = axis;
// contacts.Elements[indexToReplace].Id = data.Id;
// //Remove manifold entry
// manifold.RemoveAt(manifold.count - 1);
//}
//Alright, we ran out of contacts to replace (if, in fact, toRemove isn't empty now). Just remove the remainder.
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
{
Remove(toRemove[i]);
}
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
manifold.Get(i, out data);
ContactData newContact = new ContactData();
newContact.Position = data.Position;
newContact.PenetrationDepth = -data.Depth;
newContact.Normal = axis;
newContact.Id = data.Id;
Add(ref newContact);
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
var transform = MeshTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
CleanUpOverlappingTriangles();
//Get all the overlapped triangle indices.
int triangleCount = FindOverlappingTriangles(dt);
Matrix3X3 orientation;
Matrix3X3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
for (int i = 0; i < triangleCount; i++)
{
//Initialize the local triangle.
TriangleIndices indices;
if (ConfigureTriangle(i, out indices))
{
//Find a pairtester for the triangle.
TrianglePairTester pairTester;
if (!activePairTesters.TryGetValue(indices, out pairTester))
{
pairTester = GetTester();
pairTester.Initialize(convex.Shape, localTriangleShape);
activePairTesters.Add(indices, pairTester);
}
pairTester.Updated = true;
//Put the triangle into the local space of the convex.
Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
Matrix3X3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
Matrix3X3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
Matrix3X3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);
//Now, generate a contact between the two shapes.
ContactData contact;
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidate(out contactList))
{
for (int j = 0; j < contactList.count; j++)
{
contactList.Get(j, out contact);
if (UseImprovedBoundaryHandling)
{
if (AnalyzeCandidate(ref indices, pairTester, ref contact))
{
AddLocalContact(ref contact, ref orientation);
}
}
else
{
AddLocalContact(ref contact, ref orientation);
}
}
}
//Get the voronoi region from the contact candidate generation. Possibly just recalculate, since most of the systems don't calculate it.
//Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region. For face contacts, don't bother- just add it if unique.
//For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
//For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
//If the edge/vertex is already present in the set, then DO NOT add the contact.
//When adding a contact, add ALL other voronoi regions to the blocked sets.
}
}
if (UseImprovedBoundaryHandling)
{
for (int i = 0; i < edgeContacts.count; i++)
{
//Only correct if it's allowed AND it's blocked.
//If it's not blocked, the contact being created is necessary!
//The normal generated by the triangle-convex tester is already known not to
//violate the triangle sidedness.
if (!blockedEdgeRegions.Contains(edgeContacts.Elements[i].Edge))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
}
else if (edgeContacts.Elements[i].ShouldCorrect)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
edgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal;
edgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
for (int i = 0; i < vertexContacts.count; i++)
{
if (!blockedVertexRegions.Contains(vertexContacts.Elements[i].Vertex))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
}
else if (vertexContacts.Elements[i].ShouldCorrect)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
vertexContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref vertexContacts.Elements[i].CorrectedNormal, ref vertexContacts.Elements[i].ContactData.Normal, out dot);
vertexContacts.Elements[i].ContactData.Normal = vertexContacts.Elements[i].CorrectedNormal;
vertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
blockedEdgeRegions.Clear();
blockedVertexRegions.Clear();
vertexContacts.Clear();
edgeContacts.Clear();
}
//Remove stale pair testers.
//This will only remove 8 stale ones per frame, but it doesn't really matter.
//VERY rarely will there be more than 8 in a single frame, and they will be immediately taken care of in the subsequent frame.
var toRemove = new TinyList <TriangleIndices>();
foreach (KeyValuePair <TriangleIndices, TrianglePairTester> pair in activePairTesters)
{
if (!pair.Value.Updated)
{
if (!toRemove.Add(pair.Key))
{
break;
}
}
else
{
pair.Value.Updated = false;
}
}
for (int i = toRemove.count - 1; i >= 0; i--)
{
var pairTester = activePairTesters[toRemove[i]];
pairTester.CleanUp();
GiveBackTester(pairTester);
activePairTesters.Remove(toRemove[i]);
}
//Some child types will want to do some extra post processing on the manifold.
ProcessCandidates(candidatesToAdd);
//Check if adding the new contacts would overflow the manifold.
if (contacts.count + candidatesToAdd.count > 4)
{
//Adding all the contacts would overflow the manifold. Reduce to the best subset.
ContactReducer.ReduceContacts(contacts, candidatesToAdd, contactIndicesToRemove, reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
}
else if (candidatesToAdd.count > 0)
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
for (int i = 0; i < candidatesToAdd.count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
candidatesToAdd.Clear();
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
BoxContactDataCache manifold;
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
unsafe
{
BoxContactData* manifoldPointer = &manifold.D1;
Vector3.Negate(ref axis, out axis);
var toRemove = new TinyList<int>();
for (int i = 0; i < contacts.count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
if (contacts.Elements[i].Id == manifoldPointer[j].Id)
{
found = true;
//Update contact...
contacts.Elements[i].Position = manifoldPointer[j].Position;
contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
contacts.Elements[i].Normal = axis;
//Remove manifold entry
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
Remove(toRemove[i]);
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
var newContact = new ContactData();
newContact.Position = manifoldPointer[i].Position;
newContact.PenetrationDepth = -manifoldPointer[i].Depth;
newContact.Normal = axis;
newContact.Id = manifoldPointer[i].Id;
Add(ref newContact);
}
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
var manifold = new TinyStructList<BoxContactData>();
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
Vector3.Negate(ref axis, out axis);
TinyList<int> toRemove = new TinyList<int>();
BoxContactData data;
for (int i = 0; i < contacts.count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
manifold.Get(j, out data);
if (contacts.Elements[i].Id == data.Id)
{
found = true;
//Update contact...
contacts.Elements[i].Position = data.Position;
contacts.Elements[i].PenetrationDepth = -data.Depth;
contacts.Elements[i].Normal = axis;
//Remove manifold entry
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
////Go through the indices to remove.
////For each one, replace the removal index with a contact in the new manifold.
//int removalIndex;
//for (removalIndex = toRemove.count - 1; removalIndex >= 0 && manifold.count > 0; removalIndex--)
//{
// int indexToReplace = toRemove[removalIndex];
// toRemove.RemoveAt(removalIndex);
// manifold.Get(manifold.count - 1, out data);
// //Update contact...
// contacts.Elements[indexToReplace].Position = data.Position;
// contacts.Elements[indexToReplace].PenetrationDepth = -data.Depth;
// contacts.Elements[indexToReplace].Normal = axis;
// contacts.Elements[indexToReplace].Id = data.Id;
// //Remove manifold entry
// manifold.RemoveAt(manifold.count - 1);
//}
//Alright, we ran out of contacts to replace (if, in fact, toRemove isn't empty now). Just remove the remainder.
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
Remove(toRemove[i]);
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
manifold.Get(i, out data);
ContactData newContact = new ContactData();
newContact.Position = data.Position;
newContact.PenetrationDepth = -data.Depth;
newContact.Normal = axis;
newContact.Id = data.Id;
Add(ref newContact);
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.count - 1; i >= 0; i--)
{
Remove(i);
}
}
}