Advertisement
corp0

Untitled

Nov 19th, 2024
31
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 2.75 KB | None | 0 0
  1.    private List<SelectableTile> RetracePath(PathNode endNode)
  2.     {
  3.         List<SelectableTile> path = [];
  4.         PathNode? currentNode = endNode;
  5.  
  6.         while (currentNode != null)
  7.         {
  8.             path.Add(currentNode.Tile);
  9.             currentNode = currentNode.Parent;
  10.         }
  11.  
  12.         path.Reverse();
  13.         return path;
  14.     }
  15.  
  16.     public List<SelectableTile> FindPath(SelectableTile startTile, SelectableTile endTile)
  17.     {
  18.         PriorityQueue<PathNode, float> openSet = new();
  19.         HashSet<SelectableTile> closedSet = [];
  20.         Dictionary<SelectableTile, PathNode> allNodes = new();
  21.         const float tileDistance = 1f; // assumes all tiles have the same cost of traversal
  22.  
  23.         PathNode startNode = new(startTile)
  24.         {
  25.             GCost = 0,
  26.             HCost = CalculateHeuristicCost(startTile, endTile)
  27.         };
  28.         allNodes[startTile] = startNode;
  29.         openSet.Enqueue(startNode, startNode.FCost);
  30.  
  31.         while (openSet.Count > 0)
  32.         {
  33.             PathNode currentNode = openSet.Dequeue();
  34.  
  35.             if (currentNode.Tile == endTile)
  36.             {
  37.                 return RetracePath(currentNode);
  38.             }
  39.  
  40.             closedSet.Add(currentNode.Tile);
  41.  
  42.             foreach (SelectableTile neighbour in currentNode.Tile.Neighbours)
  43.             {
  44.                 ProcessNeighbour(currentNode, neighbour, endTile, openSet, closedSet, allNodes, tileDistance);
  45.             }
  46.         }
  47.  
  48.         // No path found!
  49.         return [];
  50.     }
  51.    
  52.     private void ProcessNeighbour(
  53.         PathNode currentNode,
  54.         SelectableTile neighbour,
  55.         SelectableTile endTile,
  56.         PriorityQueue<PathNode, float> openSet,
  57.         HashSet<SelectableTile> closedSet,
  58.         Dictionary<SelectableTile, PathNode> allNodes,
  59.         float tileDistance)
  60.     {
  61.         if (closedSet.Contains(neighbour) || neighbour.IsTaken)
  62.             return;
  63.  
  64.         float tentativeGCost = currentNode.GCost + tileDistance;
  65.  
  66.         if (!allNodes.TryGetValue(neighbour, out PathNode? node))
  67.         {
  68.             node = new PathNode(neighbour)
  69.             {
  70.                 GCost = float.PositiveInfinity,
  71.                 HCost = CalculateHeuristicCost(neighbour, endTile)
  72.             };
  73.             allNodes[neighbour] = node;
  74.         }
  75.  
  76.         if (tentativeGCost < node.GCost)
  77.         {
  78.             node.GCost = tentativeGCost;
  79.             node.Parent = currentNode;
  80.             openSet.Enqueue(node, node.FCost);
  81.         }
  82.     }
  83.    
  84.     private static float CalculateHeuristicCost(SelectableTile from, SelectableTile to)
  85.     {
  86.         Vector3 fromPos = from.Position;
  87.         Vector3 toPos = to.Position;
  88.         return Mathf.Abs(fromPos.X - toPos.X) + Mathf.Abs(fromPos.Z - toPos.Z);
  89.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement