/// <summary>
/// Score = DistanceFromStartPoint (CostFromStart) + DistanceToEndPoint (Heuristic)
/// </summary>
public static List<Point> FindPath(Map GameMap, Point StartPoint, Point EndPoint)
{
if (StartPoint == EndPoint)
{
return new List<Point>();
}
List<Point> ExploredPoints = new List<Point>();
List<Node> ClosedList = new List<Node>();
List<Node> OpenList = new List<Node>();
List<Node> ConclusionNodeList = new List<Node>();
// Initialize StartPoint to be first Node
int DistanceFromStartPoint = GameMap.DistanceOfPoints(StartPoint, StartPoint);
int DistanceToEndPoint = GameMap.DistanceOfPoints(StartPoint, EndPoint);
Node CurrentNode = new Node(StartPoint, DistanceFromStartPoint, DistanceToEndPoint);
ExploredPoints.Add(CurrentNode.GridPosition);
int RunTime = 0;
do
{
List<Point> AdjacentGridPositions = GetAdjacentGridPositions(CurrentNode);
List<Node> AdjacentNodes = ConvertPointListToNodeList(AdjacentGridPositions, GameMap, StartPoint, EndPoint);
// Add Unexplored and Walkable Nodes into OpenList
// Also set non-Walkable Nodes to Explored to prevent re-exploring
foreach (Node AdjacentNode in AdjacentNodes)
{
if (!ExploredPoints.Contains(AdjacentNode.GridPosition))
{
ExploredPoints.Add(AdjacentNode.GridPosition);
AdjacentNode.SetParent(CurrentNode);
if (GameMap.CheckTileWalkable(AdjacentNode.GridPosition))
{
OpenList.Add(AdjacentNode);
}
}
}
// Sort by Node Score and Heuristic (Lowest first)
OpenList = OpenList.OrderBy(Obj => Obj.Score).ThenBy(Obj => Obj.Heuristic).ToList();
OpenList = RandomizeSameScoreNodes(OpenList);
// Debug
if (false)
{
Console.WriteLine();
Console.WriteLine("----- " + RunTime);
Console.WriteLine("CurrentNode: " + CurrentNode.GridPosition + " S" + CurrentNode.Score + " H" + CurrentNode.Heuristic);
Console.Write("ClosedList: ");
foreach (Node node in ClosedList)
{
Console.Write(node.GridPosition + " S" + node.Score + " H" + node.Heuristic + " - ");
}
Console.Write("\n");
Console.Write("OpenList: ");
foreach (Node node in OpenList)
{
Console.Write(node.GridPosition + " S" + node.Score + " H" + node.Heuristic + " - ");
}
Console.Write("\n");
Console.Write("Conclusion: ");
foreach (Node node in ConclusionNodeList)
{
Console.Write(node.GridPosition + " S" + node.Score + " H" + node.Heuristic + " - ");
}
Console.WriteLine();
}
// Add element to ClosedList
// Take first lowest element from sorted OpenList
// Remove element from OpenList
ClosedList.Add(CurrentNode);
CurrentNode = OpenList[0];
OpenList.RemoveAt(0);
} while (CurrentNode.Heuristic > 0);
// Get resulting path
// Cycle from end node to first node
while (CurrentNode.CostFromStart > 0)
{
ConclusionNodeList.Add(CurrentNode);
CurrentNode = CurrentNode.Parent;
}
// Tile Coloring Debug
if (false)
{
GameMap.Reset();
foreach (Node node in OpenList)
{
GameMap.UpdateTileType(node.GridPosition, GameSetting.TileType.Brown);
}
foreach (Node node in ClosedList)
{
GameMap.UpdateTileType(node.GridPosition, GameSetting.TileType.Purple);
}
foreach (Node node in ConclusionNodeList)
{
GameMap.UpdateTileType(node.GridPosition, GameSetting.TileType.Yellow);
}
}
List<Point> ConclusionPointList = new List<Point>();
foreach (Node ConclusionNode in ConclusionNodeList)
{
ConclusionPointList.Insert(0, ConclusionNode.GridPosition);
}
return ConclusionPointList;
}
private static List<Node> ConvertPointListToNodeList(List<Point> PointList, Map GameMap, Point StartPoint, Point EndPoint)
{
List<Node> NodeList = new List<Node>();
foreach (Point SinglePoint in PointList)
{
int DistanceFromStartPoint = GameMap.DistanceOfPoints(StartPoint, SinglePoint);
int DistanceToEndPoint = GameMap.DistanceOfPoints(SinglePoint, EndPoint);
NodeList.Add(new Node(SinglePoint, DistanceFromStartPoint, DistanceToEndPoint));
}
return NodeList;
}