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