private ShortestPathNode simplifyPath(ShortestPathNode head)
{
var start = head;
var node = head.Next;
while (node != null)
{
if (node.IsGoal || node.Next != null && (!areInLineOfSight(start.Position, node.Next.Position) || start.TargetWayPoint != node.TargetWayPoint))
{
start.Next = node;
start.CostToNext = TimeSpan.FromSeconds(Distance.Between(start.Position, node.Position) / maxSpeed);
start = node;
}
node = node.Next;
}
return head;
}
public GridShortestPathHeuristic(
Vector startPosition,
IReadOnlyList<IGoal> wayPoints,
ITrack raceTrack,
BoundingSphereCollisionDetector collisionDetector,
double stepSize,
double maxSpeed)
{
this.raceTrack = raceTrack;
this.maxSpeed = maxSpeed;
this.collisionDetector = collisionDetector;
var path = findShortestPath(startPosition, wayPoints, stepSize);
if (path == null)
{
throw new Exception($"Cannot find path from [{startPosition.X}, {startPosition.Y}] to goal.");
}
shortestPathStart = simplifyPath(path);
}
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;
}