public Vector2[] BuildPath(Vector2 endPoint, PathfindingSearchNode node) { int length = 1; PathfindingSearchNode current = node; while (current != null) { ++length; current = current.backNode; } Vector2[] result = new Vector2[length]; int index = length - 1; result[index] = endPoint; current = node; while (current != null) { --index; result[index] = current.startPoint; current = current.backNode; } return(result); }
public Vector2[] PathTo(Vector2 end) { PathfindingNode endNode = grid.CellAt(end); while (nextNodes.Count > 0) { PathfindingSearchNode next = ExpandNext(); if (next != null && next.node == endNode) { return(BuildPath(end, next)); } } return(null); }
public Vector2[] PathToNearest(IEnumerable <Vector2> ends) { Dictionary <PathfindingNode, List <Vector2> > endNodeMapping = new Dictionary <PathfindingNode, List <Vector2> >(); foreach (Vector2 end in ends) { PathfindingNode endNode = grid.CellAt(end); if (!endNodeMapping.ContainsKey(endNode)) { endNodeMapping[endNode] = new List <Vector2>(); } endNodeMapping[endNode].Add(end); } while (nextNodes.Count > 0) { PathfindingSearchNode next = ExpandNext(); if (next != null && endNodeMapping.ContainsKey(next.node)) { Vector2 end = endNodeMapping[next.node].Aggregate((shortest, nextTest) => { if (Distance(nextTest, next.startPoint) < Distance(shortest, next.startPoint)) { return(nextTest); } else { return(shortest); } }); return(BuildPath(end, next)); } } return(null); }
public PathfindingSearchNode ExpandNext() { PathfindingSearchNode next = nextNodes.Dequeue(); if (expandedNodes.Contains(next.node)) { return(null); } expandedNodes.Add(next.node); foreach (PathfindingEdge edge in next.node.GetAdjacentNodes()) { if (!expandedNodes.Contains(edge.to)) { Vector2 nextPoint = NearestPoint(edge.edge, next.startPoint); float distance = Distance(nextPoint, next.startPoint); nextNodes.Enqueue(new PathfindingSearchNode(edge.to, nextPoint, next, next.currentDistance + distance)); } } return(next); }