/// <summary>
/// Finds a path between startPoint and endPoint.
/// </summary>
/// <param name="startPoint">Starting point</param>
/// <param name="endPoint">Ending point</param>
/// <returns>Path that connects start and end point</returns>
public Path FindPath(Point startPoint, Point endPoint)
{
if (this.Collides(startPoint) || this.Collides(endPoint))
{
return null;
}
this.StartPoint = startPoint;
this.EndPoint = endPoint;
// initialize the open set of nodes with start point
C5.IPriorityQueue<State> openSet = new C5.IntervalHeap<State>();
// associate handles with points
var handles = new Dictionary<Point, C5.IPriorityQueueHandle<State>>();
// add start point to queue and associate point with handle
C5.IPriorityQueueHandle<State> handle = null;
openSet.Add(ref handle, new State {Point = startPoint, Heuristic = 0});
handles.Add(this.StartPoint, handle);
var closedSet = new HashSet<Point>();
// the g-score is the distance from start point to the current point
var gScore = new Dictionary<Point, double> {{startPoint, 0}};
// the f-score is the g-score plus heuristic
var fScore = new Dictionary<Point, double> {{startPoint, Utils.Distance(startPoint, this.EndPoint)}};
// cameFrom is used to reconstruct path when target is found
var cameFrom = new Dictionary<Point, Point>();
// process nodes in the open set...
while (!openSet.IsEmpty)
{
// fetch highest priority node, i.e. the one with
// lowest heuristic value
State minState = openSet.DeleteMin();
Point x = minState.Point;
handles.Remove(x);
if (x == this.EndPoint)
{
// we found a solution
return this.ReconstructPath(cameFrom);
}
closedSet.Add(x);
var neighbours = this.GetNeighbours(x);
// for each neighbour...
foreach (var y in neighbours)
{
if (closedSet.Contains(y))
{
continue;
}
// total cost from start
var tentativeGScore = gScore[x] + Utils.Distance(x, y);
bool tentativeIsBetter;
Point point = y;
handle = null;
if (!openSet.Exists(s => s.Point == point) && !closedSet.Contains(y))
{
// will get priority adjusted further down
openSet.Add(ref handle, new State {Point = point});
handles.Add(point, handle);
tentativeIsBetter = true;
}
else if (tentativeGScore < gScore[y])
{
tentativeIsBetter = true;
}
else
{
tentativeIsBetter = false;
}
if (tentativeIsBetter)
{
// update f-score of y
cameFrom[y] = x;
gScore[y] = tentativeGScore;
fScore[y] = gScore[y] + Utils.Distance(y, this.EndPoint);
var heuristic = fScore[y];
// set priority of y
if (handle == null)
{
handle = handles[point];
}
openSet.Replace(handle, new State {Heuristic = heuristic, Point = y});
}
}
}
return null;
}
