private static AStarNode aStarSearch(Location start, Location end, Level level)
{
HashSet<AStarNode> open = new HashSet<AStarNode>();
HashSet<AStarNode> closed = new HashSet<AStarNode>();
AStarNode startNode = new AStarNode(start);
startNode.g = 0;
startNode.h = start.distance(end);
open.Add(startNode);
while (open.Count > 0)
{
AStarNode q = new AStarNode(new Location(0, 0));
double min = 10000;
foreach (AStarNode node in open)
{
if (node.f() < min)
{
min = node.f();
q = node;
}
}
open.Remove(q);
Direction[] directions = {Direction.North, Direction.South, Direction.East, Direction.West};
for (int i = 0; i < 4; i++)
{
Location adj = q.location.getAdjLocation(directions[i]);
if (level.getTile(adj).blocksMovement)
continue;
AStarNode node = new AStarNode(adj);
if (closed.Contains(node))
continue;
node.setParent(q);
if (adj.Equals(end))
return node;
node.g = q.g + 1;
node.h = adj.distance(end);
bool add = true;
foreach (AStarNode openNode in open)
{
if (openNode.location.Equals(node.location) && openNode.f() < node.f())
add = false;
}
foreach (AStarNode closedNode in closed)
{
if (closedNode.location.Equals(node.location) && closedNode.f() < node.f())
add = false;
}
if (add)
{
open.Add(node);
}
}
closed.Add(q);
}
return startNode;
}
/*!!! PATHFINDING ALGORITHM IS HIDDEN HERE !!!*/
public LinkedList<GridSquare> findPath(Vector3 pathFrom,Vector3 pathTo)
{
AStarNode[] aStarNodes = new AStarNode[width*height];
bool[] closedSet = new bool[width*height];
PriorityQueue<GridSquare> openSet = new PriorityQueue<GridSquare>(false);
GridSquare target = this.gridSquareFromVector3(pathTo);
GridSquare currentSquare;
AStarNode current;
//Initialize with path beginning
AStarNode temp = new AStarNode(this.gridSquareFromVector3(pathFrom),target);
aStarNodes[temp.getSquare().getHash()] = temp;
openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
//While there's still items in the open set
while ((currentSquare = openSet.dequeue()) != null) {
//openSet stores gridsquares for efficiency reasons, so get the relevant A* node
current = aStarNodes[currentSquare.getHash()];
//Add node to the closed set
closedSet[current.getSquare().getHash()] = true;
//If the current square is the target, we have found a path and
//can return
if (current.getSquare () == target) {
break;
}
//For every neighbor
foreach (GridSquare s in current.getNeighbors())
{
//If the square is already processed, skip it
if (closedSet[s.getHash()] == true) {
continue;
}
//This is why the open set stores GridSquares instead of AStarNodes
if (!openSet.Contains(s)) {
temp = new AStarNode(s,target);
aStarNodes[temp.getSquare().getHash()] = temp;
//setParent sets the g score automatically.
temp.setParent(current);
openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
} else {
//if this is a worse path, skip it.
temp = aStarNodes[s.getHash ()];
if (current.gScore + 1 >= temp.gScore) {
continue;
}
//setParent sets the g score automatically.
temp.setParent(current);
//Re add to the open set.
openSet.Remove (temp.getSquare());
openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
}
}
}
//No path was found
if (currentSquare == null) {
return default(LinkedList<GridSquare>);
}
//Reconstruct the path
LinkedList<GridSquare> path = new LinkedList<GridSquare>();
current = aStarNodes[target.getHash()];
if (current == null) {
return null;
}
do {
//Add the current square to the beginning of the path
path.AddFirst(current.getSquare());
//Set the current node to the parent
current = current.getParent ();
} while (current != null);
return path;
}