public Queue <Vector2Int> CalculatePath(NavGraph graph, Vector2Int start, Vector2Int goal)
{
Vector2IntComparer comparer = new Vector2IntComparer();
Dictionary <Vector2Int, Vector2Int> cameFrom = new Dictionary <Vector2Int, Vector2Int>(comparer);
Dictionary <Vector2Int, float> g = new Dictionary <Vector2Int, float>(comparer);
Dictionary <Vector2Int, float> f = new Dictionary <Vector2Int, float>(comparer);
HashSet <Vector2Int> open = new HashSet <Vector2Int>(comparer);
HashSet <Vector2Int> closed = new HashSet <Vector2Int>(comparer);
g[start] = 0;
f[start] = H(start, goal);
open.Add(start);
while (open.Count > 0)
{
Vector2Int current = GetCurrent(open, f);
if (comparer.Equals(current, goal))
{
return(BuildPath(cameFrom, goal));
}
open.Remove(current);
closed.Add(current);
List <NavGraph.NavEdge> neighbors = graph.GetNeighbors(current);
foreach (NavGraph.NavEdge edge in neighbors)
{
if (closed.Contains(edge.dest))
{
continue;
}
else if (!open.Contains(edge.dest))
{
open.Add(edge.dest);
}
float gScore = g[current] + edge.cost;
if (g.ContainsKey(edge.dest) && gScore >= g[edge.dest])
{
continue;
}
else
{
cameFrom[edge.dest] = current;
g[edge.dest] = gScore;
f[edge.dest] = gScore + H(edge.dest, goal);
}
}
}
return(new Queue <Vector2Int>());
}
public NavAreaMap(NavGraph navGraph)
{
Debug.Assert(navGraph != null);
this.navGraph = navGraph;
Vector2IntComparer comparer = new Vector2IntComparer();
int currentNavAreaId = 0;
Vector2Int originCell;
while (GetUnassignedCell(out originCell))
{
List <Vector2Int> currentNavArea = new List <Vector2Int>();
Queue <Vector2Int> open = new Queue <Vector2Int>();
open.Enqueue(originCell);
while (currentNavArea.Count < NAV_AREA_SIZE)
{
if (open.Count == 0)
{
break;
}
Vector2Int currentTile = open.Dequeue();
currentNavArea.Add(currentTile);
List <NavGraph.NavEdge> neighbors = navGraph.GetNeighbors(currentTile);
foreach (NavGraph.NavEdge edge in neighbors)
{
if (!currentNavArea.Contains(edge.dest) &&
!open.Contains(edge.dest) &&
!navAreaAssignments.ContainsKey(edge.dest))
{
open.Enqueue(edge.dest);
}
}
}
foreach (Vector2Int cell in currentNavArea)
{
navAreaAssignments.Add(cell, currentNavAreaId);
}
navAreaColors.Add(currentNavAreaId, Random.ColorHSV());
currentNavAreaId++;
}
}