public static List <T> CalculatePath <T>(T[,] grid, int startX, int startY, int endX, int endY, List <int> walkableIDs) where T : Node { List <T> ret = new List <T>(); int xL = grid.GetLength(0), yL = grid.GetLength(1); //out of bounds if (startX < 0 || startY < 0 || startX >= xL || startY >= yL) { return(ret); } //main lists List <NodePathable> open = new List <NodePathable>() { new NodePathable(grid[startX, startY]) }; List <Node> closed = new List <Node>(); //shortcuts NodePathable current, addable; Node n; List <NodePathable> adjecent = new List <NodePathable>(); Node destination = grid[endX, endY]; sortRoot = new Vector2(endX, endY); while (open.Count > 0) { //sort open.SuperSort(SortNodes); //update lists current = open[0]; closed.Add(open[0].node); open.RemoveAt(0); n = current.node; //check borders adjecent.Clear(); //top if (n.y + 1 < yL) { addable = new NodePathable(grid[n.x, n.y + 1], current); if (!open.Contains(addable) && !closed.Contains(addable.node)) { if (walkableIDs.Contains(addable.node.typeID)) { adjecent.Add(addable); } } } //right if (n.x + 1 < xL) { addable = new NodePathable(grid[n.x + 1, n.y], current); if (!open.Contains(addable) && !closed.Contains(addable.node)) { if (walkableIDs.Contains(addable.node.typeID)) { adjecent.Add(addable); } } } //bottom if (n.y - 1 > 0) { addable = new NodePathable(grid[n.x, n.y - 1], current); if (!open.Contains(addable) && !closed.Contains(addable.node)) { if (walkableIDs.Contains(addable.node.typeID)) { adjecent.Add(addable); } } } //left if (n.x - 1 > 0) { addable = new NodePathable(grid[n.x - 1, n.y], current); if (!open.Contains(addable) && !closed.Contains(addable.node)) { if (walkableIDs.Contains(addable.node.typeID)) { adjecent.Add(addable); } } } foreach (NodePathable node in adjecent) { open.Add(node); current = node; if (node.node == destination) { while (current.parent != null) { ret.Add(current.parent.node as T); current = current.parent; } return(ret); } } } return(ret); }
private static float SortNodes(NodePathable node) { return(Vector2.Distance(new Vector2(node.node.x, node.node.y), sortRoot)); }
public NodePathable(Node node, NodePathable parent) { this.node = node; this.parent = parent; }