private static ArrayList <Pose> reconstructPath(Node node) { LinkedList <Pose> path = new LinkedList <Pose>(); Node rsNode = null; int subpathCount = 0; while (node != null) { if (node.action != null) { path.InsertFirst(node.pose); } else { ArrayList <Pose> subpath = ReedsSheppDriver.Discretize(node.from.pose, node.actionSet, VehicleModel.TurnRadius, GridResolution); subpathCount = subpath.Count; for (int i = subpathCount - 1; i >= 1; i--) { path.InsertFirst(subpath[i]); } rsNode = node; } node = node.from; } ArrayList <Pose> pathArray = new ArrayList <Pose>(); pathArray.AddAll(path); if (rsNode != null) { rsNode.rsIndex = pathArray.Count - subpathCount; } // Move every node's gear to it's parent node int count = pathArray.Count; Gear nextGear = pathArray[count - 1].Gear; for (int i = count - 2; i >= 0; i--) { Pose curr = pathArray[i]; Gear currGear = curr.Gear; curr.Gear = nextGear; pathArray[i] = curr; nextGear = currGear; } return(pathArray); }
private void calcRS() { maxLength = float.MinValue; paths = new ArrayList <ArrayList <Pose> >(); actions = new ArrayList <ReedsSheppActionSet>(); for (int x = -cells / 2; x <= cells / 2; x++) { for (int y = -cells / 2; y <= cells / 2; y++) { Pose start = new Pose(x * cellSize, y * cellSize, orientation); ReedsSheppActionSet actionSet = ReedsSheppSolver.Solve(start, goal, VehicleModel.TurnRadius); actionSet.Length = actionSet.CalculateCost(VehicleModel.TurnRadius, HybridAStar.reverseFactor, HybridAStar.switchPenalty); if (actionSet.Length > maxLength) { maxLength = actionSet.Length; } actions.Add(actionSet); paths.Add(ReedsSheppDriver.Discretize(start, actionSet, VehicleModel.TurnRadius, 1f)); } } }
private static Node getReedsSheppChild(Pose pose, Pose goal) { ReedsSheppActionSet actions = ReedsSheppSolver.Solve(pose, goal, VehicleModel.TurnRadius); bool safe = true; foreach (Pose s in ReedsSheppDriver.Discretize(pose, actions, VehicleModel.TurnRadius, GridResolution)) { if (!grid.IsPointInGrid(s.Position) || !grid.IsSafe(s, SafetyFactor)) { safe = false; break; } } if (safe) { return(new Node(Cell.None, actions, goal, 0, 0, null)); } return(null); }