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