public void AStarSearch(Vector2 sourcePoint, Vector2 destinationPoint, out Vector2 nextNode)
{
//Console.WriteLine("Going from (" + sourcePoint.X + ", " + sourcePoint.Y + ") to (" + destinationPoint.X + ", " + destinationPoint.Y + ")");
Node source = adjacencyList[(int)(columns * sourcePoint.X + sourcePoint.Y)];
Node destination = adjacencyList[(int)(columns * destinationPoint.X + destinationPoint.Y)];
PriorityQueue <Node> Q = new PriorityQueue<Node>();
source.Distance = 0;
Q.Add(source.Priority, source);
while (Q.Count > 0)
{
Node current = Q.RemoveMin();
if (current == destination)
{
Node pathParent = current.Parent;
Stack<Node> path = new Stack<Node>();
path.Push(current);
while (pathParent != null)
{
path.Push(pathParent);
pathParent = pathParent.Parent;
}
Thread t = new Thread(new ParameterizedThreadStart(DrawPath));
t.Start(path);
path.Pop();
nextNode = new Vector2(path.Peek().X * 32, path.Peek().Y * 32);
return;
}
List<Node> allAdjacent = new List<Node>();
allAdjacent.AddRange(current.AdjacentNodes);
allAdjacent.AddRange(current.CornerNodes);
foreach (Node n in allAdjacent)
{
Node node = adjacencyList[columns * n.X + n.Y];
if (node.Distance == int.MaxValue)
{
node.Distance = current.Distance + 1;
node.Parent = current;
node.Visited = true;
node.Priority = (int)(destinationPoint - sourcePoint).Length() + node.Distance;
Q.Add(node.Priority, node);
}
}
}
nextNode = new Vector2(-1);
}
public static Path findPath(Node startNode, Node destination)
{
List<Node> closedList = new List<Node>();
PriorityQueue<Node> openList = new PriorityQueue<Node>();
Dictionary<Node, int> gScore = new Dictionary<Node,int>();
gScore[startNode] = 0;
Dictionary<Node, int> fScore = new Dictionary<Node,int>();
fScore[startNode] = startNode.HeuristicDistance(destination);
Dictionary<Node, Node> prevNode = new Dictionary<Node, Node>();
openList.Add(fScore[startNode], startNode);
while (openList.Count > 0)
{
Node current = openList.RemoveMin();
if (current.Equals(destination))
{
return getPath(prevNode, destination);
}
else
{
closedList.Add(current);
Node[] neighbours = current.GetNeighbours();
foreach (Node next in neighbours)
{
if (closedList.Contains(next))
continue;
int newGScore = gScore[current] + current.distanceBetween(next);
if (!openList.Contains(next) || newGScore < gScore[next])
{
prevNode[next] = current;
gScore[next] = newGScore;
fScore[next] = gScore[next] + next.HeuristicDistance(destination);
if (!openList.Contains(next))
openList.Add(fScore[next], next);
}
}
}
}
return null;
}