/// <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; }