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));
}
}