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