public override void Update(Pose goal)
{
one.Update(goal);
two.Update(goal);
}
public static HybridAStarResults FindPath(ObstacleGrid aGrid, Pose start, Pose aGoal)
{
grid = aGrid;
goal = aGoal;
heuristicUpdated = false;
heuristicBitmap = null;
IntervalHeap <Node> open = new IntervalHeap <Node>();
//heuristic = new EuclideanHeuristic();
//heuristic = new ObstacleRelaxed(grid);
heuristic = new NonholonomicRelaxed(grid);
//heuristic = new CombinedHeuristic(grid);
DateTime now = DateTime.Now;
heuristic.Update(goal);
heuristicUpdated = true;
TimeSpan heuristicInitTime = DateTime.Now - now;
Expanded = new LinkedList <Node>();
LinkedList <Node> discovered = new LinkedList <Node>();
int numCols = (int)Math.Ceiling(grid.NumColumns * grid.Resolution / GridResolution);
int numRows = (int)Math.Ceiling(grid.NumRows * grid.Resolution / GridResolution);
CellState?[, , ,] cells = new CellState?[numCols, numRows, numOrientations, 2];
Cell startCell = poseToCell(start, 0);
IPriorityQueueHandle <Node> startHandle = null;
float heuristicValue = heuristic.GetHeuristicValue(start, goal);
Node startNode = new Node(startCell, new ReedsSheppAction(Steer.Straight, Gear.Forward, 0f), start, 0, Epsilon * heuristicValue, null);
open.Add(ref startHandle, startNode);
discovered.Add(startNode);
cells[startCell.c, startCell.r, startCell.o, startCell.rev] = new CellState(start, Epsilon * heuristicValue, startHandle);
while (!open.IsEmpty)
{
Node n = open.DeleteMin();
lock (Expanded)
{
Expanded.Add(n);
}
CellState?c = cells[n.cell.c, n.cell.r, n.cell.o, n.cell.rev];
if (c.Value.continuousPose == goal)
{
return(new HybridAStarResults(reconstructPath(n), Expanded, discovered, heuristicInitTime));
}
float dt = GridResolution / VehicleModel.SlowVelocity;
//GridCell nodeCell = grid.PointToCellPosition(c.Value.continuousPose.Position);
//float dt = MathHelper.Max(GridResolution, 0.5f * (grid.GVD.GetObstacleDistance(nodeCell) + grid.GVD.GetVoronoiDistance(nodeCell)));
//dt /= VehicleModel.SlowVelocity;
for (int g = 0; g < VehicleModel.NumGears; g++)
{
Gear gear = (Gear)g;
foreach (Node child in getChildren(c.Value.continuousPose, gear, dt, goal))
{
int rev = gear == Gear.Forward ? 0 : 1;
float tentativeg;
if (child.actionSet == null)
{
tentativeg = n.g + pathCost(n.pose, n.cell.rev, child.pose, rev, dt);
}
else
{
tentativeg = n.g + child.actionSet.CalculateCost(VehicleModel.TurnRadius, reverseFactor, switchPenalty);
}
float tentativef = tentativeg + Epsilon * heuristic.GetHeuristicValue(child.pose, goal);
child.cell = poseToCell(child.pose, rev);
CellState?currentCell = cells[child.cell.c, child.cell.r, child.cell.o, child.cell.rev];
if (!currentCell.HasValue)
{
IPriorityQueueHandle <Node> childHandle = null;
child.g = tentativeg;
child.f = tentativef;
child.from = n;
open.Add(ref childHandle, child);
discovered.Add(child);
cells[child.cell.c, child.cell.r, child.cell.o, child.cell.rev] = new CellState(child.pose, tentativef, childHandle);
}
else if (tentativef < currentCell.Value.f)
{
IPriorityQueueHandle <Node> currentHandle = currentCell.Value.handle;
child.g = tentativeg;
child.f = tentativef;
child.from = n;
Node temp;
if (open.Find(currentHandle, out temp))
{
open[currentHandle] = child;
}
else
{
open.Add(ref currentHandle, child);
}
discovered.Add(child);
cells[child.cell.c, child.cell.r, child.cell.o, child.cell.rev] = new CellState(child.pose, tentativef, currentHandle);
}
}
}
if (Delay > 0)
{
Thread.Sleep(Delay);
}
}
return(new HybridAStarResults(new ArrayList <Pose>(), Expanded, discovered, heuristicInitTime));
}