public AStarSearch(int rows, int cols)
{
openList = new SortedDictionary<float, List<AStarNode>>();
Rows = rows;
Cols = cols;
Nodes = new AStarNode[Rows, Cols];
for (int r = 0; r < Rows; r++)
for (int c = 0; c < Cols; c++)
Nodes[r, c] = new AStarNode(c, r, new Vector3(c, 0, r));
}
private void AddToOpenList(AStarNode node, AStarNode parent = null)
{
if (!node.Passable || node.Closed) return;
if (parent == null) node.Cost = 0;
else
{
float cost = parent.Cost + 1;
if (node.Cost > cost)
{
RemoveFromOpenList(node);
node.Cost = cost;
node.Parent = parent;
}
else
return;
}
float key = node.Cost + node.Heuristic;
if (!openList.ContainsKey(key))
openList[key] = new List<AStarNode>();
openList[key].Add(node);
}
private void RemoveFromOpenList(AStarNode node)
{
float key = node.Cost + node.Heuristic;
if (openList.ContainsKey(key))
{
openList[key].Remove(node);
if (openList[key].Count == 0)
openList.Remove(key);
}
}