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 currenNode = startNode; //从起始节点开始进行递归调用 return DoPlan(routePlanData, currenNode); }
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 currenNode = startNode; //从起始节点开始进行递归调用 return(DoPlan(routePlanData, currenNode)); }
/// <summary> /// GetNodeOnLocation 目标位置location是否已存在于开放列表或关闭列表中 /// </summary> 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); }
private IList<Point> DoPlan(RoutePlanData routePlanData, AStarNode currenNode) { IList<CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections(); foreach (CompassDirections direction in allCompassDirections) { Point nextCell = GeometryHelper.GetAdjacentPoint(currenNode.Location, direction); if (!routePlanData.CellMap.Contains(nextCell)) //相邻点已经在地图之外 { continue; } if (this.obstacles[nextCell.X][nextCell.Y]) //下一个Cell为障碍物 { continue; } int costG = this.costGetter.GetCost(currenNode.Location, direction); int costH = Math.Abs(nextCell.X - routePlanData.Destination.X) + Math.Abs(nextCell.Y - routePlanData.Destination.Y); if (costH == 0) //costH为0,表示相邻点就是目的点,规划完成,构造结果路径 { IList<Point> route = new List<Point>(); route.Add(routePlanData.Destination); route.Insert(0, currenNode.Location); AStarNode tempNode = currenNode; 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.ResetPreviousNode(currenNode, costG); } } else { AStarNode newNode = new AStarNode(nextCell, currenNode, costG, costH); routePlanData.OpenedList.Add(newNode); } } //将已遍历过的节点从开放列表转移到关闭列表 routePlanData.OpenedList.Remove(currenNode); routePlanData.ClosedList.Add(currenNode); AStarNode minCostNode = this.GetMinCostNode(routePlanData.OpenedList); if (minCostNode == null) //表明从起点到终点之间没有任何通路。 { return null; } //对开放列表中的下一个代价最小的节点作递归调用 return this.DoPlan(routePlanData, minCostNode); }
/// <summary> /// GetNodeOnLocation 目标位置location是否已存在于开放列表或关闭列表中 /// </summary> 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; }
private IList <Point> DoPlan(RoutePlanData routePlanData, AStarNode currenNode) { IList <CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections(); foreach (CompassDirections direction in allCompassDirections) { Point nextCell = GeometryHelper.GetAdjacentPoint(currenNode.Location, direction); if (!routePlanData.CellMap.Contains(nextCell)) //相邻点已经在地图之外 { continue; } if (this.obstacles[nextCell.X][nextCell.Y]) //下一个Cell为障碍物 { continue; } int costG = this.costGetter.GetCost(currenNode.Location, direction); int costH = Math.Abs(nextCell.X - routePlanData.Destination.X) + Math.Abs(nextCell.Y - routePlanData.Destination.Y); if (costH == 0) //costH为0,表示相邻点就是目的点,规划完成,构造结果路径 { IList <Point> route = new List <Point>(); route.Add(routePlanData.Destination); route.Insert(0, currenNode.Location); AStarNode tempNode = currenNode; 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.ResetPreviousNode(currenNode, costG); } } else { AStarNode newNode = new AStarNode(nextCell, currenNode, costG, costH); routePlanData.OpenedList.Add(newNode); } } //将已遍历过的节点从开放列表转移到关闭列表 routePlanData.OpenedList.Remove(currenNode); routePlanData.ClosedList.Add(currenNode); AStarNode minCostNode = this.GetMinCostNode(routePlanData.OpenedList); if (minCostNode == null) //表明从起点到终点之间没有任何通路。 { return(null); } //对开放列表中的下一个代价最小的节点作递归调用 return(this.DoPlan(routePlanData, minCostNode)); }