private ShortestPathNode? findShortestPath(Vector start, IReadOnlyList<IGoal> wayPoints, double stepSize)
{
var open = new BinaryHeapOpenSet<GridKey, GridSearchNode>();
var closed = new ClosedSet<GridKey>();
open.Add(
new GridSearchNode(
key: new GridKey(start, wayPoints.Count),
start,
previous: null,
distanceFromStart: 0.0,
estimatedCost: 0.0,
wayPoints,
targetWayPoint: 0));
while (!open.IsEmpty)
{
var nodeToExpand = open.DequeueMostPromissing();
if (nodeToExpand.RemainingWayPoints.Count == 0)
{
var head = new ShortestPathNode(
wayPoints.Last().Position, // use the goal position directly
targetWayPoint: wayPoints.Count - 1); // the goal way point should be kept here
var backtrackingNode = nodeToExpand.Previous;
while (backtrackingNode != null)
{
var node = new ShortestPathNode(backtrackingNode.Position, backtrackingNode.TargetWayPoint);
node.CostToNext = TimeSpan.FromSeconds(Distance.Between(node.Position, head.Position) / maxSpeed);
node.Next = head;
head = node;
backtrackingNode = backtrackingNode.Previous;
}
return head;
}
closed.Add(nodeToExpand.Key);
for (var dx = -1; dx <= 1; dx++)
{
for (var dy = -1; dy <= 1; dy++)
{
if (dx == 0 && dy == 0) continue;
var nextPoint = new Vector(
nodeToExpand.Position.X + dx * stepSize,
nodeToExpand.Position.Y + dy * stepSize);
var reachedWayPoint = nodeToExpand.RemainingWayPoints[0].ReachedGoal(nextPoint);
var remainingWayPoints = reachedWayPoint
? nodeToExpand.RemainingWayPoints.Skip(1).ToList().AsReadOnly()
: nodeToExpand.RemainingWayPoints;
var targetWayPoint = wayPoints.Count - nodeToExpand.RemainingWayPoints.Count; // I want to keep the ID of the waypoint in the node which reaches the waypoint and only increase it for its childer
var key = new GridKey(nextPoint, remainingWayPoints.Count);
if (closed.Contains(key))
{
continue;
}
if (collisionDetector.IsCollision(nextPoint))
{
closed.Add(key);
continue;
}
var distance = nodeToExpand.DistanceFromStart + (nextPoint - nodeToExpand.Position).CalculateLength();
var node = new GridSearchNode(key, nextPoint, nodeToExpand, distance, distance + 0, remainingWayPoints, targetWayPoint);
if (open.Contains(node.Key))
{
if (node.DistanceFromStart < open.Get(node.Key).DistanceFromStart)
{
open.ReplaceExistingWithTheSameKey(node);
}
}
else
{
open.Add(node);
}
}
}
}
return null;
}
public IPlan?FindOptimalPlanFor(VehicleState initialState)
{
var heuristic = createShortestPathHeuristic(initialState);
var open = new BinaryHeapOpenSet <DiscreteState, SearchNode>();
var closed = new ClosedSet <DiscreteState>();
open.Add(
new SearchNode(
discreteState: discretizer.Discretize(initialState, wayPoints.Count),
state: initialState,
actionFromPreviousState: null,
previousState: null,
costToCome: 0,
estimatedTotalCost: 0, // we don't need estimate for the initial node
targetWayPoint: 0));
while (!open.IsEmpty)
{
var node = open.DequeueMostPromissing();
if (node.TargetWayPoint == wayPoints.Count)
{
return(node.ReconstructPlan());
}
closed.Add(node.Key);
exploredStates.OnNext(node.State);
foreach (var action in actions.AllPossibleActions)
{
var predictedStates = motionModel.CalculateNextState(node.State, action, timeStep);
var elapsedTime = predictedStates.Last().relativeTime;
var nextVehicleState = predictedStates.Last().state;
var reachedWayPoint = false;
var collided = false;
foreach (var(simulationTime, state) in predictedStates)
{
if (collisionDetector.IsCollision(state))
{
elapsedTime = simulationTime;
nextVehicleState = state;
reachedWayPoint = false;
collided = true;
break;
}
if (wayPoints[node.TargetWayPoint].ReachedGoal(state.Position))
{
reachedWayPoint = true;
}
}
var nextTargetWayPoint = reachedWayPoint
? node.TargetWayPoint + 1
: node.TargetWayPoint;
var nextDiscreteState = discretizer.Discretize(nextVehicleState, nextTargetWayPoint);
if (closed.Contains(nextDiscreteState))
{
continue;
}
if (collided)
{
closed.Add(nextDiscreteState);
continue;
}
var costToCome = node.CostToCome + timeStep.TotalSeconds;
var reachedLastWayPoint = nextTargetWayPoint != wayPoints.Count;
var costToGo = reachedLastWayPoint
? heuristic.EstimateTimeToGoal(nextVehicleState, nextTargetWayPoint).TotalSeconds
: 0;
var discoveredNode = new SearchNode(
discreteState: nextDiscreteState,
state: nextVehicleState,
actionFromPreviousState: action,
previousState: node,
costToCome: costToCome,
estimatedTotalCost: costToCome + costToGo,
nextTargetWayPoint);
if (greedy && reachedLastWayPoint)
{
return(discoveredNode.ReconstructPlan());
}
if (!open.Contains(discoveredNode.Key))
{
open.Add(discoveredNode);
}
else if (discoveredNode.CostToCome < open.Get(discoveredNode.Key).CostToCome)
{
open.ReplaceExistingWithTheSameKey(discoveredNode);
}
}
}
return(null);
}