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