/// <summary> /// 计算两个节点之间的距离 /// </summary> /// <param name="mNode1">节点1</param> /// <param name="mNode2">节点2</param> /// <returns></returns> protected double DistanceBetweenTwoNode(RRTNode mNode1, RRTNode mNode2) { //二维算法使用 return(FPoint3.DistanceBetweenTwoSpacePointsXY(mNode1.NodeLocation, mNode2.NodeLocation)); //三维算法使用 //return FPoint3.DistanceBetweenTwoSpacePoints(mNode1.NodeLocation, mNode2.NodeLocation); }
/// <summary> /// 将RRT节点转化为无人机状态 /// </summary> /// <param name="mRRTNode">点的状态</param> /// <returns>树节点</returns> public static SEUAVState ConvertNodeToUAVState(RRTNode mRRTNode) { SEUAVState mUAVState = new SEUAVState(); mUAVState.PointLocation = mRRTNode.NodeLocation; mUAVState.FlightDirection = mRRTNode.NodeDirection; return(mUAVState); }
/// <summary> /// 构造函数 - 将参数传入节点内进行初始化 /// </summary> /// <param name="mX">X坐标</param> /// <param name="mY">Y坐标</param> /// <param name="mZ">Z坐标</param> /// <param name="mParentNode">父节点</param> public RRTNode(double mX, double mY, double mZ, RRTNode mParentNode) { //将参数传入节点内进行初始化 nodeIndex = 0; nodeLocation.X = mX; nodeLocation.Y = mY; nodeLocation.Z = mZ; parentNode = mParentNode; isInRRTPath = 0; isValid = 1; }
/// <summary> /// 构造函数 - 将参数传入节点内进行初始化 /// </summary> /// <param name="mFPoint3">点</param> /// <param name="flightDirection">飞行方向(</param> /// <param name="mParentNode">父节点</param> public RRTNode(FPoint3 mFPoint3, double flightDirection, RRTNode mParentNode) { //将参数传入节点内进行初始化 nodeIndex = 0; nodeLocation.X = mFPoint3.X; nodeLocation.Y = mFPoint3.Y; nodeLocation.Z = mFPoint3.Z; nodeDirection = flightDirection; parentNode = mParentNode; isInRRTPath = 0; isValid = 1; }
/// <summary> /// 在探测范围内随机生成一个节点 /// </summary> /// <param name="iTaskIndex">无人机/任务编号</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="mRRTTree">RRT树</param> /// <returns></returns> protected RRTNode RandomConfiguration(int iTaskIndex, int iStageIndex, List <RRTNode> mRRTTree) { RRTNode mRRTRandNode = new RRTNode(); mRRTRandNode.NodeLocation.X = m_Random.NextDouble() * AlgoInput.Scenario.FlightScene.Coordinate.MaxX; mRRTRandNode.NodeLocation.Y = m_Random.NextDouble() * AlgoInput.Scenario.FlightScene.Coordinate.MaxY; mRRTRandNode.NodeLocation.Z = AlgoInput.Scenario.UAVGroup.GetUAV(AlgoInput.UAVTask[iTaskIndex].Index).StartState.PointLocation.Z; //m_RRTParameter.MissionAltitude; mRRTRandNode.ParentNode = null; // return(mRRTRandNode); }
/// <summary> /// 扩展树 /// </summary> /// <param name="iTaskIndex">无人机/任务编号</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="mRRTTree">RRT树</param> /// <param name="mRRTNearNode">最近节点</param> /// <param name="mRRTNewNode">新生成节点</param> /// <returns>1,是否有效(安全); 2,如果有效(安全),是否到达目标</returns> protected bool ExpandTree(int iTaskIndex, int iStageIndex, ref List <RRTNode> mRRTTree, RRTNode mRRTNearNode, RRTNode mRRTNewNode) { //设置是否到达目标的标志 bool isReachTarget = false; // if (mRRTNewNode != null) { //设置节点编号 mRRTNewNode.NodeIndex = mRRTTree.Count; //设置父节点 mRRTNewNode.ParentNode = mRRTNearNode; //设置是否在路径上 mRRTNewNode.IsInRRTPath = 0; //增加新节点 mRRTTree.Add(mRRTNewNode); //到达目标点 if (DistanceBetweenTwoNode(mRRTNewNode, RRTNode.ConvertUAVStateToNode(AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState)) <= RRTParameter.Error) { //设置到达标记 isReachTarget = true; //此判断防止直接选择目标点作为新节点的时候,重复增加目标点到树中 - liuwei.2011.11.28增加 if (mRRTNewNode.NodeLocation != AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState.Location) { //------------------------将目标点作为最后一个节点加入路径中---------------------// RRTNode mRRTTempNewNode = new RRTNode(); //设置节点编号 mRRTTempNewNode.NodeIndex = mRRTTree.Count; //设置父节点 mRRTTempNewNode.ParentNode = mRRTNewNode; //设置是否在路径上 mRRTTempNewNode.IsInRRTPath = 0; //新节点位置 mRRTTempNewNode.NodeLocation = AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState.Location; //增加新节点 mRRTTree.Add(mRRTTempNewNode); //------------------------将目标点作为最后一个节点加入路径中end------------------// } } else { isReachTarget = false; } } return(isReachTarget); }
/// <summary> /// 计算新的节点 /// </summary> /// <param name="iTaskIndex">无人机任务索引</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="mRRTNearNode">距离mRRTRandNode最近的节点</param> /// <param name="mRRTRandNode">随机生成的节点</param> /// <returns></returns> protected RRTNode NewConfiguration(int iTaskIndex, int iStageIndex, RRTNode mRRTNearNode, RRTNode mRRTRandNode) { RRTNode mRRTNewNode = new RRTNode(); if (DistanceBetweenTwoNode(mRRTNearNode, mRRTRandNode) <= m_RRTParameter.Step) { mRRTNewNode = mRRTRandNode; return(mRRTNewNode); } //生成新点 mRRTNewNode = NewNode(iTaskIndex, iStageIndex, m_RRTParameter.Step, mRRTNearNode, mRRTRandNode); // return(mRRTNewNode); }
/// <summary> /// 计算新的节点 /// </summary> /// <param name="iTaskIndex">无人机任务索引</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="fStep">步长</param> /// <param name="mRRTNearNode">距离mRRTRandNode最近的节点</param> /// <param name="mRRTRandNode">随机生成的节点</param> /// <returns></returns> protected RRTNode NewNode(int iTaskIndex, int iStageIndex, double fStep, RRTNode mRRTNearNode, RRTNode mRRTRandNode) { //liuwei.2012.09.24.修正计算错误 RRTNode mRRTNewNode = new RRTNode(); mRRTNewNode.NodeLocation.X = mRRTNearNode.NodeLocation.X + (fStep * (mRRTNearNode.NodeLocation.X - mRRTRandNode.NodeLocation.X)) / DistanceBetweenTwoNode(mRRTNearNode, mRRTRandNode); mRRTNewNode.NodeLocation.Y = mRRTNearNode.NodeLocation.Y + (fStep * (mRRTNearNode.NodeLocation.Y - mRRTRandNode.NodeLocation.Y)) / DistanceBetweenTwoNode(mRRTNearNode, mRRTRandNode); mRRTNewNode.NodeLocation.Z = AlgoInput.Scenario.UAVGroup.GetUAV(AlgoInput.UAVTask[iTaskIndex].Index).StartState.PointLocation.Z; //m_RRTParameter.MissionAltitude; if (DistanceBetweenTwoNode(mRRTRandNode, mRRTNewNode) >= DistanceBetweenTwoNode(mRRTRandNode, mRRTNearNode)) { mRRTNewNode.NodeLocation.X = mRRTNearNode.NodeLocation.X - (fStep * (mRRTNearNode.NodeLocation.X - mRRTRandNode.NodeLocation.X)) / DistanceBetweenTwoNode(mRRTNearNode, mRRTRandNode); mRRTNewNode.NodeLocation.Y = mRRTNearNode.NodeLocation.Y - (fStep * (mRRTNearNode.NodeLocation.Y - mRRTRandNode.NodeLocation.Y)) / DistanceBetweenTwoNode(mRRTNearNode, mRRTRandNode); mRRTNewNode.NodeLocation.Z = AlgoInput.Scenario.UAVGroup.GetUAV(AlgoInput.UAVTask[iTaskIndex].Index).StartState.PointLocation.Z; //m_RRTParameter.MissionAltitude; } return(mRRTNewNode); }
/// <summary> /// 按照一定概率选择目标点或者随机生成一个新节点 /// </summary> /// <param name="iTaskIndex">无人机/任务编号</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="mRRTTree">RRT树</param> /// <returns>节点</returns> protected RRTNode RandomConfigurationOrTargetConfiguration(int iTaskIndex, int iStageIndex, List <RRTNode> mRRTTree) { //新节点 RRTNode mRRTNode = null; //是否直接把目标点当作新点 if (m_Random.NextDouble() > m_RRTParameter.ChooseTargetThreshold) { mRRTNode = RandomConfiguration(iTaskIndex, iStageIndex, mRRTTree); } else { mRRTNode = RRTNode.ConvertUAVStateToNode(AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState); } // return(mRRTNode); }
/// <summary> /// 计算新的节点 /// </summary> /// <param name="iTaskIndex">无人机任务索引</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="mRRTNearNode">距离mRRTRandNode最近的节点</param> /// <param name="mRRTRandNode">随机生成的节点</param> /// <returns></returns> protected RRTNode NewConfigurationWithRandomStep(int iTaskIndex, int iStageIndex, RRTNode mRRTNearNode, RRTNode mRRTRandNode) { //在指定范围内随机生成同一个步长 double currentSTEP = m_RRTParameter.RandomStepMin + (m_RRTParameter.RandomStepMax - m_RRTParameter.RandomStepMin) * m_Random.NextDouble(); // RRTNode mRRTNewNode = new RRTNode(); if (currentSTEP >= DistanceBetweenTwoNode(mRRTNearNode, mRRTRandNode)) { mRRTNewNode = mRRTRandNode; return(mRRTNewNode); } //生成新点 mRRTNewNode = NewNode(iTaskIndex, iStageIndex, currentSTEP, mRRTNearNode, mRRTRandNode); return(mRRTNewNode); }
/// <summary> /// 寻找树mRRTTree中距离mRRTRandNode最近的节点 /// </summary> /// <param name="mRRTTree">RRT树</param> /// <param name="mRRTRandNode">随机生成的节点</param> /// <returns></returns> protected RRTNode NearestNeighbor(List <RRTNode> mRRTTree, RRTNode mRRTRandNode) { RRTNode mRRTNearNode = new RRTNode(); double fMinDistance = double.MaxValue; double fDistance = 0; foreach (RRTNode mRRTNode in mRRTTree) { fDistance = DistanceBetweenTwoNode(mRRTRandNode, mRRTNode); if (fDistance <= fMinDistance) { mRRTNearNode = mRRTNode; fMinDistance = fDistance; } } // return(mRRTNearNode); }
/// <summary> /// 判断新结点到目标连线是否安全, 若安全则直接连接目标点 /// </summary> /// <param name="iTaskIndex">无人机/任务编号</param> /// <param name="iStageIndex">阶段编号</param> /// <param name="mRRTCurrentNode">当前节点</param> /// <param name="mRRTTree">RRT</param> /// <returns>1,是否有效(安全); 2,如果有效(安全),是否到达目标</returns> protected bool DirectlyToTarget(int iTaskIndex, int iStageIndex, RRTNode mRRTCurrentNode, ref List <RRTNode> mRRTTree) { bool isReachTarget = false; //进一步判断新结点到目标连线是否安全, 若安全则直接连接目标点 if (IsSafeLine(mRRTCurrentNode.NodeLocation, AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState.Location)) { //设置为到达 isReachTarget = true; //设置目标点为最后一个新结点, 并将当前节点设置为她的父节点 mRRTCurrentNode = RRTNode.ConvertUAVStateToNode(AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState, mRRTCurrentNode); //设节点编号 mRRTCurrentNode.NodeIndex = mRRTTree.Count; //设置是否在路径上 mRRTCurrentNode.IsInRRTPath = 0; //增加新节点 mRRTTree.Add(mRRTCurrentNode); } return(isReachTarget); }
/// <summary> /// 构造RRT树上的路径 /// </summary> /// <param name="mRRTTree">从中选择路径的树</param> /// <returns>路径</returns> protected List <RRTNode> BuildPath(List <RRTNode> mRRTTree) { //如果树为空,则返回空 if ((null == mRRTTree)) { return(null); } //选取并存储路径 RRTNode mTempNode = mRRTTree[mRRTTree.Count - 1]; mRRTTree[mRRTTree.Count - 1].IsInRRTPath = 1; List <RRTNode> mTempRRTPath = new List <RRTNode>(); //循环搜索 while (mTempNode.ParentNode != null) { //增加节点 mTempRRTPath.Add(mTempNode); //递增 mTempNode = mTempNode.ParentNode; mRRTTree[mTempNode.NodeIndex].IsInRRTPath = 1; } mTempRRTPath.Add(mTempNode);//实际上是初始节点 //由于是按照倒序添加,这里将顺序反转 mTempRRTPath.Reverse(); //航迹简化 if ((AlgoParameter as RRTParameter).DijkstraOptimizeParameter == true) { WayPointList_Simplify_Method(ref mTempRRTPath); } //返回值 return(mTempRRTPath); }
/// <summary> /// 为单架无人机规划航迹 /// </summary> /// <param name="iTaskIndex">无人机/任务索引</param> /// <returns></returns> protected virtual MPath BuildPathForSingleUAV(int iTaskIndex) { //实例化航路 MPath mPath = new MPath(); mPath.Index = AlgoInput.UAVTask[iTaskIndex].Index; mPath.Waypoints = new List <MWaypoint>(); int iWaypointIndex = 0; //定义树 List <RRTNode> mRRTTree = new List <RRTNode>(); //定义航路 List <RRTNode> mRRTPath = new List <RRTNode>(); //定义树节点 RRTNode mRRTNode = null; //定义树中间节点 RRTNode mRRTRandNode = null; RRTNode mRRTNearNode = null; RRTNode mRRTNewNode = null; //计数器 int nCount = 1; //对每一个阶段规划航路 for (int iStageIndex = 0; iStageIndex < AlgoInput.UAVTask[iTaskIndex].Stages.Count; ++iStageIndex) { //实例化 mRRTTree = new List <RRTNode>(); //加入开始点为第一个点 mRRTNode = new RRTNode(0, AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].StartState.Location, AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].StartState.Direction, null); mRRTTree.Add(mRRTNode); //初始化计数器 nCount = 1; //是否到达目标 - 初始未到达 bool isReachTarget = false; //循环构建新点 - 到达目标时自动退出 while (!isReachTarget) { //-------------------------------随机生成节点--------------------------------// if (NodeSelectionType.Classical == m_RRTParameter.SelectionType) { //方案1:随机生成节点 - 标准RRT采用的方案 mRRTRandNode = RandomConfiguration(iTaskIndex, iStageIndex, mRRTTree); } else //if (NodeSelectionType.Optimzed == m_RRTParameter.SelectionType) { //方案2:按照一定概率随机生成节点或者采用目标点 (改进方案) mRRTRandNode = RandomConfigurationOrTargetConfiguration(iTaskIndex, iStageIndex, mRRTTree); } //-----------------------------------end-------------------------------------// //----------------------------搜索距离最近的节点-----------------------------// mRRTNearNode = NearestNeighbor(mRRTTree, mRRTRandNode); //-----------------------------------end-------------------------------------// //--------------------------------生成新节点---------------------------------// if (PlanningStepType.Constant == m_RRTParameter.StepType) { mRRTNewNode = NewConfiguration(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } else //if (PlanningStepType.Random == m_RRTParameter.m_StepType) { mRRTNewNode = NewConfigurationWithRandomStep(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } //-----------------------------------end-------------------------------------// //计数器累加 nCount = nCount + 1; //-----------------------如果到达允许的搜索上限,则退出-----------------------// if (nCount >= m_RRTParameter.MaxNodeNumber) { Console.WriteLine(""); break; } //-----------------------------------end-------------------------------------// //----------------威胁规避(禁止在威胁区域内选择/生成新点/连线)---------------// if (!IsSafePoint(mRRTNewNode.NodeLocation)) { continue; } if (!IsSafeLine(mRRTNewNode.NodeLocation, mRRTNearNode.NodeLocation)) { continue; } //-----------------------------------end-------------------------------------// //------------------------------扩展树 - 核心--------------------------------// isReachTarget = ExpandTree(iTaskIndex, iStageIndex, ref mRRTTree, mRRTNearNode, mRRTNewNode); //-----------------------------------end-------------------------------------// //------------判断新结点到目标连线是否安全, 若安全则直接连接目标点-----------// if (TargetReachMode.Direct == m_RRTParameter.ReachMode) { isReachTarget = isReachTarget || DirectlyToTarget(iTaskIndex, iStageIndex, mRRTNewNode, ref mRRTTree); } //-----------------------------------end-------------------------------------// } //构建当前阶段航路 mRRTPath = BuildPath(mRRTTree); //为可视化输出保存 Visualization.MPathForVisualizaition = mRRTTree; 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); } //添加当前阶段航路到总航路(起始航路点为当前阶段, 目标航路点为下一个阶段, 注意总航路的最后一个航路点属于最后一个阶段) for (int k = 0; k < mRRTPath.Count - 1; ++k) { mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[k]), iStageIndex)); iWaypointIndex = iWaypointIndex + 1; } } //增加最后的目标点 mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[mRRTPath.Count - 1]), AlgoInput.UAVTask[iTaskIndex].Stages.Count - 1)); //返回路径 return(mPath); }
/// <summary> /// 从指定航路点开始为单架无人机规划航迹 /// </summary> /// <param name="iTaskIndex">无人机/任务索引</param> /// <param name="mWaypoint">指定的起始航路点</param> /// <param name="mPath">当前路径</param> /// <returns></returns> protected MPath BuildPathForSingleUAV(int iTaskIndex, MWaypoint mWaypoint, MPath mPath) { //移除起始航路点之后(含)的所有航路点 if (mPath.Waypoints != null) { mPath.Waypoints.RemoveRange(mWaypoint.Index, mPath.Waypoints.Count - mWaypoint.Index); //从数据库移除 } //定义树 List <RRTNode> mRRTTree = null; //定义航路 List <RRTNode> mRRTPath = null; //定义树节点 RRTNode mRRTNode = null; //定义树中间节点 RRTNode mRRTRandNode = null; RRTNode mRRTNearNode = null; RRTNode mRRTNewNode = null; //以当前航路点为当前阶段的起始状态点(以后的阶段均不变), 构建新的阶段 List <MStage> mNewStages = new List <MStage>(); MStage mTempStage = null; mTempStage = new MStage(); mTempStage.StageIndex = mWaypoint.StageIndex; mTempStage.StartState = new MKeyState(mWaypoint.State); mTempStage.TargetState = AlgoInput.UAVTask[iTaskIndex].Stages[mWaypoint.StageIndex].TargetState; mNewStages.Add(mTempStage); for (int i = mWaypoint.StageIndex + 1; i < AlgoInput.UAVTask[iTaskIndex].Stages.Count; ++i) { mTempStage = new MStage(); mTempStage = AlgoInput.UAVTask[iTaskIndex].Stages[i]; mNewStages.Add(mTempStage); } //当前航路点编号 int iWaypointIndex = mWaypoint.Index; //计数器 int nCount = 1; //对每一个阶段规划航路 for (int iStageIndex = mNewStages[0].StageIndex; iStageIndex < mNewStages[0].StageIndex + mNewStages.Count; ++iStageIndex) { //实例化 mRRTTree = new List <RRTNode>(); //加入开始点为第一个点 mRRTNode = new RRTNode(0, mNewStages[iStageIndex - mNewStages[0].StageIndex].StartState.Location, mNewStages[iStageIndex - mNewStages[0].StageIndex].StartState.Direction, null); mRRTTree.Add(mRRTNode); //初始化计数器 nCount = 1; //是否到达目标 - 初始未到达 bool isReachTarget = false; //循环构建新点 - 到达目标时自动退出 while (!isReachTarget) { //-------------------------------随机生成节点--------------------------------// if (NodeSelectionType.Classical == m_RRTParameter.SelectionType) { //方案1:随机生成节点 - 标准RRT采用的方案 mRRTRandNode = RandomConfiguration(iTaskIndex, iStageIndex, mRRTTree); } else //if (NodeSelectionType.Optimzed == m_RRTParameter.SelectionType) { //方案2:按照一定概率随机生成节点或者采用目标点 (改进方案) mRRTRandNode = RandomConfigurationOrTargetConfiguration(iTaskIndex, iStageIndex, mRRTTree); } //-----------------------------------end-------------------------------------// //----------------------------搜索距离最近的节点-----------------------------// mRRTNearNode = NearestNeighbor(mRRTTree, mRRTRandNode); //-----------------------------------end-------------------------------------// //--------------------------------生成新节点---------------------------------// if (PlanningStepType.Constant == m_RRTParameter.StepType) { mRRTNewNode = NewConfiguration(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } else //if (PlanningStepType.Random == m_RRTParameter.m_StepType) { mRRTNewNode = NewConfigurationWithRandomStep(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } //-----------------------------------end-------------------------------------// //计数器累加 nCount = nCount + 1; //-----------------------如果到达允许的搜索上限,则退出-----------------------// if (nCount >= m_RRTParameter.MaxNodeNumber) { break; } //-----------------------------------end-------------------------------------// //----------------威胁规避(禁止在威胁区域内选择/生成新点/连线)---------------// if (!IsSafePoint(mRRTNewNode.NodeLocation)) { continue; } if (!IsSafeLine(mRRTNewNode.NodeLocation, mRRTNearNode.NodeLocation)) { continue; } //-----------------------------------end-------------------------------------// //------------------------------扩展树 - 核心--------------------------------// isReachTarget = ExpandTree(iTaskIndex, iStageIndex, ref mRRTTree, mRRTNearNode, mRRTNewNode); //-----------------------------------end-------------------------------------// //------------判断新结点到目标连线是否安全, 若安全则直接连接目标点-----------// if (TargetReachMode.Direct == m_RRTParameter.ReachMode) { isReachTarget = isReachTarget || DirectlyToTarget(iTaskIndex, iStageIndex, mRRTNewNode, ref mRRTTree); } //-----------------------------------end-------------------------------------// } //构建当前阶段航路 mRRTPath = BuildPath(mRRTTree); //添加当前阶段航路到总航路 for (int k = 0; k < mRRTPath.Count - 1; ++k) { mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[k]), iStageIndex)); iWaypointIndex = iWaypointIndex + 1; } } //增加最后的目标点 mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[mRRTPath.Count - 1]), AlgoInput.UAVTask[iTaskIndex].Stages.Count - 1)); //返回路径 return(mPath); }
/// <summary> /// 判断是否能够避免碰撞 /// </summary> /// <param name="mRRTNode">节点</param> /// <returns></returns> protected bool CollisionAvoidance(RRTNode mRRTNode) { bool bIsSafe = true; return(bIsSafe); }
/// <summary> /// 将状态和父节点组合转换为节点 /// </summary> /// <param name="mUAVState">点的状态</param> /// <param name="parentRRTNode">父节点</param> /// <returns>树节点</returns> public static RRTNode ConvertUAVStateToNode(MKeyState mUAVState, RRTNode parentRRTNode) { return(new RRTNode(mUAVState.Location, mUAVState.Direction, parentRRTNode)); }