/// <summary>
/// 构造函数 - 将参数传入节点内进行初始化
/// </summary>
/// <param name="mFPoint3">点</param>
/// <param name="mParentNode">父节点</param>
public Node(FPoint3 mFPoint3, Node mParentNode, LStarHeurstic mHeurstic = null)
{
///mHeurstic不要写成Static，否则没办法多线程并行化处理～
///函数式编程的IDEA吧。
///
NodeLocation = new FPoint3(mFPoint3.X, mFPoint3.Y, mFPoint3.Z);
ParentNode = mParentNode;
if (mHeurstic != null)
{
if (mParentNode == null)
{
GValue = 0;
HValue = mHeurstic.HeuristicFunc(mHeurstic.StartNode, mHeurstic.TargetNode);
}
else
{
///刷出来俩宝贝。。。。。
GValue = mHeurstic.GValueFunction(this);
HValue = mHeurstic.HValueFunction(this);
}
}
else
{
GValue = 0;
HValue = double.MaxValue;
}
}
/// <summary>
/// 构造函数 - 将参数传入节点内进行初始化
/// </summary>
/// <param name="iNodeIndex">节点编号</param>
/// <param name="mFPoint3">点</param>
/// <param name="flightDirection">飞行方向(</param>
/// <param name="mParentNode">父节点</param>
public Node(int iNodeIndex, FPoint3 mFPoint3, Node mParentNode) : base()
{
//将参数传入节点内进行初始化
NodeLocation = new FPoint3(mFPoint3.X, mFPoint3.Y, mFPoint3.Z);
ParentNode = mParentNode;
}
/// <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>
/// 计算两个节点之间的距离
/// </summary>
/// <param name="mNode1">节点1</param>
/// <param name="mNode2">节点2</param>
/// <returns></returns>
public double DistanceBetweenTwoNode(RrtStarNode mNode1, RrtStarNode mNode2)
{
//二维算法使用
return(FPoint3.DistanceBetweenTwoSpacePointsXY(mNode1.NodeLocation, mNode2.NodeLocation));
//三维算法使用
//return FPoint3.DistanceBetweenTwoSpacePoints(mNode1.NodeLocation, mNode2.NodeLocation);
}
private FPoint3 GetRegulatedPos(FPoint3 Pos)
{
var step = mPara.Step;
double x = Math.Round(Pos.X / step) * step;
double y = Math.Round(Pos.Y / step) * step;
return(new FPoint3(x, y, Pos.Z));
}
public NodeComponent(IPoint2 newIdx, FPoint3 newPos, byte newCost, int newBestCost, IPoint2 newDir)
{
idx = newIdx;
pos = newPos;
//No unsigned char --> by
travelCost = newCost;
bestTravelCost = newBestCost;
direction = newDir;
}
/// <summary>
/// 编写自己的构造函数
/// </summary>
/// <param name="startPara">起始点</param>
/// <param name="pointNumPara">当前点坐标</param>
/// <param name="goalPara">目标点坐标</param>
/// <param name="m_Input">输入场景信息</param>
/// <param name="mParentNode">父节点</param>
public Node(FPoint3 startPara, FPoint3 pointNumPara, FPoint3 goalPara, Node mParentNode) : base()
{
NodeLocation = pointNumPara;
ParentNode = mParentNode;
CostfromStart = mParentNode.CostfromStart + FPoint2.DistanceBetweenTwoPlanePoints(NodeLocation, mParentNode.NodeLocation);
//DistancetoGoal = FPoint3.DistanceBetweenTwoSpacePointsXY(NodeLocation, goalPara);
DistancetoGoal = FPoint2.DistanceBetweenTwoPlanePoints(NodeLocation, goalPara);
computeCostForAStar = CostfromStart + DistancetoGoal;
}
/// <summary>
/// 构造函数 - 专为初始节点设置的构造函数
/// </summary>
/// <param name="mFPoint3">点</param>
/// <param name="flightDirection">飞行方向(</param>
/// <param name="mParentNode">父节点</param>
public Node(FPoint3 mFPoint3, FPoint3 goalPara, double costfromStart, Node mParentNode)
: base()
{
//将参数传入节点内进行初始化
NodeLocation = new FPoint3(mFPoint3.X, mFPoint3.Y, mFPoint3.Z);
ParentNode = mParentNode;
CostfromStart = costfromStart;
//DistancetoGoal = Math.Min(Math.Abs(NodeLocation.X - goalPara.X), (NodeLocation.Y - goalPara.Y)) * (Math.Sqrt(2) - 1)
// + Math.Max(Math.Abs(NodeLocation.X - goalPara.X), (NodeLocation.Y - goalPara.Y));
DistancetoGoal = FPoint2.DistanceBetweenTwoPlanePoints(NodeLocation, goalPara);
}
/// <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;
}
private bool IsOutRange(FPoint3 node)
{
if (node.X < 0 || node.X > this.AlgoInput.Scenario.FlightScene.Coordinate.MaxX ||
node.Y < 0 || node.Y > this.AlgoInput.Scenario.FlightScene.Coordinate.MaxY)
{
return(true);
}
else
{
return(false);
}
}
protected bool IsPointOutrange(FPoint3 mPoint)
{
//var size = AlgoInput?.Scenario?.FlightScene?.Coordinate;
var size = AlgoInput.Scenario.FlightScene.Coordinate;
if (mPoint.X < size.MinX || mPoint.X > size.MaxX || mPoint.Y < size.MinY || mPoint.Y > size.MaxY)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 构造函数 - 将参数传入节点内进行初始化
/// </summary>
/// <param name="mFPoint3">点</param>
/// <param name="mParentNode">父节点</param>
public Node(FPoint3 mFPoint3, Node mParentNode, JPSHeurstic mHeurstic = null) : base()
{
///mHeurstic不要写成Static，否则没办法多线程并行化处理～
///函数式编程的IDEA吧。
///
NodeLocation = new FPoint3(mFPoint3.X, mFPoint3.Y, mFPoint3.Z);
ParentNode = mParentNode;
if (mHeurstic != null)
{
///刷出来俩宝贝。。。。。
GValue = mHeurstic.GValueFunction(this);
HValue = mHeurstic.HValueFunction(this);
}
else
{
GValue = 0;
HValue = double.MaxValue;
}
}
private bool IsForcedNeighbor(FPoint3 mNeighborPos, Node mTestNode)
{
//参数拿mNeighborPos是为了算离开mCurrentNode的方向
var step = mPara.Step;
var direction = GetDirection(mTestNode, mNeighborPos) * step;
var mTestPos = mTestNode.NodeLocation;
// check for forced neighbors
// along the diagonal
if (direction.X * direction.Y != 0)
{
if (IsSafePoint(mTestPos + new FPoint3(-direction.X, direction.Y, 0)) && !IsSafePoint(mTestPos + new FPoint3(direction.X, -direction.Y, 0)) ||
IsSafePoint(mTestPos + new FPoint3(direction.X, -direction.Y, 0)) && !IsSafePoint(mTestPos + new FPoint3(-direction.X, direction.Y, 0)))
{
return(true);
}
}
// horizontally/vertically
else
{
if (direction.X != 0)
{
if (IsSafePoint(mTestPos + new FPoint3(direction.X, step, 0)) && !IsSafePoint(mTestPos + new FPoint3(0, step, 0)) ||
IsSafePoint(mTestPos + new FPoint3(direction.X, -step, 0)) && !IsSafePoint(mTestPos + new FPoint3(0, -step, 0)))
{
return(true);
}
}
else //direction.y !=0
{
if (IsSafePoint(mTestPos + new FPoint3(step, direction.Y, 0)) && !IsSafePoint(mTestPos + new FPoint3(step, 0, 0)) ||
IsSafePoint(mTestPos + new FPoint3(-step, direction.Y, 0)) && !IsSafePoint(mTestPos + new FPoint3(-step, 0, 0)))
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// 核心剪纸策略
/// </summary>
/// <param name="mParentNode">init节点</param>
/// <param name="direction">原前进方向</param>
/// <returns></returns>
private Node Jump(Node mParentNode, FPoint3 direction)
{
var size = mPara.Step;
var TestNode = new Node(mParentNode.NodeLocation + direction * size, mParentNode, HeursticInfo);
//这个点是废了的
if (!IsSafePoint(TestNode.NodeLocation) || IsPointOutrange(TestNode.NodeLocation))
{
return(null);
}
if (TestNode.NodeLocation == HeursticInfo.TargetNode.NodeLocation)
{
return(TestNode);
}
foreach (var tmpNeighbor in FindPruneNeighbors4Jump(TestNode))
{
if (IsForcedNeighbor(tmpNeighbor, TestNode)) //判断TestNode是不是ForceNeighbor
{
return(TestNode);
//return new Node(TestNode.NodeLocation, TestNode.ParentNode, HeursticInfo);
//return new Node(tmpNeighbor, TestNode, HeursticInfo);
}
}
if (direction.X * direction.Y != 0)//对角线
{
FPoint3[] DirectionList = new FPoint3[2] {
new FPoint3(direction.X, 0, 0), new FPoint3(0, direction.Y, 0)
};
foreach (var tmpDir in DirectionList)
{
if (Jump(TestNode, tmpDir) != null)
{
return(TestNode);
}
}
}
return(Jump(TestNode, direction));
}
/// <summary>
/// 一个函数，求当前节点的周围的八个安全点，并放到Open列表中,核心函数
/// </summary>
/// <param name="Current">当前节点</param>
/// <param name="Open">Open集</param>
/// <param name="start">当前阶段起始点</param>
/// <param name="goal">当前阶段终点</param>
/// <returns></returns>
private List <Node> FindAroundPoint(Node Current, List <Node> Open, FPoint3 start, FPoint3 goal)
{
double step = (AlgoParameter as AStarOriginAlgorithmParameter).Step;
// double step = 1;
//获取坐标系x轴上下限
double minX = AlgoInput.Scenario.FlightScene.Coordinate.MinX;
double maxX = AlgoInput.Scenario.FlightScene.Coordinate.MaxX;
//获取坐标系Y轴上下限
double minY = AlgoInput.Scenario.FlightScene.Coordinate.MinY;
double maxY = AlgoInput.Scenario.FlightScene.Coordinate.MaxY;
FPoint3[] grid = new FPoint3[8];
grid[0] = new FPoint3(step, step, 0); //右上
grid[1] = new FPoint3(0, step, 0); //上
grid[2] = new FPoint3(-step, step, 0); //左上
grid[3] = new FPoint3(-step, 0, 0); //左
grid[4] = new FPoint3(-step, -step, 0); //左下
grid[5] = new FPoint3(0, -step, 0); //下
grid[6] = new FPoint3(step, -step, 0); //右下
grid[7] = new FPoint3(step, 0, 0); //右
for (int i = 0; i < 8; i++)
{
FPoint3 newpoint = new FPoint3();//定义当前节点坐标
//newpoint.X = Current.NodeLocation.X + grid[i].X;
//newpoint.Y = Current.NodeLocation.Y + grid[i].Y;
//newpoint.Z = Current.NodeLocation.Z;
newpoint = Current.NodeLocation + grid[i];
if (IsSafeLine(Current.NodeLocation, newpoint) && IsSafePoint(newpoint) == true && newpoint.X < maxX && newpoint.X > minX && newpoint.Y < maxY && newpoint.Y > minY) //如果此点安全且在范围内，就可以进行了
{
bool find = false; //是否在点集中找到的标志位
for (int j = 0; j < Open.Count; j++) //先在Open表中进行for循环，看能不能找到这个点
{
if (Open[j].NodeLocation == newpoint) //直到从集合中找到这个点为止
{
find = true;
//if (Open[j].sign == false) //表示这个点还在Open表中
//{
double distancetoroot = 1; //FPoint2.DistanceBetweenTwoPlanePoints(newpoint, Current.NodeLocation);//缺陷2
if ((Current.CostfromStart + distancetoroot) < Open[j].CostfromStart) //
{
Open[j].ParentNode = Current; //更新父节点 //缺陷1
Open[j].CostfromStart = Current.CostfromStart + distancetoroot;
Open[j].computeCostForAStar = Open[j].DistancetoGoal + Open[j].CostfromStart; //Current.CostfromStart + distancetoroot;//更新代价函数
if (Open[j].sign == true)
{
Open[j].sign = false;
}
}
break;//既然已经找到这个点，就没有必要找下去了，跳出第一个for循环
}
}//----------------------end for （j）-----------------------------------------//
if (find == false) //如果j到了这个程度，还没跳出这个for循环，说明找到的这个节点根本不在Open表中啊，可以直接加进去了
{
Node newNode = new Node(start, newpoint, goal, Current); //如果此点是安全的，而且在范围内，就初始化为一个节点
Open.Add(newNode);
}
}
}//------------------------------------------------end for(i)-----------------------------------------------//
return(Open);
}
public static double Euclidean(Node lhs, Node rhs)
{
return(FPoint3.DistanceBetweenTwoSpacePointsXY(lhs.NodeLocation, rhs.NodeLocation));
}
public double GValueFunction(Node CurrentNode)
{
return(CurrentNode.ParentNode == null ?
0 : CurrentNode.ParentNode.GValue + FPoint3.DistanceBetweenTwoSpacePointsXY(CurrentNode.NodeLocation, CurrentNode.ParentNode.NodeLocation));
}
private FPoint3 GetDirection(Node mCurrentNode, FPoint3 mNeighborPos)
{
FPoint3 Direction = mNeighborPos - mCurrentNode.NodeLocation;
return(new FPoint3(Math.Sign(Direction.X), Math.Sign(Direction.Y), Math.Sign(Direction.Z)));
}
private FPoint3 GetDirection(FPoint3 mCurrentPos, FPoint3 mParentPos)
{
FPoint3 Direction = mCurrentPos - mParentPos;
return(new FPoint3(Math.Sign(Direction.X), Math.Sign(Direction.Y), Math.Sign(Direction.Z)));
}
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);
}
/// <summary>
/// 路径化简，动态规划方法
/// </summary>
/// <param name="OriginNodeList"></param>
/// <param name="OUTPUTWayPointList"></param>
private void PathSimplify(List <RRTNode> OriginNodeList, ref List <RRTNode> OUTPUTWayPointList)
{
// List<FPoint3> temp_WayPointList = new List<FPoint3>();
// for (int i = 0; i < OriginNodeList.Count; i++)
// {
// temp_WayPointList.Add(OriginNodeList[i].NodeLocation * 1.0); //new
// }
List <List <double> > DistanceMatrix = new List <List <double> >();
for (int i = 0; i < OriginNodeList.Count; i++)
{
DistanceMatrix.Add(new List <double>());
for (int j = 0; j < OriginNodeList.Count; j++)
{
DistanceMatrix[i].Add(new double());
DistanceMatrix[i][j] = double.MaxValue;
}
}
/*
* x0 x1 x2 x3
* x0 max max max max
* x1 X max max max
* x2 X X max max
* x3 X X X max
*/
for (int i = 0; i < OriginNodeList.Count - 1; i++)
{
for (int j = i + 1; j < OriginNodeList.Count; j++) //下三角阵
{
//if (Line_ObstacleAvodiance(OriginNodeList[i], OriginNodeList[j], FlightScene) == true)
if (IsSafeLine(OriginNodeList[i].NodeLocation, OriginNodeList[j].NodeLocation))
{
DistanceMatrix[i][j] = FPoint3.DistanceBetweenTwoSpacePointsXY(OriginNodeList[i].NodeLocation, OriginNodeList[j].NodeLocation);
DistanceMatrix[j][i] = DistanceMatrix[i][j];
}
else
{
DistanceMatrix[i][j] = double.MaxValue;
}
}
}
//距离矩阵生成结束
//这里设X0为 初始点， Xn 为goalPoint
List <int> OptimizePrePathPoint = new List <int>(); //记录到达该店最优路径的父节点
for (int i = 0; i < OriginNodeList.Count; i++)
{
OptimizePrePathPoint.Add(new int());
OptimizePrePathPoint[i] = int.MinValue;
}
List <double> OptimizeDistance = new List <double>(); //记录到达该点的最优距离
for (int i = 0; i < OriginNodeList.Count; i++)
{
OptimizeDistance.Add(new double());
OptimizeDistance[i] = double.MaxValue;
}
Dijkstra(OriginNodeList.Count, DistanceMatrix, OriginNodeList.Count - 1, ref OptimizePrePathPoint, ref OptimizeDistance);
OUTPUTWayPointList.Clear();
int temp_index = 0;
do
{
OUTPUTWayPointList.Add(OriginNodeList[temp_index]);
temp_index = OptimizePrePathPoint[temp_index];
} while (temp_index != OptimizePrePathPoint.Count - 1);
OUTPUTWayPointList.Add(OriginNodeList[temp_index]);
}
/// <summary>
/// 本DEMO中给出的目标函数的定义,以最短距离为最优目标
/// </summary>
/// <param name="lNode"></param>
/// <param name="rNode"></param>
/// <returns></returns>
protected double CostFunc(RrtStarNode lNode, RrtStarNode rNode)
{
return(FPoint3.DistanceBetweenTwoSpacePointsXY(lNode.NodeLocation, rNode.NodeLocation));
}
/// <summary>
/// 构造函数 - 将参数传入节点内进行初始化
/// </summary>
/// <param name="mX">X坐标</param>
/// <param name="mY">Y坐标</param>
/// <param name="mZ">Z坐标</param>
/// <param name="mParentNode">父节点</param>
public Node(double mX, double mY, double mZ, Node mParentNode) : base()
{
//将参数传入节点内进行初始化
NodeLocation = new FPoint3(mX, mY, mZ);
ParentNode = mParentNode;
}
