using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Diagnostics;
namespace AStar
{
class PathFinder
{
public delegate bool IsValid(Point position);
public static Path Find(Point from, Point to, IsValid isValid)
{
if (isValid == null || isValid(from) == false || isValid(to) == false)
return null;
Path path = new Path(from, to);
while (path.OpenTable.Count != 0)
{
Node best = path.OpenTable.Pop();
path.CloseTable.Add(best);
Debug.Assert(best != null);
Console.WriteLine("Process " + best);
if (best.IsSamePoint(path.To))
{
path.CloseTable.Add(best);
return path;
}
Node[] nodes = best.Expand(to);
foreach (Node n in nodes)
{
if (isValid(n.Position) == false)
continue;
if (path.CloseTable.Contains(n))
continue;
Node o = path.OpenTable.GetAlmostNode(n);
if (o != null)
{
if (n.HasCost < o.HasCost)
path.OpenTable.Push(n);
continue;
}
path.OpenTable.Push(n);
}
}
return null;
}
}
class Path
{
private Point from = Point.Empty;
public Point From
{
get
{
return from;
}
}
private Point to = Point.Empty;
public Point To
{
get
{
return to;
}
}
private OpenTable openTable = new OpenTable();
public OpenTable OpenTable
{
get
{
return openTable;
}
}
private CloseTable closeTable = new CloseTable();
public CloseTable CloseTable
{
get
{
return closeTable;
}
}
public Path(Point from, Point to)
{
this.from = from;
this.to = to;
this.OpenTable.Push(new Node(from, to));
}
public void Reset()
{
from = Point.Empty;
to = Point.Empty;
OpenTable.Clear();
CloseTable.Clear();
}
}
class Node : IComparable<Node>, IComparable
{
private static readonly float Sqrt2 = (float)Math.Sqrt(2.0);
private Node parent = null;
public Node Parent
{
get
{
return parent;
}
}
private Point position = Point.Empty;
public Point Position
{
get
{
return position;
}
}
private float willCost = 0.0f;
/// <summary>
/// WillCost to get to this node
/// </summary>
public float WillCost
{
get
{
return willCost;
}
}
private float hasCost = 0.0f;
/// <summary>
/// total const (const + heuristic estimate)
/// </summary>
public float HasCost
{
get
{
return hasCost;
}
}
public Node(Node parent, Point position)
{
this.position = position;
this.parent = parent;
}
public Node(Point from, Point to)
{
this.position = from;
this.willCost = CostBetween(to);
}
public bool IsSamePoint(Node node)
{
if (node == null)
return false;
return this.Position == node.Position;
}
public bool IsSamePoint(Point point)
{
return this.Position == point;
}
public Node[] Expand(Point to)
{
Node[] nodes = new Node[8];
nodes[0] = new Node(this, new Point(Position.X - 1, Position.Y - 1));
nodes[1] = new Node(this, new Point(Position.X - 0, Position.Y - 1));
nodes[2] = new Node(this, new Point(Position.X + 1, Position.Y - 1));
nodes[3] = new Node(this, new Point(Position.X - 1, Position.Y - 0));
nodes[4] = new Node(this, new Point(Position.X + 1, Position.Y - 0));
nodes[5] = new Node(this, new Point(Position.X - 1, Position.Y + 1));
nodes[6] = new Node(this, new Point(Position.X - 0, Position.Y + 1));
nodes[7] = new Node(this, new Point(Position.X + 1, Position.Y + 1));
foreach (Node n in nodes)
{
n.hasCost = this.hasCost + 1;
n.willCost = n.CostBetween(to);
}
return nodes;
}
public float CostBetween(Node other)
{
if (other == null)
return float.MaxValue;
return CostBetween(other.Position);
}
public float CostBetween(Point position)
{
return CostBetween(this.Position, position);
}
public static float CostBetween(Point from, Point to)
{
float w = Math.Abs(from.X - to.X);
float h = Math.Abs(from.Y - to.Y);
return Math.Max(w, h);
}
public static float CostBetween(int fromX, int formY, int toX, int toY)
{
return CostBetween(new Point(fromX, formY), new Point(toX, toY));
}
#region IComparable<Node> 成员
public int CompareTo(Node other)
{
if (other == null)
return 1;
return this.WillCost.CompareTo(other.WillCost);
}
#endregion
#region IComparable 成员
public int CompareTo(object obj)
{
return CompareTo(obj as Node);
}
#endregion
public override string ToString()
{
return string.Format("{0}, WillCost={1}, HasCost={2}", this.Position, this.WillCost, this.HasCost);
}
public static long GetKey(Point point)
{
return (((long)point.X) << 32) | (uint)point.Y;
}
public long GetKey()
{
return GetKey(this.Position);
}
}
class OpenTable
{
private SortedList<Node, long> data = new SortedList<Node, long>();
public int Count
{
get
{
return data.Count;
}
}
public bool Contains(Node node)
{
if (node == null)
return false;
return data.ContainsValue(node.GetKey());
}
public int IndexOf(Node node)
{
if (node == null)
return -1;
return data.IndexOfValue(node.GetKey());
}
public Node this[int index]
{
get
{
if (index >= 0 && index < Count)
return data.Keys[index];
else
return null;
}
}
public void Clear()
{
data.Clear();
}
public Node Pop()
{
if (Count != 0)
{
Node node = data.Keys[0];
data.RemoveAt(0);
return node;
}
else
return null;
}
public void Push(Node node)
{
int index = IndexOf(node);
if (index < 0)
data[node] = node.GetKey();
else
data.Keys[index] = node;
}
public Node GetAlmostNode(Node node)
{
if (node == null)
return null;
return this[IndexOf(node)];
}
}
class CloseTable : IEnumerable<Node>
{
private SortedList<long, Node> data = new SortedList<long, Node>();
private Node lastNode = null;
public Node LastNode
{
get
{
return lastNode;
}
}
public int Count
{
get
{
return data.Count;
}
}
public void Clear()
{
data.Clear();
lastNode = null;
}
public bool Add(Node node)
{
if (node == null)
return false;
lastNode = node;
if (Contains(node))
return false;
data[node.GetKey()] = node;
return true;
}
public bool Contains(Node node)
{
if (node == null)
return false;
return data.Keys.Contains(node.GetKey());
}
public List<Point> ToPathPointArray()
{
List<Point> path = new List<Point>();
if (Count != 0)
{
Node n = lastNode;
while (n != null)
{
path.Add(n.Position);
n = n.Parent;
}
}
path.Reverse();
return path;
}
#region IEnumerable<Node> 成员
public IEnumerator<Node> GetEnumerator()
{
return data.Values.GetEnumerator();
}
#endregion
#region IEnumerable 成员
System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
{
return this.GetEnumerator();
}
#endregion
}
}
评论