Have you ever heard an algorithms called A* path finding. This is a algorithms which is widely used in AI,game and graph related fields. Peter Hart, Nils Nilsson, and Bertram Raphael first described the algorithm in 1968. It is an extension of Edsger Dijkstra’s 1959 algorithm. A* achieves better performance (with respect to time) by using heuristics. If you want to know more detail about this algorithm, you can refer thewikipedia page.
In this algorithm, the essential is heuristic.
A* uses a best-first search and finds the least-cost path from a given initial node to onegoal node (out of one or more possible goals).
It uses a distance-plus-cost heuristic function (usually denoted f(x)) to determine the order in which the search visits nodes in the tree. The distance-plus-cost heuristic is a sum of two functions:
- the path-cost function, which is the cost from the starting node to the current node (usually denoted g(x))
- and an admissible “heuristic estimate” of the distance to the goal (usually denoted h(x)).
The h(x) part of the f(x) function must be an admissible heuristic; that is, it must not overestimate the distance to the goal. Thus, for an application like routing, h(x) might represent the straight-line distance to the goal, since that is physically the smallest possible distance between any two points or nodes.
If the heuristic h satisfies the additional condition 
At the beginning of the algorithms, we have a start node , a destination node and a grid. Below is a simple description about the procedure of this algorithms.
- Add the start node to a list, this list contains all nodes that have not been visited,or have not been selected as a path node. The node in this list is visitable. Here, we call this list as Open List. This list is a priority list.You need get the most least F value node from this list. When adding node to the list, you need maintain this list.
- Repeat the procedure below
- Select the node which has the most least F value from the Open List.We call this code is current node
- Move the current node to a separated list, which is called Close List.This list contains all nodes that is path node.
- Deal with 8 nodes around the current node like this:If the node is not in the open list(The node should be visitable), set its parent node is current node, and add it to the open list.If the node is already in the open list, Compare the G value to find a better path.Set current node as its parent node, and re-calculate the G,H and F value.
- Terminate when: The destination node is already in the closed list.In this case, we find a path. Or the open list is empty, the termination node is not in closed list. In this case, there is no path.
- Save the path. We got a path from the termination node to start node.
When I try to learn the A* algorithm, I read some articles, there are some good articles about this topic.
http://www.policyalmanac.org/games/aStarTutorial.htm
http://blogs.msdn.com/b/ericlippert/archive/2007/10/02/path-finding-using-a-in-c-3-0.aspx
My code here is based on the second article.
namespace AStar{
public enum NodeType
{
Start,Barrier,Destination,Plain
};
public class Point
{
public int x, y;
public Point()
{ }
public Point(int _x, int _y)
{
x = _x;
y = _y;
}
}
public class Node
{
public Point Location;
public NodeType nodeType;
//private int gridLenght;
public Node(Point p, NodeType type)
{
Location = p;
nodeType = type;
}
}
public class Path : IEnumerable
{
public Node LastStep { get; private set; }
public Path PreviousSteps { get; private set; }
public double TotalCost { get; private set; }
private Path(Node lastStep, Path previousSteps, double totalCost)
{
LastStep = lastStep;
PreviousSteps = previousSteps;
TotalCost = totalCost;
}
public Path(Node start) : this(start, null, 0) { }
public Path AddStep(Node step, double stepCost)
{
return new Path(step, this, TotalCost + stepCost);
}
public IEnumerator GetEnumerator()
{
for (Path p = this; p != null; p = p.PreviousSteps)
yield return p.LastStep;
}
}
public class PriorityQueue<P,V>
{
private SortedDictionary<P, Queue<V>> list = new SortedDictionary<P, Queue<V>>();
public void Enqueue(P priority, V value)
{
Queue<V> q;
if (!list.TryGetValue(priority, out q))
{
q = new Queue<V>();
list.Add(priority, q);
}
q.Enqueue(value);
}
public V Dequeue()
{
// will throw if there isn’t any first element!
var pair = list.First();
var v = pair.Value.Dequeue();
if (pair.Value.Count == 0) // nothing left of the top priority.
list.Remove(pair.Key);
return v;
}
public bool IsEmpty
{
get { return !list.Any(); }
}
}
public class Grid
{
public Node[,] grid;
public Node StartNode;
public Node DestinationNode;
public Grid() { }
public int gridLength;
public List<Node> GetNeighbours(Node n)
{
int x = n.Location.x;
int y = n.Location.y;
List<Node> neighbours = new List<Node>();
int _x = x - 1;
int _y = y - 1;
for (int i = _x; i < _x + 3; i++)
{
for (int j = _y; j < _y + 3; j++)
{
if (i >= 0 && j >= 0 && i <= gridLength-1 && j <= gridLength-1)
{
if (!(i == x && j == y))
{
if (grid[i, j].nodeType != NodeType.Barrier && grid[i, j].nodeType != NodeType.Start)
{
neighbours.Add(grid[i, j]);
}
}
}
}
}
return neighbours;
}
public Path FindPath(Node start,
Node destination,
Func<Node, Node, double> distance,
Func<Node, double> estimate)
{
var closed = new HashSet<Node>();
var queue = new PriorityQueue<double, Path>();
queue.Enqueue(0, new Path(start));
while (!queue.IsEmpty)
{
var path = queue.Dequeue();
if (closed.Contains(path.LastStep))
continue;
if (path.LastStep.nodeType==NodeType.Destination)
return path;
closed.Add(path.LastStep);
foreach (Node n in GetNeighbours(path.LastStep))
{
if (n.nodeType == NodeType.Plain || n.nodeType == NodeType.Destination)
{
double d = distance(path.LastStep, n);
var newPath = path.AddStep(n, d);
queue.Enqueue(newPath.TotalCost + estimate(n), newPath);
}
}
}
return null;
}
}
}
{
static void Main(string[] args)
{
FileStream fs = new FileStream("data.txt",FileMode.Open);
StreamReader reader = new StreamReader(fs);
string data = reader.ReadLine();
int MaxLength = Convert.ToInt32(data);
Node[,] grid = new Node[MaxLength, MaxLength];
for (int i = 0; i < MaxLength; i++)
{
data = reader.ReadLine();
string []griddata = data.Split(new char[] { ' ' });
for (int j = 0; j < MaxLength; j++)
{
switch(griddata[j])
{
case "#":
{
grid[i, j] = new Node(new Point(i, j), NodeType.Plain);
break;
}
case "$":
{
grid[i, j] = new Node(new Point(i, j), NodeType.Start);
break;
}
case "&":
{
grid[i, j] = new Node(new Point(i, j), NodeType.Barrier);
break;
}
case "*":
{
grid[i, j] = new Node(new Point(i, j), NodeType.Destination);
break;
}
default: break;
}
}
}
reader.Close();
fs.Close();
Grid nodeGrid = new Grid();
nodeGrid.grid = grid;
nodeGrid.gridLength = MaxLength;
for (int i = 0; i < MaxLength; i++)
{
for (int j = 0; j < MaxLength; j++)
{
if(nodeGrid.grid[i,j].nodeType==NodeType.Start)
nodeGrid.StartNode=nodeGrid.grid[i,j];
if(nodeGrid.grid[i,j].nodeType==NodeType.Destination)
nodeGrid.DestinationNode=nodeGrid.grid[i,j];
}
}
Func<Node,Node,double> dist=(s,d)=>{
if(s.Location.x==d.Location.x || s.Location.y==d.Location.y)
return 10;
else
return 14;
};
Func<Node,double> estimate=n=>{
return Math.Abs(nodeGrid.DestinationNode.Location.x-n.Location.x)+
Math.Abs(nodeGrid.DestinationNode.Location.y-n.Location.y);
};
Path path=nodeGrid.FindPath(nodeGrid.StartNode, nodeGrid.DestinationNode, dist, estimate);
foreach (Node n in path)
Console.WriteLine("x:{0},y:{1}", n.Location.x, n.Location.y);
Console.ReadKey();
}
No comments:
Post a Comment