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)); }