public IList<PointInt32> FindPath(IMover mover, PointInt32 start, PointInt32 target)
{
if (m_Map.IsBlocked(mover, target)) {
return null;
}
m_Nodes.Clear();
m_Nodes[start].Cost = 0f;
m_Nodes[start].Depth = 0;
m_Closed.Clear();
m_Open.Clear();
m_Open.Add(m_Nodes[start]);
m_Nodes[target].Parent = null;
int maxDepth = 0;
while ((maxDepth < m_MaxSearchDistance) && (m_Open.Count != 0)) {
var current = m_Open.First();
if (current == m_Nodes[target]) {
break;
}
m_Open.Remove(current);
m_Closed.Add(current);
for (int x = -1; x < 2; x++) {
for (int y = -1; y < 2; y++) {
if (((x != 0) || (y != 0)) && (m_AllowDiagMovement || ((x == 0) || (y == 0)))) {
var point = new PointInt32(current.Point.X + x, current.Point.Y + y);
if (IsValidLocation(mover, start, point)) {
float nextStepCost = current.Cost + m_Map.GetCost(mover, current.Point, point);
var neighbour = m_Nodes[point];
m_Map.PathfinderCallback(point);
if (nextStepCost < neighbour.Cost) {
if (m_Open.Contains(neighbour)) {
m_Open.Remove(neighbour);
}
if (m_Closed.Contains(neighbour)) {
m_Closed.Remove(neighbour);
}
}
if (!(m_Open.Contains(neighbour) || m_Closed.Contains(neighbour))) {
neighbour.Cost = nextStepCost;
neighbour.Heuristic = m_Heuristic.GetCost(m_Map, mover, point, start, target);
maxDepth = System.Math.Max(maxDepth, neighbour.SetParent(current));
m_Open.Add(neighbour);
}
}
}
}
}
}
if (m_Nodes[target].Parent == null) {
return null;
}
var path = new List<PointInt32>();
for (var targetNode = m_Nodes[target]; targetNode != m_Nodes[start]; targetNode = targetNode.Parent) {
path.Insert(0, targetNode.Point);
}
path.Insert(0, start);
return path;
}
public AStarNode this[PointInt32 point]
{
get {
int index = point.Y * m_Width + point.X;
AStarNode result;
if (!m_Nodes.TryGetValue(index, out result)) {
result = new AStarNode(point);
m_Nodes[index] = result;
}
return result;
}
set {
int index = point.Y * m_Width + point.X;
m_Nodes[index] = value;
}
}
private static void SimpleExample()
{
Console.Clear();
Console.Out.WriteLine("Simple example - empty map.");
WriteLegend();
var map = new MapSample();
Console.Out.Write(map.ToString());
var pathFinder = new AStarPathfinder(map, 1000, false, new ManhattanDistance());
var from = new PointInt32(0, 0);
var to = new PointInt32(map.Width - 1, map.Height - 1);
Console.Out.WriteLine("Path from {0} to {1}.", from, to);
var path = pathFinder.FindPath(null, from, to);
Console.Out.Write(map.ToString(path, true));
Console.Out.WriteLine("Press any key to continue ...");
Console.In.ReadLine();
}
private static void BlocksExample()
{
Console.Clear();
Console.Out.WriteLine("Example with blocks in map.");
WriteLegend();
var map = new MapSample();
for (var y = map.Height / 2; y < map.Height; y++) {
map[map.Width / 3, y].IsBlocker = true;
map[2 * map.Width / 3, y].IsBlocker = true;
}
Console.Out.Write(map.ToString());
var pathFinder = new AStarPathfinder(map, 1000, false, new ManhattanDistance());
var from = new PointInt32(0, 0);
var to = new PointInt32(map.Width - 1, map.Height - 1);
Console.Out.WriteLine("Path from {0} to {1}.", from, to);
var path = pathFinder.FindPath(null, from, to);
Console.Out.Write(map.ToString(path, true));
Console.Out.WriteLine("Press any key to continue ...");
Console.In.ReadLine();
}
public float GetCost(ITileMap map, IMover mover, PointInt32 point, PointInt32 start, PointInt32 target)
{
float dx = target.X - point.X;
float dy = target.Y - point.Y;
return (float)System.Math.Sqrt((double)((dx * dx) + (dy * dy))) * m_Coefficient;
}
private bool IsValidLocation(IMover mover, PointInt32 start, PointInt32 point)
{
bool invalid = (((point.X < 0) || (point.Y < 0)) || (point.X >= m_Map.Width)) || (point.Y >= m_Map.Height);
if (!(invalid || ((start.X == point.X) && (start.Y == point.Y)))) {
invalid = m_Map.IsBlocked(mover, point);
}
return !invalid;
}
public AStarNode(PointInt32 point)
{
this.Point = point;
}
public float GetCost(ITileMap map, IMover mover, PointInt32 point, PointInt32 start, PointInt32 target)
{
return (System.Math.Abs(point.X - target.X) + System.Math.Abs(point.Y - target.Y)) * m_Coefficient;
}
public void PathfinderCallback(PointInt32 point)
{
m_Map[point.X][point.Y].IsVisited = true;
}
public bool IsBlocked(IMover mover, PointInt32 point)
{
return m_Map[point.X][point.Y].IsBlocker;
}
public float GetCost(IMover mover, PointInt32 source, PointInt32 target)
{
return m_Map[target.X][target.Y].Difficulty;
}
public float GetCost(ITileMap map, IMover mover, PointInt32 point, PointInt32 start, PointInt32 target)
{
return System.Math.Max(System.Math.Abs(point.X - target.X), System.Math.Abs(point.Y - target.Y));
}