/// <summary> /// 主要算法函数,求最后的所有阶段的路径集合 /// </summary> /// <param name="iTaskIndex"></param> /// <returns></returns> protected MPath BuildPathForSingleUAV(int iTaskIndex) { //算法参数 AStarOriginAlgorithmParameter mPara = AlgoParameter as AStarOriginAlgorithmParameter; #region 定义变量,需要再添加 MPath mPath = new MPath(); //初始化一条路径,最后得到的就是这个类中的一个list存放的路径序列 mPath.Index = AlgoInput.UAVTask[iTaskIndex].Index; //得到当前无人机编号,就是这条路径的编号 mPath.Waypoints = new List <MWaypoint>(); int iWaypointIndex = 0; //所有阶段的点的集合的索引 #endregion #region ----------------------------对每一个阶段规划航路------------------------------------------ for (int iStageIndex = 0; iStageIndex < AlgoInput.UAVTask[iTaskIndex].Stages.Count; iStageIndex++) { List <Node> openlist = null; var FAPath = BuildPathForSingleUAVwithSingleStageInStatic(iTaskIndex, iStageIndex, mPara, out openlist); #region 航迹可视化处理 //为可视化输出保存 Visualization.mPathForVisualizaition = openlist.Where(a => a.sign == true).ToList(); if (!PathPlaningDataVisualization.PathPlaningDataVisualization.AlgoDataDict.ContainsKey(mPath.Index.ToString() + "-" + iStageIndex.ToString())) { //为ResultShowForm分析窗口保存数据 //3.30.2018 刘洋添加 PathPlaningDataVisualization.PathPlaningDataVisualization.AlgoDataDict.Add(mPath.Index.ToString() + "-" + iStageIndex.ToString(), Visualization.MyTreeNodeConverter(Visualization.mPathForVisualizaition)); } else { PathPlaningDataVisualization.PathPlaningDataVisualization.AlgoDataDict[mPath.Index.ToString() + "-" + iStageIndex.ToString()] = Visualization.MyTreeNodeConverter(Visualization.mPathForVisualizaition); } #endregion #region 航迹保存 //添加当前阶段航路到总航路(起始航路点为当前阶段, 此阶段的目标航路点为下一个阶段的起始航路点, 注意总航路的最后一个航路点属于最后一个阶段) for (int k = 0; k < FAPath.Count - 1; k++) { mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, FAPath[k].ConvertTreeNodeToUAVState(), iStageIndex)); iWaypointIndex = iWaypointIndex + 1; } if (iStageIndex == AlgoInput.UAVTask[iTaskIndex].Stages.Count - 1)//如果到了最后一个阶段,不要忘记把最后一个点加进来 { mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, FAPath[FAPath.Count - 1].ConvertTreeNodeToUAVState(), AlgoInput.UAVTask[iTaskIndex].Stages.Count - 1)); } #endregion } #endregion return(mPath); }
private List <Node> BuildPathForSingleUAVwithSingleStageInStatic(int iTaskIndex, int iStageIndex, AStarOriginAlgorithmParameter mPara, out List <Node> openlist) { #region 定义并实例化集合 int toExtendNum = -1; //定义跳转到最小代价点在openList中的位置 bool isReachTarget = false; //是否到达目标 - 初始未到达,循环结束标志 openlist = new List <Node>(); //我觉得可以这样定义Open集和close集,这里是个局部变量 List <Node> APath = new List <Node>(); //按照算法寻找的点,比较多 List <Node> FAPath = new List <Node>(); //最终的路线,把Apath中的一部分不必要的点过滤掉 #endregion #region 在每个阶段都先获取起始点和目标点,并进行处理 var start = AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].StartState.Location; var goal = AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState.Location; Node startnode = new Node(start, goal, 0, null);//初始化开始节点 #endregion /* if (IsSafeLine(start, goal))//如果当前阶段起点与终点连线安全,那么最终的路线就是这俩点啊 * { * FAPath.Add(startnode); * Node finalnode = new Node(start, goal, goal, startnode);//初始化开始节点 * FAPath.Add(finalnode); * }*/ //else // { #region 从Open集中不断找寻代价最低的节点,直到找到目标点为止,并得到A*的路线 openlist.Add(startnode); //将开始节点加到Open表,开始了 while (!isReachTarget) { toExtendNum = FindMin(openlist); if (toExtendNum != -1) //如果找到了最小代价点,即Open集不为空,问题来了,如果Open集为空集怎么办 { openlist[toExtendNum].sign = true; //加到Close集合中 if (IsSafeLine(openlist[toExtendNum].NodeLocation, goal) && FPoint3.DistanceBetweenTwoSpacePointsXY(openlist[toExtendNum].NodeLocation, goal) <= mPara.Step * Math.Sqrt(2)) //如果当前找到的点与目标点连线安全,就可以直接加入目标点了 { Node goalnode = new Node(start, goal, goal, openlist[toExtendNum]); //将goal连接到了Close表中了 isReachTarget = true; APath.Add(goalnode); //加入目标节点 Node temp = new Node(); temp = goalnode.ParentNode; while (temp.ParentNode != null) { APath.Add(temp); Node Anode = new Node(); Anode = temp; temp = Anode.ParentNode; } APath.Add(startnode); //加入开始节点,至此路径中的点找齐了 APath.Reverse(); //将序列翻转变为正序 } else { openlist = FindAroundPoint(openlist[toExtendNum], openlist, start, goal); } } else { APath = new List <Node>(); APath.Add(startnode); Node finalnode = new Node(start, goal, goal, startnode);//初始化开始节点 APath.Add(finalnode); return(APath); } } #endregion #region 计算最终简化A*的路线 if (mPara.NeedPathSimplifed) { int j = 0; int i = APath.Count - 1; FAPath.Add(startnode); bool IsSimplify = true; while (IsSimplify) { if (IsSafeLine(APath[j].NodeLocation, APath[i].NodeLocation)) { if (i == APath.Count - 1) { IsSimplify = false; } FAPath.Add(APath[i]); j = i; i = APath.Count - 1; } else { i--; } } } else { /* int j = 1; * int i = 0; * FAPath.Add(startnode); * bool IsSimplify = true; * while (IsSimplify) * { * if (!IsSafeLine(APath[i].NodeLocation, APath[j].NodeLocation)) * { * FAPath.Add(APath[j-1]); * i= j-1; * } * else * { * if (j == APath.Count - 1) * { * FAPath.Add(APath[APath.Count - 1]); * IsSimplify = false; * } * else * { * j++; * } * } * }*/ FAPath = APath; } #endregion // } return(FAPath); }