public Path SearchAStar(NavGraph graph, Vector3 source, Vector3 target) { Stopwatch sw = new Stopwatch(); sw.Start(); BinaryHeap <Node> open = new BinaryHeap <Node>(graph.Width * graph.Depth); HashSet <Node> closed = new HashSet <Node>(); Node start = graph.GetNode(source); if (!start.walkable) { start = graph.FindNearestWalkable(start); } Node goal = graph.GetNode(target); if (!goal.walkable) { goal = graph.FindNearestWalkable(goal); } if (start == null || goal == null) { return(Path.Empty); } // F cost is used to prioritize open.Insert(start, (int)start.f); while (open.Count > 0) { Node current = open.Extract(); if (current == goal) { sw.Stop(); UnityEngine.Debug.Log("[A*]: Found path! Time: " + sw.ElapsedMilliseconds + "ms"); return(ReconstructPath(start, goal)); } closed.Add(current); // Evaluate neighbours foreach (Node neighbour in current.GetNeighbours()) { if (closed.Contains(neighbour)) { continue; } // Calculate cost from source to the neighbour float tentative_g = current.g + CalculateHeuristic(current, neighbour); // If neighbour is already in open, and its not a better path, it does not need to be saved if (open.Contains(neighbour) && tentative_g >= neighbour.g) { continue; } // Save this neighbours, as its a better path neighbour.parentNode = current; neighbour.g = tentative_g; neighbour.h = CalculateHeuristic(neighbour, goal); if (!open.Contains(neighbour)) { open.Insert(neighbour, (int)neighbour.f); } // Because f is now changed, it should be resorted in the heap. else { open.HeapifyUp(neighbour); } } } UnityEngine.Debug.LogError("[A*]: Couldnt find a path"); return(Path.Empty); }