IEnumerator GetPathCoro(Coordinate start, Coordinate end) { List <Coordinate> visited = new List <Coordinate>(); if (Target) { Priority_Queue.SimplePriorityQueue <Coordinate> queue = new Priority_Queue.SimplePriorityQueue <Coordinate>(); if (start == null) { start = prev_start_coord; } if (start != null) { start.parent = null; } if (end == null) { end = prev_end_coord; } if (start == null || end == null) { yield break; } float tstart = Time.realtimeSinceStartup; prev_start_coord = start; prev_end_coord = end; queue.Enqueue(start, GetCoordinateDistFromTarget(start)); while (Path == null) { start = queue.Dequeue(); if (start == end) { /* Debug.Log(Time.realtimeSinceStartup - tstart + " seconds : " + * queue.Count + " end routes considered : " + * start.GetNumParents() + " parents."); */ Path = start; yield break; } if (Time.realtimeSinceStartup > tstart + .01f || index != go_index) { yield return(new WaitForEndOfFrame()); tstart = Time.realtimeSinceStartup; } int safe_layer = ptr.gameObject.layer; foreach (Coordinate coord in start.GetChildren()) { coord.traverse_cost = GetCoordinateDistFromTarget(coord); if (!visited.Contains(coord)) { coord.parent = start; queue.Enqueue(coord, coord.GetTotalCost(safe_layer, can_dodge)); visited.Add(coord); } else if (queue.Contains(coord) && coord.GetTotalCost(safe_layer, can_dodge, 0, start) < queue.GetPriority(coord)) { coord.parent = start; queue.UpdatePriority(coord, coord.GetTotalCost(safe_layer, can_dodge)); } } if (queue.Count != 0) { start = queue.First; } else { break; } } } yield return(null); }
public bool GetPath(Vector2Int start, Vector2Int destination, out Vector2Int[] path) { Priority_Queue.SimplePriorityQueue <Vector2Int> open = new Priority_Queue.SimplePriorityQueue <Vector2Int>(); Dictionary <Vector2Int, bool> closed = new Dictionary <Vector2Int, bool>(); Dictionary <Vector2Int, Step> allSteps = new Dictionary <Vector2Int, Step>(); Step startStep = new Step(start, 0, null); allSteps.Add(startStep.Location, startStep); open.Enqueue(startStep.Location, 0); Step currentStep; Vector2Int currentLocation; Step neighborStep; // While lowest rank in OPEN is not the GOAL: while (open.Count > 0) { // Set current = remove lowest rank item from OPEN currentLocation = open.Dequeue(); if (!allSteps.TryGetValue(currentLocation, out currentStep)) { throw new Exception("Could not find expected step"); } if (currentLocation == destination) { // Reconstruct reverse path from goal to start by following parent pointers path = GetPoints(currentStep); return(true); } // Add current to CLOSED closed.Add(currentLocation, true); // For neighbors of current: foreach (Vector2Int m in movements) { bool inOpen; bool inClosed; int nextCost = currentStep.Cost + 1; Vector2Int nextLocation = currentLocation + m; Step nextStep = new Step(nextLocation, nextCost, currentStep); if (!IsPossibleMove(currentLocation, nextLocation)) { // Spot is bad. continue; } // If neighbor in OPEN and cost less than g(neighbor): // remove neighbor from OPEN, because new path is better if (open.Contains(nextLocation)) { if (!allSteps.TryGetValue(nextLocation, out neighborStep)) { throw new Exception("Could not find expected step"); } if (nextCost < neighborStep.Cost) { open.Remove(nextLocation); allSteps.Remove(nextLocation); inOpen = false; } else { inOpen = true; } } else { inOpen = false; } // If neighbor in CLOSED and cost less than g(neighbor): // remove neighbor from CLOSED if (closed.ContainsKey(nextLocation)) { if (!allSteps.TryGetValue(nextLocation, out neighborStep)) { throw new Exception("Could not find expected step"); } if (nextCost < neighborStep.Cost) { closed.Remove(nextLocation); inClosed = false; } else { inClosed = true; } } else { inClosed = false; } // If neighbor not in OPEN and neighbor not in CLOSED: if (!inOpen && !inClosed) { int h = Heuristic(nextLocation, destination); neighborStep = new Step(nextLocation, nextCost, currentStep); allSteps.Add(nextLocation, neighborStep); open.Enqueue(nextLocation, nextCost + h); } } } path = null; return(false); }