private List <Node> FindPathToGoalNode(Node inStartNode, Node inGoalNode) { int expands = 0; HashSet <Node> closedList = new HashSet <Node>(); List <KeyValuePair <float, PathPair> > openList = new List <KeyValuePair <float, PathPair> >(); List <Node> tempList = new List <Node>(); openList.Add( new KeyValuePair <float, PathPair>( Heuristic(inStartNode, inGoalNode), new PathPair(new List <Node> { inStartNode }, inStartNode))); while (openList.Count > 0 && expands < 1000) { float minNodeValue = openList.Min(n => n.Key); var currentNodePair = openList.FirstOrDefault(node => Math.Abs(node.Key - minNodeValue) < 0.001f); var currentNodePathPair = currentNodePair.Value; var currentPath = currentNodePathPair.Key; var currentNode = currentNodePathPair.Value; openList.Remove(currentNodePair); if (currentNode.GetType() == typeof(GoalNode)) { currentPath.Add(currentNode); return(currentPath); } tempList = WorldGrid.GetNeighbors(currentNode); foreach (var node in tempList) { node.Flagged = true; } tempList.Remove(currentPath.Last()); tempList.RemoveAll(node => !node.IsPassable); if (!closedList.Contains(currentNode)) { closedList.Add(currentNode); } foreach (var node in tempList) { expands++; if (node.IsPassable && !closedList.Contains(node)) { List <Node> newList = currentPath.ToList(); newList.Add(currentNode); openList.Add( new KeyValuePair <float, PathPair>( Heuristic(node, inGoalNode),// + newList.Count, new PathPair(newList, node))); } } } Debug.Log("aw crud"); Die(); return(new List <Node> { inStartNode }); }
/* * A* algorithm. Customized to use a heap! */ IEnumerator AStar(Vector2 startPoint, Vector2 endPoint) { // A dictionary containing all the explored nodes so far. I used a dict so search time is constant! Dictionary <Node, bool> explored = new Dictionary <Node, bool>(); // Heap array for our open set. openSet = new NodeHeapArray(); if (visualizer) { closedSet = new List <Node>(); } // Reinatialize all nodes in our grid done foreach (Node node in grid.gridNode) { node.gCost = Mathf.Infinity; node.hCost = Mathf.Infinity; node.parentNode = null; explored[node] = false; } // Player's grid position Vector2 playerGridPos = grid.WorldToGrid(startPoint); // Goal's grid Position Vector2 endPointGridPos = grid.WorldToGrid(endPoint); // Node corresponding to the player's grid position Node playerNode = grid.gridNode[(int)playerGridPos.x, (int)playerGridPos.y]; playerNode.gCost = 0; playerNode.hCost = HeuristicCost(playerGridPos, endPointGridPos); openSet.Push(playerNode); float startTime = Time.time; while (openSet.Count() > 0) { yield return(new WaitForEndOfFrame()); Node current = openSet.Pop(); explored[current] = true; if (visualizer) { closedSet.Add(current); } // check if we found our goal! if (current.point == endPointGridPos) { print("Time elapsed: " + (Time.time - startTime)); finalPath.Clear(); // Clear previous final path int iterationCount = 10000; // Ensures that we don't get stuck in an infinite cycle! Node goal = grid.gridNode[(int)endPointGridPos.x, (int)endPointGridPos.y]; while (goal.parentNode != null && iterationCount > 0) { finalPath.AddLast(goal); goal = goal.parentNode; iterationCount--; } break; } // Check all neighbors foreach (Node node in grid.GetNeighbors(current)) { // New to be gCost. // Addding a bias value to the gCost significantly reduces the number // of Nodes explored during search time! float tentativeGScore = current.gCost + HeuristicCost(node.point, current.point, bias); // If the node has already been explored, or the gCost is higher htan the current node's gCost // There's no point checking this node! if (explored[node] || tentativeGScore >= node.gCost) { continue; } if (!openSet.Contains(node)) { openSet.Push(node); } // Update this node's information! //int nodeIndex = openSet.nodeArray.IndexOf(node); node.parentNode = current; node.gCost = tentativeGScore; node.hCost = HeuristicCost(node.point, endPointGridPos); openSet.BottomTopHeapify(node.index); // re-heapify our open set! } } }