private static LinkedList <Node> getChildren(Pose pose, Gear gear, float dt, Pose goal) { LinkedList <Node> children = new LinkedList <Node>(); for (int steer = 0; steer < VehicleModel.NumSteers; steer++) { float length; Pose child = VehicleModel.NextPose(pose, (Steer)steer, gear, dt, out length); child.Gear = gear; if (grid.IsPointInGrid(child.Position) && grid.IsSafe(child, SafetyFactor)) { children.Add(new Node(Cell.None, new ReedsSheppAction((Steer)steer, (Gear)gear, length), child, 0, 0, null)); } } // Add a Reeds-Shepp curve as a child every now and then float cost = heuristic.GetHeuristicValue(pose, goal); // Made the fall off rate quadratic because Reeds-Shepp curves are expsensive! if (cost < 10f || random.NextDouble() < 10f / (cost * cost)) { Node reedsShepp = getReedsSheppChild(pose, goal); if (reedsShepp != null) { children.Add(reedsShepp); } } return(children); }