public static NonholonomiHeuristicInfo Calculate(float neighborhoodSize, float cellSize, int numOrientations)
{
orientationSize = MathHelper.TwoPi / numOrientations;
numCells = (int)Math.Ceiling(neighborhoodSize / cellSize);
if (numCells % 2 == 0)
{
numCells++;
}
offset = (float)Math.Floor(numCells / 2f) * cellSize;
Pose goal = new Pose(0f, 0f, 0f);
float[, ,] heur = new float[numCells, numCells, numOrientations];
int totalCells = numCells * numCells * numOrientations;
int count = 0;
float maxHeuristic = float.MinValue;
for (int c = 0; c < numCells; c++)
{
for (int r = 0; r < numCells; r++)
{
for (int o = 0; o < numOrientations; o++)
{
Pose pose = new Pose(c * cellSize - offset, r * cellSize - offset, o * orientationSize);
ReedsSheppActionSet actions = ReedsSheppSolver.Solve(pose, goal, VehicleModel.TurnRadius);
heur[c, r, o] = actions.CalculateCost(VehicleModel.TurnRadius, 1f, 0f);
if (actions.Length > maxHeuristic)
{
maxHeuristic = actions.Length;
}
count++;
if (CellCalculated != null)
{
CellCalculated(null, new CalculatedArgs()
{
C = c,
R = r,
O = o,
Progress = (float)count / totalCells
});
}
}
}
}
return(new NonholonomiHeuristicInfo()
{
CellSize = cellSize,
NumCells = numCells,
OrientationSize = orientationSize,
NumOrientations = numOrientations,
Heuristic = heur,
MaxHeuristic = maxHeuristic
});
}
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);
}