+ astar project

1 files changed, 747 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs
new file mode 100644
index 0000000..9a8a4cc
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs
@@ -0,0 +1,747 @@
+using Pathfinding.Util;
+using System.Collections.Generic;
+using Unity.Burst;
+using Unity.Collections;
+using Unity.Jobs;
+using Unity.Mathematics;
+using UnityEngine;
+using System.Linq;
+
+namespace Pathfinding {
+	/// <summary>
+	/// Contains useful functions for working with paths and nodes.
+	/// This class works a lot with the <see cref="Pathfinding.GraphNode"/> class, a useful function to get nodes is AstarPath.GetNearest.
+	/// See: <see cref="AstarPath.GetNearest"/>
+	/// See: <see cref="Pathfinding.GraphUpdateUtilities"/>
+	/// See: <see cref="Pathfinding.GraphUtilities"/>
+	/// </summary>
+	public static class PathUtilities {
+		/// <summary>
+		/// Returns if there is a walkable path from node1 to node2.
+		/// This method is extremely fast because it only uses precalculated information.
+		///
+		/// <code>
+		/// GraphNode node1 = AstarPath.active.GetNearest(point1, NNConstraint.Walkable).node;
+		/// GraphNode node2 = AstarPath.active.GetNearest(point2, NNConstraint.Walkable).node;
+		///
+		/// if (PathUtilities.IsPathPossible(node1, node2)) {
+		///     // Yay, there is a path between those two nodes
+		/// }
+		/// </code>
+		///
+		/// Equivalent to calling <see cref="IsPathPossible(List<GraphNode>)"/> with a list containing node1 and node2.
+		///
+		/// See: graph-updates (view in online documentation for working links)
+		/// See: <see cref="AstarPath.GetNearest"/>
+		/// See: <see cref="Pathfinding.HierarchicalGraph"/>
+		/// </summary>
+		public static bool IsPathPossible (GraphNode node1, GraphNode node2) {
+			return node1.Walkable && node2.Walkable && node1.Area == node2.Area;
+		}
+
+		/// <summary>
+		/// Returns if there are walkable paths between all nodes in the list.
+		///
+		/// Returns true for empty lists.
+		///
+		/// See: graph-updates (view in online documentation for working links)
+		/// See: <see cref="AstarPath.GetNearest"/>
+		/// </summary>
+		public static bool IsPathPossible (List<GraphNode> nodes) {
+			if (nodes.Count == 0) return true;
+
+			uint area = nodes[0].Area;
+			for (int i = 0; i < nodes.Count; i++) if (!nodes[i].Walkable || nodes[i].Area != area) return false;
+			return true;
+		}
+
+		/// <summary>
+		/// Returns if there are walkable paths between all nodes in the list.
+		///
+		/// This method will actually only check if the first node can reach all other nodes. However this is
+		/// equivalent in 99% of the cases since almost always the graph connections are bidirectional.
+		/// If you are not aware of any cases where you explicitly create unidirectional connections
+		/// this method can be used without worries.
+		///
+		/// Returns true for empty lists
+		///
+		/// Warning: This method is significantly slower than the IsPathPossible method which does not take a tagMask
+		///
+		/// See: graph-updates (view in online documentation for working links)
+		/// See: <see cref="AstarPath.GetNearest"/>
+		/// </summary>
+		public static bool IsPathPossible (List<GraphNode> nodes, int tagMask) {
+			if (nodes.Count == 0) return true;
+
+			// Make sure that the first node has a valid tag
+			if (((tagMask >> (int)nodes[0].Tag) & 1) == 0) return false;
+
+			// Fast check first
+			if (!IsPathPossible(nodes)) return false;
+
+			// Make sure that the first node can reach all other nodes
+			var reachable = GetReachableNodes(nodes[0], tagMask);
+			bool result = true;
+
+			// Make sure that the first node can reach all other nodes
+			for (int i = 1; i < nodes.Count; i++) {
+				if (!reachable.Contains(nodes[i])) {
+					result = false;
+					break;
+				}
+			}
+
+			// Pool the temporary list
+			ListPool<GraphNode>.Release(ref reachable);
+
+			return result;
+		}
+
+		/// <summary>
+		/// Returns all nodes reachable from the seed node.
+		/// This function performs a DFS (depth-first-search) or flood fill of the graph and returns all nodes which can be reached from
+		/// the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node.
+		/// In the editor areas are displayed as different colors of the nodes.
+		/// The only case where it will not be so is when there is a one way path from some part of the area to the seed node
+		/// but no path from the seed node to that part of the graph.
+		///
+		/// The returned list is not sorted in any particular way.
+		///
+		/// Depending on the number of reachable nodes, this function can take quite some time to calculate
+		/// so don't use it too often or it might affect the framerate of your game.
+		///
+		/// See: bitmasks (view in online documentation for working links).
+		///
+		/// Returns: A List<Node> containing all nodes reachable from the seed node.
+		/// For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool.
+		/// </summary>
+		/// <param name="seed">The node to start the search from.</param>
+		/// <param name="tagMask">Optional mask for tags. This is a bitmask.</param>
+		/// <param name="filter">Optional filter for which nodes to search. You can combine this with tagMask = -1 to make the filter determine everything.
+		///      Only walkable nodes are searched regardless of the filter. If the filter function returns false the node will be treated as unwalkable.</param>
+		public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1, System.Func<GraphNode, bool> filter = null) {
+			Stack<GraphNode> dfsStack = StackPool<GraphNode>.Claim();
+			List<GraphNode> reachable = ListPool<GraphNode>.Claim();
+
+			/// <summary>TODO: Pool</summary>
+			var map = new HashSet<GraphNode>();
+
+			System.Action<GraphNode> callback;
+			// Check if we can use the fast path
+			if (tagMask == -1 && filter == null) {
+				callback = (GraphNode node) => {
+					if (node.Walkable && map.Add(node)) {
+						reachable.Add(node);
+						dfsStack.Push(node);
+					}
+				};
+			} else {
+				callback = (GraphNode node) => {
+					if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add(node)) {
+						if (filter != null && !filter(node)) return;
+
+						reachable.Add(node);
+						dfsStack.Push(node);
+					}
+				};
+			}
+
+			callback(seed);
+
+			while (dfsStack.Count > 0) {
+				dfsStack.Pop().GetConnections(callback);
+			}
+
+			StackPool<GraphNode>.Release(dfsStack);
+			return reachable;
+		}
+
+		static Queue<GraphNode> BFSQueue;
+		static Dictionary<GraphNode, int> BFSMap;
+
+		/// <summary>
+		/// Returns all nodes up to a given node-distance from the seed node.
+		/// This function performs a BFS (<a href="https://en.wikipedia.org/wiki/Breadth-first_search">breadth-first search</a>) or flood fill of the graph and returns all nodes within a specified node distance which can be reached from
+		/// the seed node. In almost all cases when depth is large enough this will be identical to returning all nodes which have the same area as the seed node.
+		/// In the editor areas are displayed as different colors of the nodes.
+		/// The only case where it will not be so is when there is a one way path from some part of the area to the seed node
+		/// but no path from the seed node to that part of the graph.
+		///
+		/// The returned list is sorted by node distance from the seed node
+		/// i.e distance is measured in the number of nodes the shortest path from seed to that node would pass through.
+		/// Note that the distance measurement does not take heuristics, penalties or tag penalties.
+		///
+		/// Depending on the number of nodes, this function can take quite some time to calculate
+		/// so don't use it too often or it might affect the framerate of your game.
+		///
+		/// Returns: A List<GraphNode> containing all nodes reachable up to a specified node distance from the seed node.
+		/// For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool
+		///
+		/// Warning: This method is not thread safe. Only use it from the Unity thread (i.e normal game code).
+		///
+		/// The video below shows the BFS result with varying values of depth. Points are sampled on the nodes using <see cref="GetPointsOnNodes"/>.
+		/// [Open online documentation to see videos]
+		///
+		/// <code>
+		/// var seed = AstarPath.active.GetNearest(transform.position, NNConstraint.Walkable).node;
+		/// var nodes = PathUtilities.BFS(seed, 10);
+		/// foreach (var node in nodes) {
+		///     Debug.DrawRay((Vector3)node.position, Vector3.up, Color.red, 10);
+		/// }
+		/// </code>
+		/// </summary>
+		/// <param name="seed">The node to start the search from.</param>
+		/// <param name="depth">The maximum node-distance from the seed node.</param>
+		/// <param name="tagMask">Optional mask for tags. This is a bitmask.</param>
+		/// <param name="filter">Optional filter for which nodes to search. You can combine this with depth = int.MaxValue and tagMask = -1 to make the filter determine everything.
+		///      Only walkable nodes are searched regardless of the filter. If the filter function returns false the node will be treated as unwalkable.</param>
+		public static List<GraphNode> BFS (GraphNode seed, int depth, int tagMask = -1, System.Func<GraphNode, bool> filter = null) {
+			BFSQueue = BFSQueue ?? new Queue<GraphNode>();
+			var que = BFSQueue;
+
+			BFSMap = BFSMap ?? new Dictionary<GraphNode, int>();
+			var map = BFSMap;
+
+			// Even though we clear at the end of this function, it is good to
+			// do it here as well in case the previous invocation of the method
+			// threw an exception for some reason
+			// and didn't clear the que and map
+			que.Clear();
+			map.Clear();
+
+			List<GraphNode> result = ListPool<GraphNode>.Claim();
+
+			int currentDist = -1;
+			System.Action<GraphNode> callback;
+			if (tagMask == -1) {
+				callback = node => {
+					if (node.Walkable && !map.ContainsKey(node)) {
+						if (filter != null && !filter(node)) return;
+
+						map.Add(node, currentDist+1);
+						result.Add(node);
+						que.Enqueue(node);
+					}
+				};
+			} else {
+				callback = node => {
+					if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && !map.ContainsKey(node)) {
+						if (filter != null && !filter(node)) return;
+
+						map.Add(node, currentDist+1);
+						result.Add(node);
+						que.Enqueue(node);
+					}
+				};
+			}
+
+			callback(seed);
+
+			while (que.Count > 0) {
+				GraphNode n = que.Dequeue();
+				currentDist = map[n];
+
+				if (currentDist >= depth) break;
+
+				n.GetConnections(callback);
+			}
+
+			que.Clear();
+			map.Clear();
+			return result;
+		}
+
+		/// <summary>
+		/// Returns points in a spiral centered around the origin with a minimum clearance from other points.
+		/// The points are laid out on the involute of a circle
+		/// See: http://en.wikipedia.org/wiki/Involute
+		/// Which has some nice properties.
+		/// All points are separated by clearance world units.
+		/// This method is O(n), yes if you read the code you will see a binary search, but that binary search
+		/// has an upper bound on the number of steps, so it does not yield a log factor.
+		///
+		/// Note: Consider recycling the list after usage to reduce allocations.
+		/// See: Pathfinding.Util.ListPool
+		/// </summary>
+		public static List<Vector3> GetSpiralPoints (int count, float clearance) {
+			List<Vector3> pts = ListPool<Vector3>.Claim(count);
+
+			// The radius of the smaller circle used for generating the involute of a circle
+			// Calculated from the separation distance between the turns
+			float a = clearance/(2*Mathf.PI);
+			float t = 0;
+
+
+			pts.Add(InvoluteOfCircle(a, t));
+
+			for (int i = 0; i < count; i++) {
+				Vector3 prev = pts[pts.Count-1];
+
+				// d = -t0/2 + sqrt( t0^2/4 + 2d/a )
+				// Minimum angle (radians) which would create an arc distance greater than clearance
+				float d = -t/2 + Mathf.Sqrt(t*t/4 + 2*clearance/a);
+
+				// Binary search for separating this point and the previous one
+				float mn = t + d;
+				float mx = t + 2*d;
+				while (mx - mn > 0.01f) {
+					float mid = (mn + mx)/2;
+					Vector3 p = InvoluteOfCircle(a, mid);
+					if ((p - prev).sqrMagnitude < clearance*clearance) {
+						mn = mid;
+					} else {
+						mx = mid;
+					}
+				}
+
+				pts.Add(InvoluteOfCircle(a, mx));
+				t = mx;
+			}
+
+			return pts;
+		}
+
+		/// <summary>
+		/// Returns the XZ coordinate of the involute of circle.
+		/// See: http://en.wikipedia.org/wiki/Involute
+		/// </summary>
+		private static Vector3 InvoluteOfCircle (float a, float t) {
+			return new Vector3(a*(Mathf.Cos(t) + t*Mathf.Sin(t)), 0, a*(Mathf.Sin(t) - t*Mathf.Cos(t)));
+		}
+
+		/// <summary>
+		/// Will calculate a number of points around p which are on the graph and are separated by clearance from each other.
+		/// This is like GetPointsAroundPoint except that previousPoints are treated as being in world space.
+		/// The average of the points will be found and then that will be treated as the group center.
+		/// </summary>
+		/// <param name="p">The point to generate points around</param>
+		/// <param name="g">The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
+		/// Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.</param>
+		/// <param name="previousPoints">The points to use for reference. Note that these are in world space.
+		///      The new points will overwrite the existing points in the list. The result will be in world space.</param>
+		/// <param name="radius">The final points will be at most this distance from p.</param>
+		/// <param name="clearanceRadius">The points will if possible be at least this distance from each other.</param>
+		public static void GetPointsAroundPointWorld (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
+			if (previousPoints.Count == 0) return;
+
+			Vector3 avg = Vector3.zero;
+			for (int i = 0; i < previousPoints.Count; i++) avg += previousPoints[i];
+			avg /= previousPoints.Count;
+
+			for (int i = 0; i < previousPoints.Count; i++) previousPoints[i] -= avg;
+
+			GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius);
+		}
+
+		/// <summary>
+		/// Will calculate a number of points around center which are on the graph and are separated by clearance from each other.
+		/// The maximum distance from center to any point will be radius.
+		/// Points will first be tried to be laid out as previousPoints and if that fails, random points will be selected.
+		/// This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
+		/// this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
+		/// some kind of local avoidance.
+		///
+		/// TODO: Write unit tests
+		/// </summary>
+		/// <param name="center">The point to generate points around</param>
+		/// <param name="g">The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
+		/// Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.</param>
+		/// <param name="previousPoints">The points to use for reference. Note that these should not be in world space. They are treated as relative to center.
+		///      The new points will overwrite the existing points in the list. The result will be in world space, not relative to center.</param>
+		/// <param name="radius">The final points will be at most this distance from center.</param>
+		/// <param name="clearanceRadius">The points will if possible be at least this distance from each other.</param>
+		public static void GetPointsAroundPoint (Vector3 center, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
+			if (g == null) throw new System.ArgumentNullException("g");
+
+			var graph = g as NavGraph;
+
+			if (graph == null) throw new System.ArgumentException("g is not a NavGraph");
+
+			var nn = graph.GetNearest(center, NNConstraint.Walkable);
+			center = nn.position;
+
+			if (nn.node == null) {
+				// No valid point to start from
+				return;
+			}
+
+
+			// Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
+			radius = Mathf.Max(radius, 1.4142f*clearanceRadius*Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
+			clearanceRadius *= clearanceRadius;
+
+			for (int i = 0; i < previousPoints.Count; i++) {
+				Vector3 dir = previousPoints[i];
+				float magn = dir.magnitude;
+
+				if (magn > 0) dir /= magn;
+
+				float newMagn = radius;//magn > radius ? radius : magn;
+				dir *= newMagn;
+
+				GraphHitInfo hit;
+
+				int tests = 0;
+				while (true) {
+					Vector3 pt = center + dir;
+
+					if (g.Linecast(center, pt, out hit)) {
+						if (hit.point == Vector3.zero) {
+							// Oops, linecast actually failed completely
+							// try again unless we have tried lots of times
+							// then we just continue anyway
+							tests++;
+							if (tests > 8) {
+								previousPoints[i] = pt;
+								break;
+							}
+						} else {
+							pt = hit.point;
+						}
+					}
+
+					bool worked = false;
+
+					for (float q = 0.1f; q <= 1.0f; q += 0.05f) {
+						Vector3 qt = Vector3.Lerp(center, pt, q);
+						worked = true;
+						for (int j = 0; j < i; j++) {
+							if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius) {
+								worked = false;
+								break;
+							}
+						}
+
+						// Abort after 8 tests or when we have found a valid point
+						if (worked || tests > 8) {
+							worked = true;
+							previousPoints[i] = qt;
+							break;
+						}
+					}
+
+					// Break out of nested loop
+					if (worked) {
+						break;
+					}
+
+					// If we could not find a valid point, reduce the clearance radius slightly to improve
+					// the chances next time
+					clearanceRadius *= 0.9f;
+					// This will pick points in 2D closer to the edge of the circle with a higher probability
+					dir = UnityEngine.Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5);
+					dir.y = 0;
+					tests++;
+				}
+			}
+		}
+
+		[BurstCompile(FloatMode = FloatMode.Fast)]
+		struct JobFormationPacked : IJob {
+			public NativeArray<float3> positions;
+			public float3 destination;
+			public float agentRadius;
+			public NativeMovementPlane movementPlane;
+
+			public float CollisionTime (float2 pos1, float2 pos2, float2 v1, float2 v2, float r1, float r2) {
+				var relativeVelocity = v1 - v2;
+
+				if (math.all(relativeVelocity == float2.zero)) {
+					// No collision
+					return float.MaxValue;
+				}
+				var radius = r1 + r2;
+				var relativePos = pos2 - pos1;
+				var relativeDir = math.normalize(relativeVelocity);
+				var d1 = math.dot(relativePos, relativeDir);
+				var d2sq = math.lengthsq(relativePos - relativeDir * d1);
+				var offsetSq = radius*radius - d2sq;
+				if (offsetSq <= 0) {
+					// No collision
+					return float.MaxValue;
+				}
+				var offset = math.sqrt(offsetSq);
+				var collisionDistance = d1 - offset;
+				if (collisionDistance < -radius) {
+					// No collision (collision is in the imagined past)
+					return float.MaxValue;
+				}
+				return collisionDistance * math.rsqrt(math.lengthsq(relativeVelocity));
+				//return collisionDistance / math.length(relativeVelocity);
+			}
+
+			struct DistanceComparer : IComparer<int> {
+				public NativeArray<float2> positions;
+
+				public int Compare (int x, int y) {
+					return (int)math.sign(math.lengthsq(positions[x]) - math.lengthsq(positions[y]));
+				}
+			}
+
+			public void Execute () {
+				if (positions.Length == 0) return;
+
+				NativeArray<float2> positions2D = new NativeArray<float2>(positions.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+				NativeArray<int> indices = new NativeArray<int>(positions.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+				for (int i = 0; i < positions.Length; i++) {
+					positions2D[i] =  movementPlane.ToPlane(positions[i]);
+					indices[i] = i;
+				}
+
+				float2 mean = float2.zero;
+				for (int i = 0; i < positions2D.Length; i++) {
+					mean += positions2D[i];
+				}
+				mean /= positions2D.Length;
+
+				for (int i = 0; i < positions2D.Length; i++) {
+					positions2D[i] -= mean;
+				}
+
+				// Sort agents by their distance to the center
+				indices.Sort(new DistanceComparer { positions = positions2D });
+
+				NativeArray<float> minTimes = new NativeArray<float>(positions.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+				for (int a = 0; a < positions.Length; a++) {
+					var ta = float.MaxValue;
+					var ia = indices[a];
+					for (int b = 0; b < a; b++) {
+						var ib = indices[b];
+						//float tb = CollisionTime(positions2D[ia], positions2D[ib], -positions2D[ia], -positions2D[ib], agentRadius, agentRadius);
+						float tb = CollisionTime(positions2D[ia], positions2D[ib], -positions2D[ia], float2.zero, agentRadius, agentRadius);
+						ta = math.min(ta, tb);
+					}
+					minTimes[ia] = ta;
+					positions2D[ia] -= positions2D[ia] * math.min(1.0f, minTimes[indices[a]]);
+				}
+
+				for (int i = 0; i < positions.Length; i++) {
+					positions[i] = movementPlane.ToWorld(positions2D[i]) + destination;
+				}
+			}
+		}
+
+		public static void FormationPacked (List<Vector3> currentPositions, Vector3 destination, float clearanceRadius, NativeMovementPlane movementPlane) {
+			var positions = new NativeArray<float3>(currentPositions.Count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory);
+
+			for (int i = 0; i < positions.Length; i++) positions[i] = currentPositions[i];
+			new JobFormationPacked {
+				positions = positions,
+				destination = destination,
+				agentRadius = clearanceRadius,
+				movementPlane = movementPlane,
+			}.Schedule().Complete();
+			for (int i = 0; i < positions.Length; i++) currentPositions[i] = positions[i];
+			positions.Dispose();
+		}
+
+		public enum FormationMode {
+			SinglePoint,
+			Packed,
+		}
+
+		public static List<Vector3> FormationDestinations (List<IAstarAI> group, Vector3 destination, FormationMode formationMode, float marginFactor = 0.1f) {
+			if (group.Count == 0) return new List<Vector3>();
+
+			var positions = group.Select(u => u.position).ToList();
+
+			if (formationMode == FormationMode.SinglePoint) {
+				for (int i = 0; i < positions.Count; i++) positions[i] = destination;
+			} else {
+				var previousMean = Vector3.zero;
+				for (int i = 0; i < positions.Count; i++) previousMean += positions[i];
+				previousMean /= positions.Count;
+
+				// Assume the whole group uses the same movement plane, or at least a similar one
+				var movementPlane = group[0].movementPlane;
+				Debug.Log(((Quaternion)movementPlane.rotation).eulerAngles);
+
+				// Figure out if the group if the destination point is in the middle of the group,
+				// or if it is outside the group
+				var standardDeviation = Mathf.Sqrt(positions.Average(p => Vector3.SqrMagnitude(p - previousMean)));
+				var thresholdDistance = standardDeviation*1.0f;
+
+				if (Vector3.Distance(destination, previousMean) > thresholdDistance) {
+					// If the destination is outside of the group, use a packed formation
+					Pathfinding.PathUtilities.FormationPacked(positions, destination, group[0].radius * (1 + marginFactor), movementPlane);
+				} else {
+					// If the destination is inside the group, move all agents to the same point
+					for (int i = 0; i < positions.Count; i++) positions[i] = destination;
+				}
+			}
+
+			return positions;
+		}
+
+		class ConstrainToSet : NNConstraint {
+			public HashSet<GraphNode> nodes;
+
+			public override bool Suitable (GraphNode node) {
+				return nodes.Contains(node);
+			}
+		}
+
+		public static void GetPointsAroundPointWorldFlexible (Vector3 center, Quaternion rotation, List<Vector3> positions) {
+			if (positions.Count == 0) return;
+
+			var snapped = AstarPath.active.GetNearest(center, NNConstraint.Walkable);
+
+			// Move slightly toward the node center just to avoid the group center being on a node edge
+			var groupPos = Vector3.Lerp(snapped.position, (Vector3)snapped.node.position, 0.001f);
+
+			var previousMean = Vector3.zero;
+			for (int i = 0; i < positions.Count; i++) previousMean += positions[i];
+			previousMean /= positions.Count;
+
+			var maxSqrDistance = 0f;
+			for (int i = 0; i < positions.Count; i++) {
+				positions[i] -= previousMean;
+				maxSqrDistance = Mathf.Max(maxSqrDistance, positions[i].sqrMagnitude);
+			}
+
+			// Multiplying by 4 doubles the normal distance
+			maxSqrDistance *= 2*2;
+
+			// Search at least this number of nodes regardless of the distance to the nodes
+			int minNodes = 10;
+
+			var nodes = PathUtilities.BFS(snapped.node, int.MaxValue, -1, node => {
+				minNodes--;
+				return minNodes > 0 || ((Vector3)node.position - groupPos).sqrMagnitude < maxSqrDistance;
+			});
+
+			NNConstraint nn = new ConstrainToSet() {
+				nodes = new HashSet<GraphNode>(nodes)
+			};
+
+			int iterations = 3;
+			for (int k = 0; k < iterations; k++) {
+				float totalWeight = 0f;
+				Vector3 totalSum = Vector3.zero;
+
+				for (int i = 0; i < positions.Count; i++) {
+					var rel = rotation * positions[i];
+					var p = groupPos + rel;
+
+					var near = AstarPath.active.GetNearest(p, nn).position;
+					// TODO: Handle case when no close node was found
+
+					var weight = Vector3.Distance(p, near);
+
+					totalSum += (near - rel) * weight;
+					totalWeight += weight;
+				}
+
+				// If no changes were required, then break early
+				if (totalWeight <= 0.0000001f) break;
+
+				var newCenter = totalSum / totalWeight;
+
+				groupPos = AstarPath.active.GetNearest(newCenter, nn).position;
+			}
+
+			for (int i = 0; i < positions.Count; i++) {
+				positions[i] = groupPos + rotation * positions[i];
+			}
+		}
+
+		/// <summary>
+		/// Returns randomly selected points on the specified nodes with each point being separated by clearanceRadius from each other.
+		/// Selecting points ON the nodes only works for TriangleMeshNode (used by Recast Graph and Navmesh Graph) and GridNode (used by GridGraph).
+		/// For other node types, only the positions of the nodes will be used.
+		///
+		/// clearanceRadius will be reduced if no valid points can be found.
+		///
+		/// Note: This method assumes that the nodes in the list have the same type for some special cases.
+		/// More specifically if the first node is not a TriangleMeshNode or a GridNode, it will use a fast path
+		/// which assumes that all nodes in the list have the same surface area (which usually is a surface area of zero and the
+		/// nodes are all PointNodes).
+		/// </summary>
+		public static List<Vector3> GetPointsOnNodes (List<GraphNode> nodes, int count, float clearanceRadius = 0) {
+			if (nodes == null) throw new System.ArgumentNullException("nodes");
+			if (nodes.Count == 0) throw new System.ArgumentException("no nodes passed");
+
+			List<Vector3> pts = ListPool<Vector3>.Claim(count);
+
+			// Square
+			clearanceRadius *= clearanceRadius;
+
+			if (clearanceRadius > 0 || nodes[0] is TriangleMeshNode
+#if !ASTAR_NO_GRID_GRAPH
+				|| nodes[0] is GridNode
+#endif
+				) {
+				// Accumulated area of all nodes
+				List<float> accs = ListPool<float>.Claim(nodes.Count);
+
+				// Total area of all nodes so far
+				float tot = 0;
+
+				for (int i = 0; i < nodes.Count; i++) {
+					var surfaceArea = nodes[i].SurfaceArea();
+					// Ensures that even if the nodes have a surface area of 0, a random one will still be picked
+					// instead of e.g always picking the first or the last one.
+					surfaceArea += 0.001f;
+					tot += surfaceArea;
+					accs.Add(tot);
+				}
+
+				for (int i = 0; i < count; i++) {
+					// Pick point
+					int testCount = 0;
+					int testLimit = 10;
+
+					while (true) {
+						bool worked = true;
+
+						// If no valid points could be found, progressively lower the clearance radius until such a point is found
+						if (testCount >= testLimit) {
+							// Note that clearanceRadius is a squared radius
+							clearanceRadius *= 0.9f*0.9f;
+							testLimit += 10;
+							if (testLimit > 100) clearanceRadius = 0;
+						}
+
+						// Pick a random node among the ones in the list weighted by their area
+						float tg = UnityEngine.Random.value*tot;
+						int v = accs.BinarySearch(tg);
+						if (v < 0) v = ~v;
+
+						if (v >= nodes.Count) {
+							// Cover edge cases
+							continue;
+						}
+
+						var node = nodes[v];
+						var p = node.RandomPointOnSurface();
+
+						// Test if it is some distance away from the other points
+						if (clearanceRadius > 0) {
+							for (int j = 0; j < pts.Count; j++) {
+								if ((pts[j]-p).sqrMagnitude < clearanceRadius) {
+									worked = false;
+									break;
+								}
+							}
+						}
+
+						if (worked) {
+							pts.Add(p);
+							break;
+						}
+						testCount++;
+					}
+				}
+
+				ListPool<float>.Release(ref accs);
+			} else {
+				// Fast path, assumes all nodes have the same area (usually zero)
+				for (int i = 0; i < count; i++) {
+					pts.Add((Vector3)nodes[UnityEngine.Random.Range(0, nodes.Count)].RandomPointOnSurface());
+				}
+			}
+
+			return pts;
+		}
+	}
+}