public static List <Point> DijkstraInGrid(RGGrid grid, Point startPos, Point endPos)
{
List <Point> unfinishedVertices = new List <Point>();
Dictionary <Point, float> distanceMap = new Dictionary <Point, float>();
Dictionary <Point, Point> visitedMap = new Dictionary <Point, Point>();
unfinishedVertices.Add(startPos);
distanceMap.Add(startPos, 0);
visitedMap.Add(startPos, null);
while (unfinishedVertices.Count > 0)
{
Point vertex = GetClosestVertex(unfinishedVertices, distanceMap);
unfinishedVertices.Remove(vertex);
if (vertex.Equals(endPos))
{
return(GeneratePath(visitedMap, vertex));
}
foreach (Point adj in grid.GetAdjacentCells(vertex))
{
if (!visitedMap.ContainsKey(adj))
{
unfinishedVertices.Add(adj);
}
float adjDist = distanceMap.ContainsKey(adj) ? distanceMap[adj] : int.MaxValue;
float vDist = distanceMap.ContainsKey(vertex) ? distanceMap[vertex] : int.MaxValue;
if (adjDist > vDist + grid.GetCostOfEnteringCell(adj))
{
if (distanceMap.ContainsKey(adj))
{
distanceMap[adj] = vDist + grid.GetCostOfEnteringCell(adj);
}
else
{
distanceMap.Add(adj, vDist + grid.GetCostOfEnteringCell(adj));
}
if (visitedMap.ContainsKey(adj))
{
visitedMap[adj] = vertex;
}
else
{
visitedMap.Add(adj, vertex);
}
}
}
}
return(null);
}
public static RGSearchResult BestFirstSearch(RGGrid grid, Point StartPosition, Point EndPosition)
{
RGPriorityQueue <Point> queue = new RGPriorityQueue <Point>();
Dictionary <Point, float> distanceMap = new Dictionary <Point, float>();
Dictionary <Point, Point> visitedMap = new Dictionary <Point, Point>();
queue.Enqueue(StartPosition, 0);
distanceMap.Add(StartPosition, 0);
visitedMap.Add(StartPosition, null);
while (!queue.Empty)
{
Point current = queue.Dequeue();
if (current.Equals(EndPosition))
{
return(new RGSearchResult
{
Path = GeneratePath(visitedMap, current),
Visited = new List <Point>(visitedMap.Keys)
});
}
foreach (Point neighbor in grid.GetAdjacentCells(current))
{
if (!visitedMap.ContainsKey(neighbor))
{
float priority = Heuristic(EndPosition, neighbor);
queue.Enqueue(neighbor, priority);
visitedMap.Add(neighbor, current);
distanceMap.Add(neighbor, distanceMap[current] + grid.GetCostOfEnteringCell(neighbor));
}
}
}
return(new RGSearchResult());
}
public static RGSearchResult DijkstraGeneral(RGGrid grid, Point startPos)
{
RGPriorityQueue <Point> queue = new RGPriorityQueue <Point>();
Dictionary <Point, float> distanceMap = new Dictionary <Point, float>();
Dictionary <Point, Point> visitedMap = new Dictionary <Point, Point>();
queue.Enqueue(startPos, 0);
distanceMap.Add(startPos, 0);
visitedMap.Add(startPos, null);
while (!queue.Empty)
{
Point current = queue.Dequeue();
foreach (Point adj in grid.GetAdjacentCells(current))
{
float newDist = distanceMap[current] + grid.GetCostOfEnteringCell(adj);
if (!distanceMap.ContainsKey(adj) || newDist < distanceMap[adj])
{
distanceMap[adj] = newDist;
visitedMap[adj] = current;
queue.Enqueue(adj, newDist);
}
}
}
return(new RGSearchResult()
{
VisitedMap = visitedMap,
DistanceMap = distanceMap
});
}
public static RGSearchResult DijkstraWithPriorityQueue(RGGrid grid, Point StartPosition, Point EndPosition)
{
RGPriorityQueue <Point> queue = new RGPriorityQueue <Point>();
Dictionary <Point, float> distanceMap = new Dictionary <Point, float>();
Dictionary <Point, Point> visitedMap = new Dictionary <Point, Point>();
queue.Enqueue(StartPosition, 0);
distanceMap.Add(StartPosition, 0);
visitedMap.Add(StartPosition, null);
while (!queue.Empty)
{
Point current = queue.Dequeue();
if (current.Equals(EndPosition))
{
return(new RGSearchResult
{
Path = GeneratePath(visitedMap, current),
Visited = new List <Point>(visitedMap.Keys)
});
}
foreach (Point adj in grid.GetAdjacentCells(current))
{
float newDist = distanceMap[current] + grid.GetCostOfEnteringCell(adj);
if (!distanceMap.ContainsKey(adj) || newDist < distanceMap[adj])
{
distanceMap[adj] = newDist;
visitedMap[adj] = current;
queue.Enqueue(adj, newDist);
}
}
}
return(new RGSearchResult());
}
public static RGSearchResult AStarBiDirectional(RGGrid grid, Point StartPosition, Point EndPosition)
{
Dictionary <Point, bool> openedBy = new Dictionary <Point, bool>();
RGPriorityQueue <Point> startQueue = new RGPriorityQueue <Point>();
RGPriorityQueue <Point> endQueue = new RGPriorityQueue <Point>();
Dictionary <Point, float> startDistanceMap = new Dictionary <Point, float>();
Dictionary <Point, Point> startVisitedMap = new Dictionary <Point, Point>();
Dictionary <Point, float> endDistanceMap = new Dictionary <Point, float>();
Dictionary <Point, Point> endVisitedMap = new Dictionary <Point, Point>();
startQueue.Enqueue(StartPosition, 0);
startDistanceMap[StartPosition] = 0;
startVisitedMap[StartPosition] = null;
openedBy[StartPosition] = true;
endQueue.Enqueue(EndPosition, 0);
endDistanceMap[EndPosition] = 0;
endVisitedMap[EndPosition] = null;
openedBy[EndPosition] = false;
// Even if onequeue is empty, then it means there is no path
while (!startQueue.Empty && !endQueue.Empty)
{
Point current = startQueue.Dequeue();
if (openedBy.ContainsKey(current) && openedBy[current] == false)
{
// If it enters here, it means the two lookups have met.
List <Point> startPath = GeneratePath(startVisitedMap, current);
List <Point> endPath = GeneratePath(endVisitedMap, current);
List <Point> allPath = new List <Point>(startPath);
allPath.AddRange(endPath);
List <Point> allVisited = new List <Point>(startVisitedMap.Keys);
allVisited.AddRange(endVisitedMap.Keys);
return(new RGSearchResult
{
Path = allPath,
Visited = allVisited
});
}
foreach (Point neighbour in grid.GetAdjacentCells(current))
{
float newCost = startDistanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
if (!startDistanceMap.ContainsKey(neighbour) || newCost < startDistanceMap[neighbour])
{
startDistanceMap[neighbour] = newCost;
openedBy[neighbour] = true;
float priority = newCost + Heuristic(EndPosition, neighbour);
startQueue.Enqueue(neighbour, priority);
startVisitedMap[neighbour] = current;
}
}
// From end
current = endQueue.Dequeue();
if (openedBy.ContainsKey(current) && openedBy[current] == true)
{
// Found goal or the frontier from the start queue
// Return solution
List <Point> startPath = GeneratePath(startVisitedMap, current);
List <Point> endPath = GeneratePath(endVisitedMap, current);
List <Point> allPath = new List <Point>(startPath);
allPath.AddRange(endPath);
List <Point> allVisited = new List <Point>(startVisitedMap.Keys);
allVisited.AddRange(endVisitedMap.Keys);
return(new RGSearchResult
{
Path = allPath,
Visited = allVisited
});
}
foreach (Point neighbour in grid.GetAdjacentCells(current))
{
float newCost = endDistanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
if (!endDistanceMap.ContainsKey(neighbour) || newCost < endDistanceMap[neighbour])
{
endDistanceMap[neighbour] = newCost;
openedBy[neighbour] = false;
float priority = newCost + Heuristic(StartPosition, neighbour);
endQueue.Enqueue(neighbour, priority);
endVisitedMap[neighbour] = current;
}
}
}
return(new RGSearchResult());
}