//This is very slow depending on how many contacts there are...
void CleanUp(List <ClosestTrianglePoint> closestPoints, List <ContactInfo> resultsBuffer)
{
if (closestPoints.Count > 0)
{
ignoreBehindPlanes.Clear();
//Taking advantage of C# built in QuickSort algorithm.
closestPoints.Sort(ClosestTrianglePointComparerAscend.defaultComparer);
//Note - If we are in a corner and our sphere origin is behind 1 wall while also large enough to touch the other wall, the first wall normal would block the second due to the .0001 offset below since the points lie on the same plane.
//This is actually desired since the interpolatednormal of that point would have been going towards inside the other wall, which means we would be try to depenetrate into inside the wall...
//Problem - I have noticed cases where this removes points we actually wanted, such as when we are on an edge of a single mesh touching its ceiling and floor. We would get the ceiling and floor normals as
//well as the edges side normals. One of the side normals would end up removing 1 of the ceiling / floor normals, giving our depenetration method trouble and possibly getting us stuck.
for (int i = 0; i < closestPoints.Count; i++)
{
ClosestTrianglePoint closestPoint = closestPoints[i];
Vector3 planeNormal = tree.GetTriangleNormal(closestPoint.triangleIndex);
if (planeNormal == Vector3.zero)
{
continue;
}
if (!MPlane.IsBehindPlanes(closestPoint.position, ignoreBehindPlanes, -.0001f)) //thanks to the .0001 offset we avoid duplicates
{
resultsBuffer.Add(new ContactInfo(transform.TransformPoint(closestPoint.position), transform.TransformDirection(planeNormal)));
}
ignoreBehindPlanes.Add(new MPlane(planeNormal, closestPoint.position, false));
}
}
}
public static int CompareNormalTo(ClosestTrianglePoint x, ClosestTrianglePoint y)
{
return(CompareNormalTo(x.sphereOrigin, x.position, x.meshTree.tree.GetTriangleNormal(x.triangleIndex), y.sphereOrigin, y.position, y.meshTree.tree.GetTriangleNormal(y.triangleIndex)));
}
