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); } }