private IList <Point> DoPlan(RoutePlanData routePlanData, AStarNode currentNode)
{
//Console.WriteLine(currentNode.ToString());
IList <CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections();
foreach (CompassDirections direction in allCompassDirections)
{
Point nextCell = GeometryHelper.GetAdjacentPoint(currentNode.Location, direction);
if (!routePlanData.CellMap.Contains(nextCell))
{
continue;
}
if (this.obstacles[nextCell.X][nextCell.Y])
{
continue;
}
int costG = this.costGetter.GetCost(currentNode.Location, direction);
int costH = Math.Abs(nextCell.X - routePlanData.Destination.X) + Math.Abs(nextCell.Y - routePlanData.Destination.Y);
if (costH == 0)
{
IList <Point> route = new List <Point>();
route.Add(routePlanData.Destination);
route.Insert(0, currentNode.Location);
AStarNode tempNode = currentNode;
while (tempNode.PreviousNode != null)
{
route.Insert(0, tempNode.PreviousNode.Location);
tempNode = tempNode.PreviousNode;
}
return(route);
}
AStarNode existNode = this.GetNodeOnLocation(nextCell, routePlanData);
if (existNode != null)
{
if (existNode.CostG > costG)
{
existNode.ResetPreviosNode(currentNode, costG);
}
}
else
{
AStarNode newNode = new AStarNode(nextCell, currentNode, costG, costH);
routePlanData.OpenedList.Add(newNode);
}
}
routePlanData.OpenedList.Remove(currentNode);
routePlanData.ClosedList.Add(currentNode);
AStarNode minCostNode = this.GetMinCostNode(routePlanData.OpenedList);
if (minCostNode == null)
{
return(null);
}
return(this.DoPlan(routePlanData, minCostNode));
}
public IList <Point> Plan(Point start, Point destination)
{
Rectangle map = new Rectangle(0, 0, this.columnCount, this.lineCount);
if ((!map.Contains(start)) || (!map.Contains(destination)))
{
throw new Exception("StartPoint or Destination not in the current map!");
}
RoutePlanData routePlanData = new RoutePlanData(map, destination);
AStarNode startNode = new AStarNode(start, null, 0, 0);
routePlanData.OpenedList.Add(startNode);
AStarNode currentNode = startNode;
return(DoPlan(routePlanData, currentNode));
}
private AStarNode GetNodeOnLocation(Point location, RoutePlanData routePlanData)
{
foreach (AStarNode temp in routePlanData.OpenedList)
{
if (temp.Location == location)
{
return(temp);
}
}
foreach (AStarNode temp in routePlanData.ClosedList)
{
if (temp.Location == location)
{
return(temp);
}
}
return(null);
}
