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