Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- private List<SelectableTile> RetracePath(PathNode endNode)
- {
- List<SelectableTile> path = [];
- PathNode? currentNode = endNode;
- while (currentNode != null)
- {
- path.Add(currentNode.Tile);
- currentNode = currentNode.Parent;
- }
- path.Reverse();
- return path;
- }
- public List<SelectableTile> FindPath(SelectableTile startTile, SelectableTile endTile)
- {
- PriorityQueue<PathNode, float> openSet = new();
- HashSet<SelectableTile> closedSet = [];
- Dictionary<SelectableTile, PathNode> allNodes = new();
- const float tileDistance = 1f; // assumes all tiles have the same cost of traversal
- PathNode startNode = new(startTile)
- {
- GCost = 0,
- HCost = CalculateHeuristicCost(startTile, endTile)
- };
- allNodes[startTile] = startNode;
- openSet.Enqueue(startNode, startNode.FCost);
- while (openSet.Count > 0)
- {
- PathNode currentNode = openSet.Dequeue();
- if (currentNode.Tile == endTile)
- {
- return RetracePath(currentNode);
- }
- closedSet.Add(currentNode.Tile);
- foreach (SelectableTile neighbour in currentNode.Tile.Neighbours)
- {
- ProcessNeighbour(currentNode, neighbour, endTile, openSet, closedSet, allNodes, tileDistance);
- }
- }
- // No path found!
- return [];
- }
- private void ProcessNeighbour(
- PathNode currentNode,
- SelectableTile neighbour,
- SelectableTile endTile,
- PriorityQueue<PathNode, float> openSet,
- HashSet<SelectableTile> closedSet,
- Dictionary<SelectableTile, PathNode> allNodes,
- float tileDistance)
- {
- if (closedSet.Contains(neighbour) || neighbour.IsTaken)
- return;
- float tentativeGCost = currentNode.GCost + tileDistance;
- if (!allNodes.TryGetValue(neighbour, out PathNode? node))
- {
- node = new PathNode(neighbour)
- {
- GCost = float.PositiveInfinity,
- HCost = CalculateHeuristicCost(neighbour, endTile)
- };
- allNodes[neighbour] = node;
- }
- if (tentativeGCost < node.GCost)
- {
- node.GCost = tentativeGCost;
- node.Parent = currentNode;
- openSet.Enqueue(node, node.FCost);
- }
- }
- private static float CalculateHeuristicCost(SelectableTile from, SelectableTile to)
- {
- Vector3 fromPos = from.Position;
- Vector3 toPos = to.Position;
- return Mathf.Abs(fromPos.X - toPos.X) + Mathf.Abs(fromPos.Z - toPos.Z);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement