public override void Update(double dt)
{
RigidTransform transform = new RigidTransform(mesh.Position);
RigidTransform convexTransform = convex.WorldTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convexTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
var overlaps = new QuickList <Vector3i>(BufferPools <Vector3i> .Thread);
mesh.ChunkShape.GetOverlaps(mesh.Position, convex.BoundingBox, ref overlaps);
var candidatesToAdd = new QuickList <ContactData>(BufferPools <ContactData> .Thread, BufferPool <int> .GetPoolIndex(overlaps.Count));
for (int i = 0; i < overlaps.Count; i++)
{
if (!ActivePairs.TryGetValue(overlaps.Elements[i], out GeneralConvexPairTester manifold))
{
manifold = GetPair(ref overlaps.Elements[i]);
}
else
{
ActivePairs.FastRemove(overlaps.Elements[i]);
}
activePairsBackBuffer.Add(overlaps.Elements[i], manifold);
if (manifold.GenerateContactCandidate(out ContactData contactCandidate))
{
candidatesToAdd.Add(ref contactCandidate);
}
}
overlaps.Dispose();
for (int i = ActivePairs.Count - 1; i >= 0; i--)
{
ReturnPair(ActivePairs.Values[i]);
ActivePairs.FastRemove(ActivePairs.Keys[i]);
}
var temp = ActivePairs;
ActivePairs = activePairsBackBuffer;
activePairsBackBuffer = temp;
if (contacts.Count + candidatesToAdd.Count > 4)
{
var reducedCandidates = new QuickList <ContactData>(BufferPools <ContactData> .Thread, 3);
ContactReducer.ReduceContacts(contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.Count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
reducedCandidates.Dispose();
}
else if (candidatesToAdd.Count > 0)
{
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
candidatesToAdd.Dispose();
}
///<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.
ContactRefresher.ContactRefresh(contacts, supplementData, ref collidableA.worldTransform,
ref collidableB.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Now, generate a contact between the two shapes.
ContactData contact;
if (PairTester.GenerateContactCandidate(out contact))
{
//Eliminate any old contacts which have normals which would fight with this new contact.
for (int i = 0; i < contacts.Count; ++i)
{
float normalDot;
Vector3.Dot(ref contacts.Elements[i].Normal, ref contact.Normal, out normalDot);
if (normalDot < 0)
{
Remove(i);
break;
}
}
//If a contact is unique, add it to the manifold separately.
//If it is redundant, it will be used to update an existing contact... within the IsContactUnique call.
//In other words: THIS FUNCTION HAS IMPORTANT SNEAKY SIDE EFFECTS.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in.
Add(ref contact);
}
}
}
else
{
//No collision, clean out the manifold.
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.
ContactRefresher.ContactRefresh(contacts, supplementData, ref collidableA.worldTransform, ref collidableB.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Now, generate a contact between the two shapes.
ContactData contact;
if (pairTester.GenerateContactCandidate(out contact))
{
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in.
Add(ref contact);
}
}
}
else
{
//No collision, clean out the manifold.
for (int i = contacts.count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Compute the local triangle vertices.
//TODO: this could be quicker and cleaner.
localTriangleShape.collisionMargin = triangle.Shape.collisionMargin;
localTriangleShape.sidedness = triangle.Shape.sidedness;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation);
Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC);
Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA);
Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB);
Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC);
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.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
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.GenerateContactCandidates(localTriangleShape, out contactList))
{
for (int i = 0; i < contactList.Count; i++)
{
contactList.Get(i, out contact);
//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 if the contact is unique before proceeding.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
Add(ref contact);
}
}
}
}
else
{
//Clear out the contacts, it's separated.
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);
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();
}
public override void Update(float dt)
{
//Refresh the contact manifold for this frame.
var transform = new RigidTransform(voxelGrid.Position);
var convexTransform = convex.WorldTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convexTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
//Collect the set of overlapped cell indices.
//Not the fastest way to do this, but it's relatively simple and easy.
var overlaps = new QuickList <Int3>(BufferPools <Int3> .Thread);
voxelGrid.Shape.GetOverlaps(voxelGrid.Position, convex.BoundingBox, ref overlaps);
var candidatesToAdd = new QuickList <ContactData>(BufferPools <ContactData> .Thread, BufferPool <int> .GetPoolIndex(overlaps.Count));
for (int i = 0; i < overlaps.Count; ++i)
{
GeneralConvexPairTester manifold;
if (!ActivePairs.TryGetValue(overlaps.Elements[i], out manifold))
{
//This manifold did not previously exist.
manifold = GetPair(ref overlaps.Elements[i]);
}
else
{
//It did previously exist.
ActivePairs.FastRemove(overlaps.Elements[i]);
}
activePairsBackBuffer.Add(overlaps.Elements[i], manifold);
ContactData contactCandidate;
if (manifold.GenerateContactCandidate(out contactCandidate))
{
candidatesToAdd.Add(ref contactCandidate);
}
}
overlaps.Dispose();
//Any pairs remaining in the activePairs set no longer exist. Clean them up.
for (int i = ActivePairs.Count - 1; i >= 0; --i)
{
ReturnPair(ActivePairs.Values[i]);
ActivePairs.FastRemove(ActivePairs.Keys[i]);
}
//Swap the pair sets.
var temp = ActivePairs;
ActivePairs = activePairsBackBuffer;
activePairsBackBuffer = temp;
//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.
var reducedCandidates = new QuickList <ContactData>(BufferPools <ContactData> .Thread, 3);
ContactReducer.ReduceContacts(contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.Count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
reducedCandidates.Dispose();
}
else if (candidatesToAdd.Count > 0)
{
//Won't overflow the manifold, so just toss it in.
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
candidatesToAdd.Dispose();
}
///<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();
}
