public ContactInfo ClosestPointOnSurface(Vector3 segment0, Vector3 segment1, float radius)
{
trianglesBufferSingleCaps.Clear();
CapsuleShape capsule = CapsuleShape.ToLocalOfUniformScale(new CapsuleShape(segment0, segment1, radius), transform);
Vector3 localSegment0 = capsule.top;
Vector3 localSegment1 = capsule.bottom;
radius = capsule.radius;
AABB aabb = AABB.CreateCapsuleAABB(localSegment0, localSegment1, radius + .001f);
tree.FindClosestTriangles(tree.rootNode, aabb, trianglesBufferSingleCaps, infoDebug);
Vector3 p1, p2, p3;
IntersectPoints near;
IntersectPoints nearest = new IntersectPoints();
float radiusSqrd = radius * radius;
float shortestDistance = float.MaxValue;
int shortestTriangleIndex = -1;
HashSet <int> .Enumerator enumerator = trianglesBufferSingleCaps.GetEnumerator();
while (enumerator.MoveNext())
{
tree.GetTriangleVertices(enumerator.Current, out p1, out p2, out p3);
near = Geometry.ClosestPointOnTriangleToLine(localSegment0, localSegment1, p1, p2, p3, true);
float distance = (near.second - near.first).sqrMagnitude;
if (PointIsBetter(distance, shortestDistance, nearest.first, nearest.second, near.first, near.second, shortestTriangleIndex, enumerator.Current, radiusSqrd))
{
shortestDistance = distance;
nearest = near;
shortestTriangleIndex = enumerator.Current;
}
}
if (nearest.second == Vector3.zero)
{
return(new ContactInfo());
}
return(new ContactInfo(transform.TransformPoint(nearest.second), transform.TransformDirection(tree.GetTriangleNormal(shortestTriangleIndex))));
}
public List <ContactInfo> ClosestPointsOnSurface(Vector3 segment0, Vector3 segment1, float radius, List <ContactInfo> resultsBuffer)
{
resultsBuffer.Clear();
trianglesBufferMultiCaps.Clear();
closestPointsBufferMultiCaps.Clear();
CapsuleShape capsule = CapsuleShape.ToLocalOfUniformScale(new CapsuleShape(segment0, segment1, radius), transform);
Vector3 localSegment0 = capsule.top;
Vector3 localSegment1 = capsule.bottom;
radius = capsule.radius;
AABB aabb = AABB.CreateCapsuleAABB(localSegment0, localSegment1, radius + .001f); //We add a small radius increase due to float point precision issues that rarely happen.
tree.FindClosestTriangles(tree.rootNode, aabb, trianglesBufferMultiCaps, infoDebug);
Vector3 p1, p2, p3;
IntersectPoints nearest;
float radiusSqrd = radius * radius;
float distance = 0;
HashSet <int> .Enumerator enumerator = trianglesBufferMultiCaps.GetEnumerator();
while (enumerator.MoveNext())
{
tree.GetTriangleVertices(enumerator.Current, out p1, out p2, out p3);
nearest = Geometry.ClosestPointOnTriangleToLine(localSegment0, localSegment1, p1, p2, p3, true);
distance = (nearest.second - nearest.first).sqrMagnitude;
if (distance <= radiusSqrd)
{
closestPointsBufferMultiCaps.Add(new ClosestTrianglePoint(nearest.second, distance, enumerator.Current, nearest.first, this));
}
}
CleanUp(closestPointsBufferMultiCaps, resultsBuffer);
return(resultsBuffer);
}
