/// <summary> /// Method to generate a List of nodes arranged in a square grid. /// Set randomWeight to true to randomize the weight of the nodes (Useful for PCG). /// </summary> public static List <wszpathfindernode> GenerateGrid(ushort sizeX, ushort sizeY, bool randomWeight) { Random random = new Random(); List <wszpathfindernode> grid = new List <wszpathfindernode>(); int i = sizeY; while (i-- > 0) { int j = sizeX; while (j-- > 0) { wszpathfindernode n = new wszpathfindernode(); n.X = j; n.Y = i; n.W = (randomWeight) ? (ushort)random.Next(1, 5) : (ushort)1; n.Children = new List <wszpathfindernode>(); grid.Add(n); } } i = sizeY; while (i-- > 0) { int j = sizeX; while (j-- > 0) { wszpathfindernode n = grid[i * sizeX + j]; int np = i * sizeX + j; if (np - sizeX >= 0 && grid[np - sizeX] != null) { n.Children.Add(grid[np - sizeX]); // Add top child if it exists. } if (j != 0 && grid[np - 1] != null) { n.Children.Add(grid[np - 1]); // Add left child if it exists. } if (np + sizeX < sizeY * sizeX && grid[np + sizeX] != null) { n.Children.Add(grid[np + sizeX]); // Add bottom child if it exists. } if (j != sizeX - 1 && grid[np + 1] != null) { n.Children.Add(grid[np + 1]); // Add right child if it exists. } } } return(grid); }
/// <summary> /// Main A* pathfinding algorithm, expects a Start and End nodes located in the List Grid. /// The useH param toggle between using an heuristic or breadth-first search, using an heuristic requires nodes with X and Y params. /// </summary> public List <wszpathfindernode> FindPath(wszpathfindernode Start, wszpathfindernode End, bool useH) { List <wszpathfindernode> Open = new List <wszpathfindernode>(); List <wszpathfindernode> Closed = new List <wszpathfindernode>(); Open.Add(Start); while (Open.Count > 0) { Open.Sort(Sort); wszpathfindernode A = Open[0]; if (A == End) { break; } Open.RemoveAt(0); byte i = (byte)A.Children.Count; // Not expecting the amount of children to be bigger than one byte. while (i-- > 0) { wszpathfindernode C = A.Children[i]; // A bit more memory to store the child by at least we're not gonna keep accessing the Children List. // If we are not using an heuristic (AKA have nodes with X, Y coordinates), use only G (Basically Breadth-First). int newF = (useH) ? C.G + GetH(C, End) : C.G; // Skip node if it's in the closed list. if (Closed.Contains(C)) { continue; } // If true, this node has already been processed and this new path isn't shorter, abort this check. if (Open.Contains(C) && C.F <= newF) { continue; } // Otherwise, update the values and add it to the open list. C.G = A.G + C.W; C.F = newF; C.P = A; Open.Add(C); } Closed.Add(A); } // Pathfinding is done, return the path List <wszpathfindernode> Path = new List <wszpathfindernode>(); wszpathfindernode N = Open[0]; while (N != null) { Path.Add(N); N = N.P; } Path.Reverse(); // Flush the parent info from the nodes to allow reusability for (byte i = 0; i < Grid.Count; i++) { Grid[i].P = null; } return(Path); }
/// <summary> /// The heuristic method used if the useH param of FindPath is set to true. /// </summary> private int GetH(wszpathfindernode Node, wszpathfindernode TargetNode) { return((int)Math.Sqrt((TargetNode.X - Node.X) * (TargetNode.X - Node.X) + (TargetNode.Y - Node.Y) * (TargetNode.Y - Node.Y))); }