//TODO: Having a specialized triangle-triangle pair test would be nice. Even if it didn't use an actual triangle-triangle test, certain assumptions could still make it speedier and more elegant.
//"Closest points between triangles" + persistent manifolding would probably be the best approach (a lot faster than the triangle-convex general case anyway).
public override bool GenerateContactCandidates(TriangleShape triangle,
out TinyStructList <ContactData> contactList)
{
if (base.GenerateContactCandidates(triangle, out contactList))
{
//The triangle-convex pair test has already rejected contacts whose normals would violate the first triangle's sidedness.
//However, since it's a vanilla triangle-convex test, it doesn't know about the sidedness of the other triangle!
TriangleShape shape = (TriangleShape)convex;
Vector3 normal;
//Lots of recalculating ab-bc!
Vector3 ab, ac;
Vector3.Subtract(ref shape.vB, ref shape.vA, out ab);
Vector3.Subtract(ref shape.vC, ref shape.vA, out ac);
Vector3.Cross(ref ab, ref ac, out normal);
TriangleSidedness sidedness = shape.sidedness;
if (sidedness != TriangleSidedness.DoubleSided)
{
for (int i = contactList.Count - 1; i >= 0; i--)
{
ContactData item;
contactList.Get(i, out item);
float dot;
Vector3.Dot(ref item.Normal, ref normal, out dot);
if (sidedness == TriangleSidedness.Clockwise)
{
if (dot < 0)
{
contactList.RemoveAt(i);
}
}
else
{
if (dot > 0)
{
contactList.RemoveAt(i);
}
}
}
}
return(contactList.Count > 0);
}
return(false);
}
//TODO: Having a specialized triangle-triangle pair test would be nice. Even if it didn't use an actual triangle-triangle test, certain assumptions could still make it speedier and more elegant.
//"Closest points between triangles" + persistent manifolding would probably be the best approach (a lot faster than the triangle-convex general case anyway).
public override bool GenerateContactCandidate(out TinyStructList<ContactData> contactList)
{
if (base.GenerateContactCandidate(out contactList))
{
//The triangle-convex pair test has already rejected contacts whose normals would violate the first triangle's sidedness.
//However, since it's a vanilla triangle-convex test, it doesn't know about the sidedness of the other triangle!
var shape = ((TriangleShape)convex);
Vector3 normal;
//Lots of recalculating ab-bc!
Vector3 ab, ac;
Vector3.Subtract(ref shape.vB, ref shape.vA, out ab);
Vector3.Subtract(ref shape.vC, ref shape.vA, out ac);
Vector3.Cross(ref ab, ref ac, out normal);
var sidedness = shape.sidedness;
if (sidedness != TriangleSidedness.DoubleSided)
{
for (int i = contactList.Count - 1; i >= 0; i--)
{
ContactData item;
contactList.Get(i, out item);
float dot;
Vector3.Dot(ref item.Normal, ref normal, out dot);
if (sidedness == TriangleSidedness.Clockwise)
{
if (dot < 0)
{
contactList.RemoveAt(i);
}
}
else
{
if (dot > 0)
{
contactList.RemoveAt(i);
}
}
}
}
return contactList.Count > 0;
}
return false;
}
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);
}
}
}
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);
}
}
}