public SearchNode(SearchNode parent, int x, int y, int g, int h) { this.Parent = parent; this.X = x; this.Y = y; this.G = g; this.H = h; this.F = g + h; }
public bool updateGScore(SearchNode subject, int cost) { if (subject.G + cost < this.G) { this.G = subject.G + cost; this.F = this.G + this.H; this.Parent = subject; return true; } return false; }
/// returns true if the destination node is found private bool sample(SearchNode node, int x, int y, int cost) { // make sure x,y are in bounds and that the location is not an obstacle if (x >= 0 && x < gridLengthX && y >= 0 && y < gridLengthY && grid[y, x]) { string key = getKey(x, y); // ignore nodes in the closed list if (closedList.ContainsKey(key)) { return false; } // check if the node is already in the openlist else if (openList.ContainsKey(key)) { SearchNode fromOpenList = openList[key]; // update the g-score of the encountered node and resort the list if necessary if (fromOpenList.updateGScore(node, cost)) { sortOpenList(); } return false; } // we had better make a new node now and add it to the lists SearchNode newNode = new SearchNode(node, x, y, node.G + cost, getHeuristicFrom(x, y)); openList.Add(key, newNode); sortedOpenList.Add(newNode); // now sort the sorted list so we keep it like a priority q sortOpenList(); // we may have just created our end node, so build the soltution path if we can if (x == endX && y == endY) { buildSolution(newNode); return true; } } return false; }
public AStarSearch run(int startX, int startY, int endX, int endY) { this.startX = startX; this.endX = endX; this.startY = startY; this.endY = endY; setDefaults(); validateStartAndEndPoints(); SearchNode start = new SearchNode(null, this.startX, this.startY, 0, getHeuristicFrom(this.startX, this.startY)); openList.Add(getKey(this.startX, this.startY), start); sortedOpenList.Add(start); while (openList.Count > 0) { // get lowest F score in the list and add it to the closed list SearchNode current = sortedOpenList.First(); closedList.Add(getKey(current.X, current.Y), current); // remove current from the dictionary and list openList.Remove(getKey(current.X, current.Y)); sortedOpenList.RemoveAt(0); int currX = current.X; int currY = current.Y; int left = currX - 1; int right = currX + 1; int up = currY - 1; int down = currY + 1; if (Math.Abs(currX - this.endX) > Math.Abs(currY - this.endY)) { // sample the nodes around current node in this order // // 3 // 1 x 2 // 4 // // gives direcetion x priority over y when more distance is required in the x direction if (sample(current, left, currY, MOVE_STRAIGHT) || sample(current, right, currY, MOVE_STRAIGHT) || sample(current, currX, up, MOVE_STRAIGHT) || sample(current, currX, down, MOVE_STRAIGHT)) { break; } } else { // sample the nodes around current node in this order // // 1 // 3 x 4 // 2 // // gives direcetion y priority over x when more distance is required in the y direction if (sample(current, currX, up, MOVE_STRAIGHT) || sample(current, currX, down, MOVE_STRAIGHT) || sample(current, left, currY, MOVE_STRAIGHT) || sample(current, right, currY, MOVE_STRAIGHT)) { break; } } // If diagonals are allowed, sample the nodes around current node in this order // // 1 2 // x // 3 4 // // Diagonals are not allowed to cut across boundary corners, thus the complicated if statements. Example: // // x s Invalid // x x e Cannot get from s to e with down-right diagonal because there is a boundary collision // // x s Valid // x e Can get from s to e with down-right diagonal because there is no boundary nested at the diagonal if (allowDiagonalPath) { if (left >= 0 && grid[currY, left] && up >= 0 && grid[up, currX] && sample(current, left, up, MOVE_DIAGONAL)) { break; } if (right < gridLengthX && grid[currY, right] && up >= 0 && grid[up, currX] && sample(current, right, up, MOVE_DIAGONAL)) { break; } if (left >= 0 && grid[currY, left] && down < gridLengthY && grid[down, currX] && sample(current, left, down, MOVE_DIAGONAL)) { break; } if (right < gridLengthX && grid[currY, right] && down < gridLengthY && grid[down, currX] && sample(current, right, down, MOVE_DIAGONAL)) { break; } } } return this; }
private void buildSolution(SearchNode endNode) { Solution = new LinkedList<CoordinateDto>(); Solution.AddFirst(new CoordinateDto(endNode.X, endNode.Y, gridLengthX, gridLengthY)); SearchNode current = endNode; while (current.Parent != null) { current = current.Parent; Solution.AddFirst(new CoordinateDto(current.X, current.Y, gridLengthX, gridLengthY)); } }