private List <Node> FindPathToGoalNode(Node inStartNode, Node inGoalNode)
{
int expands = 0;
HashSet <Node> closedList = new HashSet <Node>();
List <KeyValuePair <float, PathPair> > openList = new List <KeyValuePair <float, PathPair> >();
List <Node> tempList = new List <Node>();
openList.Add(
new KeyValuePair <float, PathPair>(
Heuristic(inStartNode, inGoalNode),
new PathPair(new List <Node> {
inStartNode
}, inStartNode)));
while (openList.Count > 0 && expands < 1000)
{
float minNodeValue = openList.Min(n => n.Key);
var currentNodePair = openList.FirstOrDefault(node => Math.Abs(node.Key - minNodeValue) < 0.001f);
var currentNodePathPair = currentNodePair.Value;
var currentPath = currentNodePathPair.Key;
var currentNode = currentNodePathPair.Value;
openList.Remove(currentNodePair);
if (currentNode.GetType() == typeof(GoalNode))
{
currentPath.Add(currentNode);
return(currentPath);
}
tempList = WorldGrid.GetNeighbors(currentNode);
foreach (var node in tempList)
{
node.Flagged = true;
}
tempList.Remove(currentPath.Last());
tempList.RemoveAll(node => !node.IsPassable);
if (!closedList.Contains(currentNode))
{
closedList.Add(currentNode);
}
foreach (var node in tempList)
{
expands++;
if (node.IsPassable && !closedList.Contains(node))
{
List <Node> newList = currentPath.ToList();
newList.Add(currentNode);
openList.Add(
new KeyValuePair <float, PathPair>(
Heuristic(node, inGoalNode),// + newList.Count,
new PathPair(newList, node)));
}
}
}
Debug.Log("aw crud");
Die();
return(new List <Node> {
inStartNode
});
}
/*
* A* algorithm. Customized to use a heap!
*/
IEnumerator AStar(Vector2 startPoint, Vector2 endPoint)
{
// A dictionary containing all the explored nodes so far. I used a dict so search time is constant!
Dictionary <Node, bool> explored = new Dictionary <Node, bool>();
// Heap array for our open set.
openSet = new NodeHeapArray();
if (visualizer)
{
closedSet = new List <Node>();
}
// Reinatialize all nodes in our grid done
foreach (Node node in grid.gridNode)
{
node.gCost = Mathf.Infinity;
node.hCost = Mathf.Infinity;
node.parentNode = null;
explored[node] = false;
}
// Player's grid position
Vector2 playerGridPos = grid.WorldToGrid(startPoint);
// Goal's grid Position
Vector2 endPointGridPos = grid.WorldToGrid(endPoint);
// Node corresponding to the player's grid position
Node playerNode = grid.gridNode[(int)playerGridPos.x, (int)playerGridPos.y];
playerNode.gCost = 0;
playerNode.hCost = HeuristicCost(playerGridPos, endPointGridPos);
openSet.Push(playerNode);
float startTime = Time.time;
while (openSet.Count() > 0)
{
yield return(new WaitForEndOfFrame());
Node current = openSet.Pop();
explored[current] = true;
if (visualizer)
{
closedSet.Add(current);
}
// check if we found our goal!
if (current.point == endPointGridPos)
{
print("Time elapsed: " + (Time.time - startTime));
finalPath.Clear(); // Clear previous final path
int iterationCount = 10000; // Ensures that we don't get stuck in an infinite cycle!
Node goal = grid.gridNode[(int)endPointGridPos.x, (int)endPointGridPos.y];
while (goal.parentNode != null && iterationCount > 0)
{
finalPath.AddLast(goal);
goal = goal.parentNode;
iterationCount--;
}
break;
}
// Check all neighbors
foreach (Node node in grid.GetNeighbors(current))
{
// New to be gCost.
// Addding a bias value to the gCost significantly reduces the number
// of Nodes explored during search time!
float tentativeGScore = current.gCost + HeuristicCost(node.point, current.point, bias);
// If the node has already been explored, or the gCost is higher htan the current node's gCost
// There's no point checking this node!
if (explored[node] || tentativeGScore >= node.gCost)
{
continue;
}
if (!openSet.Contains(node))
{
openSet.Push(node);
}
// Update this node's information!
//int nodeIndex = openSet.nodeArray.IndexOf(node);
node.parentNode = current;
node.gCost = tentativeGScore;
node.hCost = HeuristicCost(node.point, endPointGridPos);
openSet.BottomTopHeapify(node.index); // re-heapify our open set!
}
}
}
