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)); }