IEnumerator Search(Vector3 beginning, Vector3 end) { HeapArray <State> openSet = new HeapArray <State>(gridMap.LargestSize); State goal = gridMap.RetrieveState(end); State start = gridMap.RetrieveState(beginning); openSet.Enqueue(start); HashSet <State> cameFrom = new HashSet <State>(); Vector3[] intermediates = new Vector3[0]; bool foundPath = false; while (openSet.Count > 0) { State present = openSet.Dequeue(); cameFrom.Add(present); if (present == goal) { foundPath = true; intermediates = ReconstructPath(start, goal); break; } else { List <State> adjacents = gridMap.RetrieveAdjacentStates(present); foreach (State s in adjacents) { if (cameFrom.Contains(s) || !(s.unblocked)) { continue; } else { int yWeight = Mathf.Abs(present.yCoordinate - s.yCoordinate); int xWeight = Mathf.Abs(present.xCoordinate - s.xCoordinate); int edgeWeight = 0; if (xWeight < yWeight) { edgeWeight = 10 * (yWeight - xWeight) + 14 * xWeight; } else { edgeWeight = 10 * (xWeight - yWeight) + 14 * yWeight; } int tentativeCost = present.gOfN + edgeWeight; if (!(openSet.Contains(s)) || tentativeCost < s.gOfN) { s.parent = present; s.gOfN = tentativeCost; yWeight = Mathf.Abs(s.yCoordinate - goal.yCoordinate); xWeight = Mathf.Abs(s.xCoordinate - goal.xCoordinate); if (xWeight < yWeight) { s.hOfN = 10 * (yWeight - xWeight) + 14 * xWeight; } else { s.hOfN = 10 * (xWeight - yWeight) + 14 * yWeight; } if (!openSet.Contains(s)) { openSet.Enqueue(s); } else { openSet.UpdateState(s); } } } } } } yield return(null); searchHandler.FinishedPresentSearch(intermediates, foundPath); }