public static int OverlapCapsule(Vector3 point1, Vector3 point2, float radius, ICollection<Collider> results,
int layerMask = Physics.AllLayers, QueryTriggerInteraction query = QueryTriggerInteraction.UseGlobal)
{
if (results == null) throw new System.ArgumentNullException("results");
//note, HashSets are inherently unique collections... so no need to check if contains during second and third overlaps
using (var tmpSet = TempCollection.GetSet<Collider>())
{
if (VectorUtil.FuzzyEquals(point1, point2))
{
int cnt = Physics.OverlapSphereNonAlloc(point1, radius, _nonallocColliderBuffer, layerMask, query);
for (int i = 0; i < cnt; i++)
{
tmpSet.Add(_nonallocColliderBuffer[i]);
_nonallocColliderBuffer[i] = null;
}
}
else
{
if (_nonallocColliderBuffer == null) _nonallocColliderBuffer = new Collider[MAX_BUFFER];
if (_nonallocRaycastBuffer == null) _nonallocRaycastBuffer = new RaycastHit[MAX_BUFFER];
int cnt;
cnt = Physics.OverlapSphereNonAlloc(point1, radius, _nonallocColliderBuffer, layerMask, query);
for (int i = 0; i < cnt; i++)
{
tmpSet.Add(_nonallocColliderBuffer[i]);
_nonallocColliderBuffer[i] = null;
}
cnt = Physics.OverlapSphereNonAlloc(point2, radius, _nonallocColliderBuffer, layerMask, query);
for (int i = 0; i < cnt; i++)
{
tmpSet.Add(_nonallocColliderBuffer[i]);
_nonallocColliderBuffer[i] = null;
}
var dir = point2 - point1;
var dist = dir.magnitude;
cnt = Physics.SphereCastNonAlloc(point1, radius, dir.normalized, _nonallocRaycastBuffer, dist, layerMask, query);
for (int i = 0; i < cnt; i++)
{
tmpSet.Add(_nonallocRaycastBuffer[i].collider);
_nonallocRaycastBuffer[i] = default(RaycastHit);
}
}
//done, now fill collection
if (results is Collider[])
{
var arr = results as Collider[];
int cnt = Mathf.Min(arr.Length, tmpSet.Count);
int i = -1;
var e = tmpSet.GetEnumerator();
while (e.MoveNext() && ++i < cnt)
{
arr[i] = e.Current;
}
return cnt;
}
else
{
if (results is List<Collider>)
{
var lst = results as List<Collider>;
var num = tmpSet.Count + lst.Count;
if (lst.Capacity < num) lst.Capacity = num;
}
var e = tmpSet.GetEnumerator();
while (e.MoveNext())
{
results.Add(e.Current);
}
return tmpSet.Count;
}
}
}
public static bool Compare(IWaypoint a, Waypoint b)
{
//return (a.Position == b.Position) && (a.Heading == b._heading);
return(VectorUtil.FuzzyEquals(a.Position, b.Position) && VectorUtil.FuzzyEquals(a.Heading, b.Heading));
}