public void BranchAndBound(ref HashSet <RrtStarNode> mFinalNodeSet, ref HashSet <RrtStarNode> mRrtStarTree)
{
var targetNode = RrtStarNode.ConvertUavStateToNode(_mInput.UAVTask[ITaskIndex].Stages[IStageIndex].TargetState);
var minCost = mFinalNodeSet.Min(e => e.CostFuncValue + MLineCostFunc(e, targetNode));
RrtStarNode mOptimalNode = mFinalNodeSet.FirstOrDefault(e => e.CostFuncValue + MLineCostFunc(e, targetNode) == minCost);
if (mOptimalNode == null) //default
{
return;
}
//foreach(var item in mRRTStarTree.Where(e => e.IsValid == true)) //寻找mFinalNodeSet相同内容
foreach (var item in mFinalNodeSet)
{
if (item.NodeIndex == mOptimalNode.NodeIndex)
{
continue;
}
//跳过最优值，剩下的都删了
//TODO
//Step 1. Find out the GGGGGrandParent that should be abandoned.
var topNodeParent = item;
RrtStarNode topNode = item; //TopNode的子节点, item 一定是 abandoned 的节点。
while (topNodeParent.CostFuncValue + MLineCostFunc(topNodeParent, targetNode) > minCost)
{
if (topNodeParent.ParentNode != null)
{
topNode = topNodeParent;
topNodeParent = topNodeParent.ParentNode;
}
else
{
break;
}
}
//Step 2. Delete ChildNode reference
if (topNode.ParentNode != null)
{
topNode.ParentNode.ChildNodes.Remove(topNode);
}
//Step 3. Set All child as abandoned
SetDeleted(topNode);
}
//Step Finally. Delete! Them! All!
mRrtStarTree.RemoveWhere(e => e.IsAbandoned);
mFinalNodeSet.Clear();
mFinalNodeSet.Add(mOptimalNode);
}
public void MarkingValid(RrtStarNode mRrtCurrentNode, ref HashSet <RrtStarNode> finalNodeSet)
{
var targetNode = RrtStarNode.ConvertUavStateToNode(_mInput.UAVTask[ITaskIndex].Stages[IStageIndex].TargetState);
//路径连通而且距离不长
if (_isSafeLine(mRrtCurrentNode.NodeLocation, targetNode.NodeLocation))
//&& DistanceBetweenTwoNode(mRRTCurrentNode, TargetNode) <= RRTParameter.Error)
{
//打上 Valid标记（gradually模式用）
mRrtCurrentNode.IsValid = true;
finalNodeSet.Add(mRrtCurrentNode);
}
}
private RrtStarNode BiasRandomConfiguration()
{
//新节点
RrtStarNode mRrtNode = null;
//是否直接把目标点当作新点
if (_mRandom.NextDouble() > MRrtParameter.ChooseTargetThreshold)
{
mRrtNode = RandomConfigurationOrigin();
}
else
{
mRrtNode = RrtStarNode.ConvertUavStateToNode(_mInput.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>
public bool DirectlyToTarget(RrtStarNode mRrtCurrentNode, ref HashSet <RrtStarNode> mRrtTree)
{
bool isReachTarget = false;
//进一步判断新结点到目标连线是否安全, 若安全则直接连接目标点
if (_isSafeLine(mRrtCurrentNode.NodeLocation, _mInput.UAVTask[ITaskIndex].Stages[IStageIndex].TargetState.Location))
{
//设置为到达
isReachTarget = true;
//设置目标点为最后一个新结点, 并将当前节点设置为她的父节点
mRrtCurrentNode = RrtStarNode.ConvertUavStateToNode(_mInput.UAVTask[ITaskIndex].Stages[IStageIndex].TargetState, mRrtCurrentNode);
//设节点编号
mRrtCurrentNode.NodeIndex = mRrtTree.Count;
//增加新节点
mRrtTree.Add(mRrtCurrentNode);
}
return(isReachTarget);
}
public void GraduallyToTarget(int count, ref HashSet <RrtStarNode> mRrtTree)
{
//当count < MaxNodeNumber 的木一个值时有可能之前被 continue掉，所以不能这么写
//所以应该取count == m_RRTParameter.MaxNodeNumber并在最终异常部分处理
//判断mRRTCurrentNode是不是有效的.
var targetNode = RrtStarNode.ConvertUavStateToNode(_mInput.UAVTask[ITaskIndex].Stages[IStageIndex].TargetState);
var finalCost = mRrtTree.Where(e => e.IsValid).Min(e => e.CostFuncValue + MLineCostFunc(e, targetNode));
RrtStarNode finalNode = mRrtTree.FirstOrDefault(e => e.CostFuncValue + MLineCostFunc(e, targetNode) == finalCost);
/*
* var TargetNode = RRTStarNode.ConvertUAVStateToNode(m_Input.UAVTask[iTaskIndex].Stages[iStageIndex].TargetState);
* RRTStarNode FinalNode = null;
* double FinalCost = double.MaxValue;
* //寻找最短路径
* foreach (var tmpNode in mRRTTree)
* {
* if (tmpNode.IsValid)
* {
* var tmpCost = tmpNode.CostFuncValue + mLineCostFunc(tmpNode, TargetNode);
* if (tmpCost < FinalCost)
* {
* FinalCost = tmpCost;
* FinalNode = tmpNode;
* }
* }
* }*/
if (finalNode == null)
{
ConstructFailedPath(ref mRrtTree);
}
else
{
//设置目标点为最后一个新结点, 并将当前节点设置为她的父节点
var mRrtFinalNode = RrtStarNode.ConvertUavStateToNode(_mInput.UAVTask[ITaskIndex].Stages[IStageIndex].TargetState, finalNode);
//设节点编号
mRrtFinalNode.NodeIndex = count;
//增加新节点
mRrtTree.Add(mRrtFinalNode);
}
}
