/// <summary>
/// Returns a list that is the path that the unit should take.
/// The list is 'backwards' so to speak, as in the closest move to make
/// is at the end of the list.
/// </summary>
/// <param name="START"></param>
/// <param name="GOAL"></param>
/// <returns></returns>
public List <Vertex> ShortestPathSlow(Vertex START, Vertex GOAL)
{
OPEN.Clear();
CLOSED.Clear();
_currentVertex = START;
OPEN.Enqueue(_currentVertex);
// Set this so taht we can traverse later
Vertex startingPoint = _currentVertex;
GOAL.VertColor = Color.Red;
// While the peek of open is not the goal
while (_OPEN.Peek() != GOAL)
{
// Take the current node out of open
// Current should be the thing on top of the priority queue
_currentVertex = _OPEN.DequeueTwo();
_CLOSED.Add(_currentVertex);
// Find all of the neighbors of the current vertex
List <Vertex> currentNeighbors = GetNieghbors(_currentVertex);
foreach (Vertex neighbor in currentNeighbors)
{
// Get the cost, which is G(current)
double cost = _currentVertex.Distance + GetCost(_currentVertex, neighbor);
// If neighbor is in OPEN and cost less then neighbor.distance
// Neighbor.Distance is G(neighbor)
if (_OPEN.heap.Contains(neighbor) && cost < neighbor.Distance)
{
// Remove neighbor from OPEN, because the new path is better
_OPEN.heap.Remove(neighbor);
_currentVertex = OPEN.DequeueTwo();
}
// If neighbor is in CLOSED and cost is less then g(neighbor)
if (_CLOSED.Contains(neighbor) && cost < neighbor.Distance)
{
// Remove neighbor from CLOSED
_CLOSED.Remove(neighbor);
}
// If neighbor is not in open and neighbor is not in CLOSED:
if (!_OPEN.heap.Contains(neighbor) && !_CLOSED.Contains(neighbor))
{
neighbor.Distance = cost;
_OPEN.Enqueue(neighbor);
neighbor.NeighboringVertex = _currentVertex;
}
}
}
// Reconstruct the reverse path from goal to start
_ONES_TO_DRAW = ColorPathSlow(startingPoint, GOAL);
return(_ONES_TO_DRAW);
}
/// <summary>
/// Calls the Reset() method on every vertex in the grid
/// </summary>
/// <param name="currentVertex"></param>
public void ResetAllVertecies(Vertex currentVertex)
{
foreach (Vertex v in _ALL_VERTECIES)
{
v.Reset();
}
OPEN.Clear();
CLOSED.Clear();
_currentVertex = currentVertex;
OPEN.Enqueue(_currentVertex);
}