public static System.Tuple <bool, List <Vector3> > RetracePath(PathfindGrid grid, Vector2Int startCoord, Vector2Int endCoord, Dictionary <Vector2Int, Vector2Int> comeFromDict, bool reversePath)
{
List <Vector3> path = new List <Vector3>();
Vector2Int currCoord = endCoord;
while (currCoord != startCoord)
{
path.Add(grid.CoordsToWorld(currCoord));
if (!comeFromDict.TryGetValue(currCoord, out currCoord))
{
return(new Tuple <bool, List <Vector3> >(false, path));
}
}
path.Add(grid.CoordsToWorld(startCoord));
if (reversePath)
{
path.Reverse();
}
return(new Tuple <bool, List <Vector3> >(true, path));
}
//**********************
// BREADTH FIRST SEARCH
//**********************
// - guaranties shortest path
// - assumes all edges are of equal cost(uniform heuristic)
// - best for 1:n or n:1
//public static Dictionary<T, T> BreadthFirstSearch(T startNode, T endNode, INavigatable<T> graph)
//{
// Queue<T> openSet = new Queue<T>();
// Dictionary<T, T> comeFromDict = new Dictionary<T, T>();
// openSet.Enqueue(startNode);
// comeFromDict.Add(startNode, default(T));
// while (openSet.Count > 0)
// {
// T currNode = openSet.Dequeue();
// if (currNode.Equals(endNode))
// return comeFromDict;
// foreach (T neighbour in graph.GetNeighbours(currNode))
// if (!comeFromDict.ContainsKey(neighbour))
// {
// openSet.Enqueue(neighbour);
// comeFromDict.Add(neighbour, currNode);
// }
// }
// return comeFromDict;
//}
//*****************************
// DIJKSTRA (uniform cost search)
//*****************************
// - guaranties shortest path
// - can deal with diferent weights
// - best for 1:n or n:1
//public static Dictionary<T, T> Dijkstra(T startNode, T endNode, INavigatable<T> graph)
//{
// Heap<DijkstraNode<T>> openSet = new Heap<DijkstraNode<T>>(graph.NodeCount);
// Dictionary<T, T> comeFromDict = new Dictionary<T, T>();
// Dictionary<T, DijkstraNode<T>> allDijkstraNodes = new Dictionary<T, DijkstraNode<T>>();
// DijkstraNode<T> newDijkstraNode = new DijkstraNode<T>(startNode, 0);
// openSet.Add(newDijkstraNode);
// allDijkstraNodes[startNode] = newDijkstraNode;
// ////until there are no more unchecked nodes...
// while (openSet.Count > 0)
// {
// //we take best (Fcost or if equal Hcost) node on the heap
// DijkstraNode<T> currDijkstra = openSet.Extract();
// T currNode = currDijkstra.m_item;
// //if we reached end node we are done
// if (currNode.Equals(endNode))
// return comeFromDict;
// //we check all its neighbours
// foreach (T neighbour in graph.GetNeighbours(currNode))
// {
// float newCostToNeighbour = currDijkstra.Gcost + graph.GetDistance(currNode, neighbour);
// if (!comeFromDict.ContainsKey(neighbour))
// {
// DijkstraNode<T> neighbourDijkstra = new DijkstraNode<T>(neighbour, newCostToNeighbour);
// openSet.Add(neighbourDijkstra);
// comeFromDict.Add(neighbour, currNode);
// allDijkstraNodes[neighbour] = neighbourDijkstra;
// }
// else if (newCostToNeighbour < allDijkstraNodes[neighbour].Gcost)
// {
// comeFromDict[neighbour] = currNode;
// DijkstraNode<T> neigbourDijkstra = allDijkstraNodes[neighbour];
// neigbourDijkstra.Gcost = newCostToNeighbour;
// openSet.Update(neigbourDijkstra);
// }
// }
// }
// return comeFromDict;
//}
//***************************
// GREEDY FIRST SEARCH
//***************************
// - does NOT guarantee shortest path
// - generally very fast
// - works for 1:1
//public static Dictionary<T, T> GreedyFirst(T startNode, T endNode, INavigatable<T> graph)
//{
// Heap<GreedyNode<T>> openSet = new Heap<GreedyNode<T>>(graph.NodeCount);
// Dictionary<T, T> comeFromDict = new Dictionary<T, T>();
// GreedyNode<T> newGreedyNode = new GreedyNode<T>(startNode, graph.GetDistance(startNode, endNode));
// openSet.Add(newGreedyNode);
// //until there are no more unchecked nodes...
// while (openSet.Count > 0)
// {
// //we take best (Fcost or if equal Hcost) node on the heap
// GreedyNode<T> currGreedyNode = openSet.Extract();
// T currNode = currGreedyNode.m_item;
// //if we reached end node we are done
// if (currNode.Equals(endNode))
// return comeFromDict;
// //we check all its neighbours
// foreach (T neighbour in graph.GetNeighbours(currNode))
// {
// if (!comeFromDict.ContainsKey(neighbour))
// {
// openSet.Add(new GreedyNode<T>(neighbour, graph.GetDistance(neighbour, endNode)));
// comeFromDict[neighbour] = currNode;
// }
// }
// }
// return comeFromDict;
//}
//*******************
// A*
//*******************
// - guaranties shortest path
// - mostly use this one
// - works for 1:1
//public static Dictionary<T, T> Astar(T startNode, T endNode, INavigatable<T> graph)
//{
// Heap<AStarNode<T>> openSet = new Heap<AStarNode<T>>(graph.NodeCount);
// Dictionary<T, T> comeFromDict = new Dictionary<T, T>();
// Dictionary<T, AStarNode<T>> allAstarNodes = new Dictionary<T, AStarNode<T>>();
// AStarNode<T> newAstarNode = new AStarNode<T>(startNode, 0, graph.GetDistance(startNode, endNode));
// openSet.Add(newAstarNode);
// allAstarNodes.Add(startNode, newAstarNode);
// //until there are no more unchecked nodes...
// while (openSet.Count > 0)
// {
// AStarNode<T> currAstarNode = openSet.Extract();
// T currNode = currAstarNode.m_item;
// if (currNode.Equals(endNode))
// return comeFromDict;
// //we check all its neighbours
// foreach (T neighbour in graph.GetNeighbours(currNode))
// {
// if (!neighbour.IsWalkable)
// continue;
// float newCostToNeighbour = currAstarNode.Gcost + graph.GetDistance(currNode, neighbour);
// if (!comeFromDict.ContainsKey(neighbour))
// {
// AStarNode<T> neighbourAstar = new AStarNode<T>(neighbour, newCostToNeighbour, graph.GetDistance(neighbour, endNode));
// openSet.Add(neighbourAstar);
// comeFromDict.Add(neighbour, currNode);
// allAstarNodes.Add(neighbour, neighbourAstar);
// }
// else if (newCostToNeighbour < allAstarNodes[neighbour].Gcost)
// {
// comeFromDict[neighbour] = currNode;
// AStarNode<T> neigbourAstar = allAstarNodes[neighbour];
// neigbourAstar.Gcost = newCostToNeighbour;
// openSet.Update(neigbourAstar);
// }
// }
// }
// return comeFromDict;
//}
//******************
// A* distance
//******************
// - using A* finds only the distance between two nodes
//public static float AstarDistance(T startNode, T endNode, INavigatable<T> graph)
//{
// Heap<AStarNode<T>> openSet = new Heap<AStarNode<T>>(graph.NodeCount);
// HashSet<T> closedSet = new HashSet<T>();
// Dictionary<T, AStarNode<T>> allAstarNodes = new Dictionary<T, AStarNode<T>>();
// AStarNode<T> newAstarNode = new AStarNode<T>(startNode, 0, graph.GetDistance(startNode, endNode));
// openSet.Add(newAstarNode);
// closedSet.Add(startNode);
// allAstarNodes.Add(startNode, newAstarNode);
// //until there are no more unchecked nodes...
// while (openSet.Count > 0)
// {
// AStarNode<T> currAstarNode = openSet.Extract();
// T currNode = currAstarNode.m_item;
// if (currNode.Equals(endNode))
// return 1000000; //TODO....
// //we check all its neighbours
// foreach (T neighbour in graph.GetNeighbours(currNode))
// {
// //if (neighbour.IsWalkable)
// //{
// float newCostToNeighbour = currAstarNode.Gcost + graph.GetDistance(currNode, neighbour);
// if (!closedSet.Contains(neighbour))
// {
// AStarNode<T> neighbourAstar = new AStarNode<T>(neighbour, newCostToNeighbour, graph.GetDistance(neighbour, endNode));
// openSet.Add(neighbourAstar);
// closedSet.Add(neighbour);
// allAstarNodes.Add(neighbour, neighbourAstar);
// }
// else if (newCostToNeighbour < allAstarNodes[neighbour].Gcost)
// {
// AStarNode<T> neigbourAstar = allAstarNodes[neighbour];
// neigbourAstar.Gcost = newCostToNeighbour;
// openSet.Update(neigbourAstar);
// }
// }
// }
// return -1;
//}
//public static System.Tuple<bool, List<Vector3>> RetracePath(INavigatable<T> graph, T startNode, T endNode, Dictionary<T, T> comeFromDict, bool reversePath)
//{
// List<Vector3> path = new List<Vector3>();
// T currNode = endNode;
// while (!currNode.Equals(startNode))
// {
// path.Add(graph.GetPosition(currNode));
// if (!comeFromDict.TryGetValue(currNode, out currNode))
// return new Tuple<bool, List<Vector3>>(false, path);
// }
// path.Add(graph.GetPosition(startNode));
// if (reversePath)
// path.Reverse();
// return new Tuple<bool, List<Vector3>>(true, path);
//}
public static Dictionary <Vector2Int, Vector2Int> Astar(Vector2Int startCoord, Vector2Int endCoord, PathfindGrid grid)
{
Heap <AStarNode <Vector2Int> > openSet = new Heap <AStarNode <Vector2Int> >(grid.NodeCount);
Dictionary <Vector2Int, Vector2Int> comeFromDict = new Dictionary <Vector2Int, Vector2Int>();
Dictionary <Vector2Int, AStarNode <Vector2Int> > allAstarNodes = new Dictionary <Vector2Int, AStarNode <Vector2Int> >();
AStarNode <Vector2Int> newAstarNode = new AStarNode <Vector2Int>(startCoord, 0, grid.GetDistance(startCoord, endCoord));
openSet.Add(newAstarNode);
allAstarNodes.Add(startCoord, newAstarNode);
//until there are no more unchecked nodes...
while (openSet.Count > 0)
{
AStarNode <Vector2Int> currAstarNode = openSet.Extract();
Vector2Int currCoord = currAstarNode.m_item;
if (currCoord == endCoord)
{
return(comeFromDict);
}
//we check all its neighbours
foreach (Vector2Int neighbour in grid.Get8Neighbours(currCoord))
{
if (!grid.IsWalkable(neighbour))
{
continue;
}
float newCostToNeighbour = currAstarNode.Gcost + grid.GetDistance(currCoord, neighbour);
if (!comeFromDict.ContainsKey(neighbour))
{
AStarNode <Vector2Int> neighbourAstar = new AStarNode <Vector2Int>(neighbour, newCostToNeighbour, grid.GetDistance(neighbour, endCoord));
openSet.Add(neighbourAstar);
comeFromDict.Add(neighbour, currCoord);
allAstarNodes.Add(neighbour, neighbourAstar);
}
else if (newCostToNeighbour < allAstarNodes[neighbour].Gcost)
{
comeFromDict[neighbour] = currCoord;
AStarNode <Vector2Int> neigbourAstar = allAstarNodes[neighbour];
neigbourAstar.Gcost = newCostToNeighbour;
openSet.Update(neigbourAstar);
}
}
}
return(comeFromDict);
}