/** * <summary> * Search the nodes from start to the goal * and get the paths * </summary> * * <param name="start">Node start</param> * <param name="goal">Node goal</param> * * <returns> * List<Node> paths * </returns> */ public List <Node> Search(Node start, Node goal) { // Paths List <Node> paths = new List <Node>(); // Priority queue BinaryMinHeap queue = new BinaryMinHeap(); queue.Insert(start); //start.SetCameFrom(null); start.SetGScore(0); start.SetFScore(this.Hueristic(start, goal)); // Filter out empty nodes List <Node> filtered = queue.GetNodes().FindAll((node) => { return(node != null); }); while (filtered.Count > 0) { Node current = queue.Extract(); // If the goal is found, get and return reconstructed paths if (current.GetID() == goal.GetID()) { return(paths = this.ReconstructPath(current)); } for (int i = 0; i < current.GetNeighbours().Count; i++) { Node neighbour = current.GetNeighbours()[i]; // The cost of moving to the next neighbour float tentativeGScore = current.GetGScore() + this.MovementCost(current, neighbour); // if the new gScore is less than the neighbour gScore if (tentativeGScore < neighbour.GetGScore()) { // Set neighbour cameFrom to the current neighbour.SetCameFrom(current); // Set the neighbour gScore to the lower gScore neighbour.SetGScore(tentativeGScore); // Set the new FScore neighbour.SetFScore(neighbour.GetGScore() + this.Hueristic(neighbour, goal)); bool exist = false; // Is the neighbour in the open set or priority queue for (int n = 0; n < queue.GetNodes().Count; n++) { Node node = queue.GetNodes()[n]; if (node != null && node.GetID() == neighbour.GetID()) { exist = true; break; } } if (!exist) { queue.Insert(neighbour); } } } } return(paths); }