//A* implementation public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action<Vector2[], bool> callback){ #if DEBUG Stopwatch sw = new Stopwatch(); sw.Start(); #endif var openList = new Heap<PathNode>(allNodes.Length); var closedList = new HashSet<PathNode>(); var success = false; openList.Add(startNode); while (openList.Count > 0){ var currentNode = openList.RemoveFirst(); closedList.Add(currentNode); if (currentNode == endNode){ #if DEBUG sw.Stop(); //UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms."); #endif success = true; break; } foreach (var neighbour in currentNode.links.Select( index => allNodes[index] )){ if (closedList.Contains(neighbour)) continue; var costToNeighbour = currentNode.gCost + GetDistance( currentNode, neighbour ); if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour) ){ neighbour.gCost = costToNeighbour; neighbour.hCost = GetDistance(neighbour, endNode); neighbour.parent = currentNode; if (!openList.Contains(neighbour)){ openList.Add(neighbour); openList.UpdateItem(neighbour); } } } } yield return null; if (success){ callback( RetracePath(startNode, endNode), true ); } else { callback( new Vector2[0], false ); } }
private static Vector2[] RetracePath(PathNode startNode, PathNode endNode){ var path = new List<Vector2>(); var currentNode = endNode; while(currentNode != startNode){ path.Add(currentNode.pos); currentNode = currentNode.parent; } path.Add(startNode.pos); path.Reverse(); return path.ToArray() ; }
private static Vector2[] RetracePath(PathNode startNode, PathNode endNode) { var path = new List <Vector2>(); var currentNode = endNode; while (currentNode != startNode) { path.Add(currentNode.pos); currentNode = currentNode.parent; } path.Add(startNode.pos); path.Reverse(); return(path.ToArray()); }
public int Compare(System.Object a, System.Object b) { PathNode nodeA = a as PathNode; PathNode nodeB = b as PathNode; if (nodeA.estimatedCost < nodeB.estimatedCost) { return(-1); } else if (nodeA.estimatedCost > nodeB.estimatedCost) { return(1); } else { return(0); } }
private static float GetDistance(PathNode a, PathNode b){ return (a.pos - b.pos).magnitude; }
//A* implementation public static IEnumerator CalculatePath(PathNode start, PathNode end, PathNode[] allNodes, System.Action <List <Vector2> > callback) { int n = 0; PriorityQueue openList = new PriorityQueue(); PriorityQueue closedList = new PriorityQueue(); openList.Push(start); start.cost = 0; start.estimatedCost = HeuristicEstimate(start, end, heuristicWeight); PathNode currentNode = null; while (openList.Count != 0) { currentNode = openList.Front(); if (currentNode == end) { break; } List <int> links = currentNode.links; for (int i = 0; i != links.Count; i++) { var endNode = allNodes[links[i]]; float incrementalCost = GetCost(currentNode, endNode); float endNodeCost = currentNode.cost + incrementalCost; if (closedList.Contains(endNode)) { if (endNode.cost <= endNodeCost) { continue; } closedList.Remove(endNode); } else if (openList.Contains(endNode)) { if (endNode.cost <= endNodeCost) { continue; } } float endNodeHeuristic = HeuristicEstimate(endNode, end, heuristicWeight); endNode.cost = endNodeCost; endNode.parent = currentNode; endNode.estimatedCost = endNodeCost + endNodeHeuristic; if (!openList.Contains(endNode)) { openList.Push(endNode); } } closedList.Push(currentNode); openList.Remove(currentNode); n++; if (n > 300) { yield return(null); } } if (!currentNode.Equals(end)) { // Debug.LogWarning("No path found :("); callback(new List <Vector2>()); yield break; } else { var path = new List <Vector2>(); while (currentNode != null) { path.Add(currentNode.pos); currentNode = currentNode.parent; } path.Reverse(); callback(path); yield break; } }
public void Remove(PathNode node) { nodes.Remove(node); nodes.Sort(); }
public bool Contains(PathNode node) { return(nodes.Contains(node)); }
public int Push(PathNode node) { nodes.Add(node); nodes.Sort(); return(nodes.Count); }
private static float GetCost(PathNode nodeA, PathNode nodeB) { return((nodeA.pos - nodeB.pos).magnitude); }
private static float HeuristicEstimate(PathNode currentNode, PathNode endNode, float heuristicWeight) { return((currentNode.pos - endNode.pos).magnitude * heuristicWeight); }
private static float GetDistance(PathNode a, PathNode b) { return((a.pos - b.pos).magnitude); }
//A* implementation public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action <Vector2[], bool> callback) { #if DEBUG Stopwatch sw = new Stopwatch(); sw.Start(); #endif var openList = new Heap <PathNode>(allNodes.Length); var closedList = new HashSet <PathNode>(); var success = false; openList.Add(startNode); while (openList.Count > 0) { var currentNode = openList.RemoveFirst(); closedList.Add(currentNode); if (currentNode == endNode) { #if DEBUG sw.Stop(); UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms."); #endif success = true; break; } foreach (var neighbour in currentNode.links.Select(index => allNodes[index])) { if (closedList.Contains(neighbour)) { continue; } var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour)) { neighbour.gCost = costToNeighbour; neighbour.hCost = GetDistance(neighbour, endNode); neighbour.parent = currentNode; if (!openList.Contains(neighbour)) { openList.Add(neighbour); openList.UpdateItem(neighbour); } } } } yield return(null); if (success) { callback(RetracePath(startNode, endNode), true); } else { callback(new Vector2[0], false); } }
//A* implementation public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, List <PathNode> allNodes, Action <Vector2[]> callback) { var sw = new Stopwatch(); sw.Start(); var openList = new Heap <PathNode>(allNodes.Count); var closedList = new HashSet <PathNode>(); var success = false; openList.Add(startNode); while (openList.Count > 0) { var currentNode = openList.RemoveFirst(); closedList.Add(currentNode); if (currentNode == endNode) { sw.Stop(); success = true; break; } var linkIndeces = currentNode.links; for (var i = 0; i < linkIndeces.Count; i++) { var neighbour = allNodes[linkIndeces[i]]; if (closedList.Contains(neighbour)) { continue; } var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour)) { neighbour.gCost = costToNeighbour; neighbour.hCost = GetDistance(neighbour, endNode); neighbour.parent = currentNode; if (!openList.Contains(neighbour)) { openList.Add(neighbour); openList.UpdateItem(neighbour); } } } if (sw.ElapsedMilliseconds > 30) { yield return(null); } } yield return(null); if (success) { callback(RetracePath(startNode, endNode)); } else { callback(null); } }
private static Vector2[] Internal_CalculatePath(PathNode startNode, PathNode endNode, List <PathNode> allNodes) { var openList = new Heap <PathNode>(allNodes.Count); var closedList = new HashSet <PathNode>(); var success = false; openList.Add(startNode); while (openList.Count > 0) { var currentNode = openList.RemoveFirst(); if (currentNode == endNode) { success = true; break; } closedList.Add(currentNode); var linkIndeces = currentNode.links; for (var i = 0; i < linkIndeces.Count; i++) { var neighbour = allNodes[linkIndeces[i]]; if (closedList.Contains(neighbour)) { continue; } var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour)) { neighbour.gCost = costToNeighbour; neighbour.hCost = GetDistance(neighbour, endNode); neighbour.parent = currentNode; if (!openList.Contains(neighbour)) { openList.Add(neighbour); openList.UpdateItem(neighbour); } } } } if (success) //Retrace Path if one exists { var path = new List <Vector2>(); var currentNode = endNode; while (currentNode != startNode) { path.Add(currentNode.pos); currentNode = currentNode.parent; } path.Add(startNode.pos); path.Reverse(); return(path.ToArray()); } return(null); }
//A* implementation public static void CalculatePath(PathNode startNode, PathNode endNode, List <PathNode> allNodes, Action <Vector2[]> callback) { var path = Internal_CalculatePath(startNode, endNode, allNodes); callback(path); }