/// <summary> /// 使用A*算法依据当前信息计算一条最短的路径。 /// 注意,如果目标点在警戒线以内,会返回一条并非如你期望的路径。 /// 所以请自行实现目标点无法到达时的处理逻辑。 /// </summary> /// <param name="curPos"></param> /// <param name="aimPos"></param> /// <returns></returns> public NaviPoint[] CalPathUseAStar(Vector2 curPos, Vector2 aimPos) { #region 制导航图 GraphPoint <NaviPoint>[] map = new GraphPoint <NaviPoint> [Map.Length + 2]; GraphPoint <NaviPoint>[] temp = GraphPoint <NaviPoint> .DepthCopy(Map); for (int i = 0; i < temp.Length; i++) { map[i] = temp[i]; } #endregion #region 将当前点和目标点加入到导航图中 int prePointSum = temp.Length; GraphPoint <NaviPoint> curNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, curPos), new List <GraphPath <NaviPoint> >()); GraphPoint <NaviPoint> aimNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, aimPos), new List <GraphPath <NaviPoint> >()); AddCurPosToNaviMap(map, curNaviPoint, prePointSum, GuardLines, BorderLines); AddAimPosToNaviMap(map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines); #endregion #region 计算最短路径,使用A*算法 List <NodeAStar> open = new List <NodeAStar>(); List <NodeAStar> close = new List <NodeAStar>(); open.Add(new NodeAStar(curNaviPoint, null)); NodeAStar cur = null; while (open.Count != 0) { cur = open[open.Count - 1]; if (cur.point == aimNaviPoint) { break; } open.RemoveAt(open.Count - 1); close.Add(cur); foreach (GraphPath <NaviPoint> path in cur.point.neighbors) { if (Contains(close, path.neighbor)) { continue; } else { NodeAStar inOpenNode; if (Contains(open, path.neighbor, out inOpenNode)) { float G = cur.G + path.weight; if (inOpenNode.G > G) { inOpenNode.G = G; inOpenNode.F = G + inOpenNode.H; } } else { NodeAStar childNode = new NodeAStar(path.neighbor, cur); childNode.G = cur.G + path.weight; childNode.H = Vector2.Distance(aimPos, childNode.point.value.Pos); childNode.F = childNode.G + childNode.H; SortInsert(open, childNode); } } } } //if (cur == null) // return null; Stack <NodeAStar> cahe = new Stack <NodeAStar>(); while (cur.father != null) { cahe.Push(cur); cur = cur.father; } NaviPoint[] result = new NaviPoint[cahe.Count]; int j = 0; foreach (NodeAStar node in cahe) { result[j] = node.point.value; j++; } return(result); #endregion }
/// <summary> /// 使用A*算法依据当前信息计算一条最短的路径。 /// 注意,如果目标点在警戒线以内,会返回一条并非如你期望的路径。 /// 所以请自行实现目标点无法到达时的处理逻辑。 /// </summary> /// <param name="curPos"></param> /// <param name="aimPos"></param> /// <returns></returns> public NaviPoint[] CalPathUseAStar ( Vector2 curPos, Vector2 aimPos ) { #region 复制导航图 GraphPoint<NaviPoint>[] map = new GraphPoint<NaviPoint>[Map.Length + 2]; GraphPoint<NaviPoint>[] temp = GraphPoint<NaviPoint>.DepthCopy( Map ); for (int i = 0; i < temp.Length; i++) { map[i] = temp[i]; } #endregion #region 将当前点和目标点加入到导航图中 int prePointSum = temp.Length; GraphPoint<NaviPoint> curNaviPoint = new GraphPoint<NaviPoint>( new NaviPoint( null, -1, curPos ), new List<GraphPath<NaviPoint>>() ); GraphPoint<NaviPoint> aimNaviPoint = new GraphPoint<NaviPoint>( new NaviPoint( null, -1, aimPos ), new List<GraphPath<NaviPoint>>() ); AddCurPosToNaviMap( map, curNaviPoint, prePointSum, GuardLines, BorderLines ); AddAimPosToNaviMap( map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines ); #endregion #region 计算最短路径,使用A*算法 List<NodeAStar> open = new List<NodeAStar>(); List<NodeAStar> close = new List<NodeAStar>(); open.Add( new NodeAStar( curNaviPoint, null ) ); NodeAStar cur = null; while (open.Count != 0) { cur = open[open.Count - 1]; if (cur.point == aimNaviPoint) break; open.RemoveAt( open.Count - 1 ); close.Add( cur ); foreach (GraphPath<NaviPoint> path in cur.point.neighbors) { if (Contains( close, path.neighbor )) { continue; } else { NodeAStar inOpenNode; if (Contains( open, path.neighbor, out inOpenNode )) { float G = cur.G + path.weight; if (inOpenNode.G > G) { inOpenNode.G = G; inOpenNode.F = G + inOpenNode.H; } } else { NodeAStar childNode = new NodeAStar( path.neighbor, cur ); childNode.G = cur.G + path.weight; childNode.H = Vector2.Distance( aimPos, childNode.point.value.Pos ); childNode.F = childNode.G + childNode.H; SortInsert( open, childNode ); } } } } //if (cur == null) // return null; Stack<NodeAStar> cahe = new Stack<NodeAStar>(); while (cur.father != null) { cahe.Push( cur ); cur = cur.father; } NaviPoint[] result = new NaviPoint[cahe.Count]; int j = 0; foreach (NodeAStar node in cahe) { result[j] = node.point.value; j++; } return result; #endregion }