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