public static Stack <T> ThetaStar <T>(T start, Func <T, bool> isGoal, Func <T, float> heuristic, Func <T, T, bool> lineOfSight, Func <T, IEnumerable <Arc <T> > > explode)
{
var queue = new PriorityQueue <T>();
var distances = new Dictionary <T, float>();
var parents = new Dictionary <T, T>();
var visited = new HashSet <T>();
distances[start] = 0;
queue.Enqueue(new Arc <T>(start, 0));
while (!queue.IsEmpty)
{
var poped = queue.Dequeue();
visited.Add(poped.Element);
if (isGoal(poped.Element))
{
return(CreatePath(parents, poped.Element));
}
var toEnqueue = explode(poped.Element);
foreach (var item in toEnqueue)
{
var element = item.Element;
if (parents.ContainsKey(poped.Element) && !lineOfSight(parents[poped.Element], element))
{
var elementToParentDistance = item.Weight + poped.Weight;
var startToElementDistance = distances.ContainsKey(element) ? distances[element] : float.MaxValue;
var startToParentDistance = distances[parents[poped.Element]];
var newDist = startToParentDistance + elementToParentDistance;
if (!visited.Contains(element) && startToElementDistance > newDist)
{
ListHandling.UpdateDictionary(distances, element, newDist);
ListHandling.UpdateDictionary(parents, element, parents[poped.Element]);
queue.Enqueue(new Arc <T>(element, newDist + heuristic(element)));
}
}
else
{
var elementToPopedDistance = item.Weight;
var startToElementDistance = distances.ContainsKey(element) ? distances[element] : float.MaxValue;
var startToPopedDistance = distances[poped.Element];
var newDist = startToPopedDistance + elementToPopedDistance;
if (!visited.Contains(element) && startToElementDistance > newDist)
{
ListHandling.UpdateDictionary(distances, element, newDist);
ListHandling.UpdateDictionary(parents, element, poped.Element);
queue.Enqueue(new Arc <T>(element, newDist + heuristic(element)));
}
}
}
}
return(null);
}
static public IEnumerable <Room> Explode(Room room)
{
var toVisit = new List <Room>();
for (int i = 0; i < 4; i++)
{
var nextRoomX = room.x + xMovementVector[i];
var nextRoomY = room.y + yMovementVector[i];
if (0 <= nextRoomX && nextRoomX < room.visited.GetLength(0) &&
0 <= nextRoomY && nextRoomY < room.visited.GetLength(1) &&
!room.visited[nextRoomX, nextRoomY])
{
var child = Instantiate(room.prefab);
child.Configure(nextRoomX, nextRoomY, room.basePosition, room, room.prefab, i, room.visited, room.transform.parent);
toVisit.Add(child);
}
}
return(ListHandling.RandomizeList(toVisit));
}
