public static SearchResult DijkstraGeneral(Grid grid, Point startPos)
{
FakePriorityQueue <Point> queue = new FakePriorityQueue <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 SearchResult()
{
VisitedMap = visitedMap,
DistanceMap = distanceMap
});
}
public static SearchResult BestFirstSearch(Grid grid, Point startPos, Point endPos)
{
FakePriorityQueue <Point> queue = new FakePriorityQueue <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();
if (current.Equals(endPos))
{
return(new SearchResult
{
Path = GeneratePath(visitedMap, current),
Visited = new List <Point>(visitedMap.Keys)
});
}
foreach (Point neighbour in grid.GetAdjacentCells(current))
{
if (!visitedMap.ContainsKey(neighbour))
{
float priority = Heuristic(endPos, neighbour);
queue.Enqueue(neighbour, priority);
visitedMap.Add(neighbour, current);
distanceMap.Add(neighbour, distanceMap[current] + grid.GetCostOfEnteringCell(neighbour));
}
}
}
return(new SearchResult());
}
public static SearchResult DijkstraPriority(Grid grid, Grid.Point startPos, Grid.Point endPos)
{
FakePriorityQueue <Grid.Point> queue = new FakePriorityQueue <Grid.Point>();
Dictionary <Grid.Point, float> distanceMap = new Dictionary <Grid.Point, float>();
Dictionary <Grid.Point, Grid.Point> visitedMap = new Dictionary <Grid.Point, Grid.Point>();
queue.Enqueue(startPos, 0);
distanceMap.Add(startPos, 0);
visitedMap.Add(startPos, null);
while (!queue.Empty)
{
Grid.Point current = queue.Dequeue();
if (current.Equals(endPos))
{
return(new SearchResult
{
Path = GeneratePath(visitedMap, current),
Visited = new List <Grid.Point>(visitedMap.Keys)
});
}
foreach (Grid.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 SearchResult());
}
public static SearchResult AStarSearchWithCost(Grid grid, Point startPos, Point endPos, Dictionary <Point, float> costMap)
{
FakePriorityQueue <Point> queue = new FakePriorityQueue <Point>();
Dictionary <Point, float> costSoFar = new Dictionary <Point, float>();
Dictionary <Point, Point> cameFrom = new Dictionary <Point, Point>();
queue.Enqueue(startPos, 0);
costSoFar[startPos] = 0;
cameFrom[startPos] = null;
while (!queue.Empty)
{
Point current = queue.Dequeue();
if (current.Equals(endPos))
{
return(new SearchResult
{
Path = GeneratePath(cameFrom, current),
Visited = new List <Point>(cameFrom.Keys)
});
}
foreach (Point neighbour in grid.GetAdjacentCells(current))
{
float cost = costMap.ContainsKey(neighbour) ? costMap[neighbour] : 1;
float newCost = costSoFar[current] + cost;
if (!costSoFar.ContainsKey(neighbour) || newCost < costSoFar[neighbour])
{
costSoFar[neighbour] = newCost;
float priority = newCost + Heuristic(endPos, neighbour);
queue.Enqueue(neighbour, priority);
cameFrom[neighbour] = current;
}
}
}
return(new SearchResult());
}
public static SearchResult AStarSearch(Grid grid, Grid.Point startPos, Grid.Point endPos)
{
FakePriorityQueue <Grid.Point> queue = new FakePriorityQueue <Grid.Point>();
Dictionary <Grid.Point, float> distanceMap = new Dictionary <Grid.Point, float>();
Dictionary <Grid.Point, Grid.Point> visitedMap = new Dictionary <Grid.Point, Grid.Point>();
queue.Enqueue(startPos, 0);
distanceMap[startPos] = 0;
visitedMap[startPos] = null;
while (!queue.Empty)
{
Grid.Point current = queue.Dequeue();
if (current.Equals(endPos))
{
return(new SearchResult
{
Path = GeneratePath(visitedMap, current),
Visited = new List <Grid.Point>(visitedMap.Keys)
});
}
foreach (Grid.Point neighbour in grid.GetAdjacentCells(current))
{
float newCost = distanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
if (!distanceMap.ContainsKey(neighbour) || newCost < distanceMap[neighbour])
{
distanceMap[neighbour] = newCost;
float priority = newCost + Heuristic(endPos, neighbour);
queue.Enqueue(neighbour, priority);
visitedMap[neighbour] = current;
}
}
}
return(new SearchResult());
}
public static SearchResult BiDirectionalAStarSearch(Grid grid, Grid.Point startPos, Grid.Point endPos)
{
// True if opened from the start queue, false if opened by the end queue, not opened if not exists
Dictionary <Grid.Point, bool> openedBy = new Dictionary <Grid.Point, bool>();
FakePriorityQueue <Grid.Point> startQueue = new FakePriorityQueue <Grid.Point>();
FakePriorityQueue <Grid.Point> endQueue = new FakePriorityQueue <Grid.Point>();
Dictionary <Grid.Point, float> startDistanceMap = new Dictionary <Grid.Point, float>();
Dictionary <Grid.Point, Grid.Point> startVisitedMap = new Dictionary <Grid.Point, Grid.Point>();
Dictionary <Grid.Point, float> endDistanceMap = new Dictionary <Grid.Point, float>();
Dictionary <Grid.Point, Grid.Point> endVisitedMap = new Dictionary <Grid.Point, Grid.Point>();
startQueue.Enqueue(startPos, 0);
startDistanceMap[startPos] = 0;
startVisitedMap[startPos] = null;
openedBy[startPos] = true;
endQueue.Enqueue(endPos, 0);
endDistanceMap[endPos] = 0;
endVisitedMap[endPos] = null;
openedBy[endPos] = false;
while (!startQueue.Empty && !endQueue.Empty)
{
// From start
Grid.Point current = startQueue.Dequeue();
if (openedBy.ContainsKey(current) && openedBy[current] == false)
{
// Found goal or the frontier from the end queue
// Return solution
List <Grid.Point> startPath = GeneratePath(startVisitedMap, current);
List <Grid.Point> endPath = GeneratePath(endVisitedMap, current);
List <Grid.Point> allPath = new List <Grid.Point>(startPath);
allPath.AddRange(endPath);
List <Grid.Point> allVisited = new List <Grid.Point>(startVisitedMap.Keys);
allVisited.AddRange(endVisitedMap.Keys);
return(new SearchResult
{
Path = allPath,
Visited = allVisited
});
}
foreach (Grid.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(endPos, 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 <Grid.Point> startPath = GeneratePath(startVisitedMap, current);
List <Grid.Point> endPath = GeneratePath(endVisitedMap, current);
List <Grid.Point> allPath = new List <Grid.Point>(startPath);
allPath.AddRange(endPath);
List <Grid.Point> allVisited = new List <Grid.Point>(startVisitedMap.Keys);
allVisited.AddRange(endVisitedMap.Keys);
return(new SearchResult
{
Path = allPath,
Visited = allVisited
});
}
foreach (Grid.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(startPos, neighbour);
endQueue.Enqueue(neighbour, priority);
endVisitedMap[neighbour] = current;
}
}
}
return(new SearchResult());
}
