ℹ️ Select 'Choose Exercise', or randomize 'Next Random Exercise' in selected language.

Choose Exercise:
Timer 00:00
WPM --
Score --
Acc --
Correct chars --

Pathfinding with A* Algorithm and Heuristic Functions

C# (Unity)

Goal -- WPM

Ready
Exercise Algorithm Area
1using System;

2using System.Collections.Generic;

3using System.Linq;

4using UnityEngine;

5

6// Represents a node in the pathfinding grid

7public class PathNode

8{

9public Vector2Int GridPosition;

10public bool IsWalkable;

11public float G_Cost; // Cost from start node to this node

12public float H_Cost; // Heuristic cost from this node to the end node

13public float F_Cost { get { return G_Cost + H_Cost; } } // Total cost

14public PathNode ParentNode;

15

16public PathNode(Vector2Int gridPosition, bool isWalkable = true)

17{

18GridPosition = gridPosition;

19IsWalkable = isWalkable;

20G_Cost = float.MaxValue;

21H_Cost = 0;

22ParentNode = null;

23}

24

25public override bool Equals(object obj)

26{

27if (obj is PathNode other)

28{

29return GridPosition == other.GridPosition;

30}

31return false;

32}

33

34public override int GetHashCode()

35{

36return GridPosition.GetHashCode();

37}

38}

39

40// Represents the pathfinding grid

41public class PathfindingGrid

42{

43private PathNode[,] grid;

44private int width;

45private int height;

46

47public PathfindingGrid(int width, int height, bool[,] obstacles)

48{

49this.width = width;

50this.height = height;

51grid = new PathNode[width, height];

52

53for (int x = 0; x < width; x++)

54{

55for (int y = 0; y < height; y++)

56{

57grid[x, y] = new PathNode(new Vector2Int(x, y), !obstacles[x, y]);

58}

59}

60}

61

62public PathNode GetNode(Vector2Int position)

63{

64if (position.x >= 0 && position.x < width && position.y >= 0 && position.y < height)

65{

66return grid[position.x, position.y];

67}

68return null; // Out of bounds

69}

70

71public IEnumerable<PathNode> GetNeighbours(PathNode node)

72{

73Vector2Int[] directions = { new Vector2Int(0, 1), new Vector2Int(0, -1), new Vector2Int(1, 0), new Vector2Int(-1, 0), // Cardinal directions

74new Vector2Int(1, 1), new Vector2Int(1, -1), new Vector2Int(-1, 1), new Vector2Int(-1, -1) }; // Diagonal directions

75

76foreach (var dir in directions)

77{

78Vector2Int neighbourPos = node.GridPosition + dir;

79PathNode neighbour = GetNode(neighbourPos);

80if (neighbour != null && neighbour.IsWalkable)

81{

82yield return neighbour;

83}

84}

85}

86

87public int Width { get { return width; } }

88public int Height { get { return height; } }

89}

90

91// A* Pathfinding Algorithm Implementation

92public class AStarPathfinder

93{

94private PathfindingGrid grid;

95

96public AStarPathfinder(PathfindingGrid grid)

97{

98this.grid = grid;

99}

100

101// Heuristic function (Manhattan distance for grid-based movement)

102private float CalculateHeuristic(Vector2Int from, Vector2Int to)

103{

104// Using Manhattan distance for grid movement without diagonals

105// For diagonal movement, Euclidean or Chebyshev distance might be better.

106// For this example, we'll use Manhattan for simplicity, assuming 4-way movement.

107// If diagonal movement is allowed and costs 1.4, then Chebyshev is better.

108// Let's stick to Manhattan for now, assuming cost of 1 for cardinal moves.

109float dx = Mathf.Abs(from.x - to.x);

110float dy = Mathf.Abs(from.y - to.y);

111return dx + dy;

112}

113

114public List<Vector2Int> FindPath(Vector2Int startPos, Vector2Int endPos)

115{

116PathNode startNode = grid.GetNode(startPos);

117PathNode endNode = grid.GetNode(endPos);

118

119if (startNode == null || endNode == null || !startNode.IsWalkable || !endNode.IsWalkable)

120{

121Debug.LogError("Start or end node is invalid or not walkable.");

122return new List<Vector2Int>(); // Invalid start or end

123}

124

125// Open set: nodes to be evaluated, prioritized by F_Cost

126// Using a List and sorting for simplicity, a PriorityQueue would be more efficient

127List<PathNode> openSet = new List<PathNode>();

128

129// Closed set: nodes already evaluated

130HashSet<PathNode> closedSet = new HashSet<PathNode>();

131

132// Initialize start node

133startNode.G_Cost = 0;

134startNode.H_Cost = CalculateHeuristic(startPos, endPos);

135openSet.Add(startNode);

136

137while (openSet.Count > 0)

138{

139// Sort openSet to get the node with the lowest F_Cost

140openSet.Sort((a, b) => a.F_Cost.CompareTo(b.F_Cost));

141PathNode currentNode = openSet[0];

142

143// If we reached the end node, reconstruct and return the path

144if (currentNode == endNode)

145{

146return ReconstructPath(currentNode);

147}

148

149// Move current node from open set to closed set

150openSet.RemoveAt(0);

151closedSet.Add(currentNode);

152

153// Explore neighbours

154foreach (PathNode neighbour in grid.GetNeighbours(currentNode))

155{

156// Skip if neighbour is already evaluated or not walkable

157if (closedSet.Contains(neighbour) || !neighbour.IsWalkable)

158{

159continue;

160}

161

162// Calculate tentative G_Cost for the neighbour

163// Assuming cost of 1 for cardinal moves. For diagonal, it would be sqrt(2) or 1.4.

164float tentativeGCost = currentNode.G_Cost + Vector2Int.Distance(currentNode.GridPosition, neighbour.GridPosition); // Using Vector2Int.Distance for cardinal and diagonal cost

165

166// If this path to the neighbour is better than any previous one

167if (tentativeGCost < neighbour.G_Cost)

168{

169neighbour.ParentNode = currentNode;

170neighbour.G_Cost = tentativeGCost;

171neighbour.H_Cost = CalculateHeuristic(neighbour.GridPosition, endPos);

172

173// If neighbour is not in the open set, add it

174if (!openSet.Contains(neighbour))

175{

176openSet.Add(neighbour);

177}

178// If it is in the open set, its costs have been updated, and it will be re-prioritized by the sort.

179}

180}

181}

182

183// If the loop finishes and we haven't found the end, the path is unreachable

184Debug.LogWarning("Path not found.");

185return new List<Vector2Int>(); // Path not found

186}

187

188// Reconstructs the path from the end node back to the start node

189private List<Vector2Int> ReconstructPath(PathNode endNode)

190{

191List<Vector2Int> path = new List<Vector2Int>();

192PathNode currentNode = endNode;

193

194while (currentNode != null)

195{

196path.Add(currentNode.GridPosition);

197currentNode = currentNode.ParentNode;

198}

199

200path.Reverse(); // Path is reconstructed backwards, so reverse it

201return path;

202}

203

204// Example Usage (would typically be in a MonoBehaviour in Unity)

205public static void Main(string[] args)

206{

207// Define grid dimensions and obstacles

208int gridWidth = 10;

209int gridHeight = 10;

210bool[,] obstacles = new bool[gridWidth, gridHeight];

211

212// Add some obstacles

213for (int i = 0; i < gridWidth; i++) obstacles[i, 5] = true; // Horizontal wall

214for (int i = 0; i < gridHeight; i++) obstacles[5, i] = true; // Vertical wall

215obstacles[3, 3] = true;

216obstacles[4, 3] = true;

217obstacles[5, 3] = true;

218obstacles[6, 3] = true;

219

220PathfindingGrid pathGrid = new PathfindingGrid(gridWidth, gridHeight, obstacles);

221AStarPathfinder pathfinder = new AStarPathfinder(pathGrid);

222

223Vector2Int start = new Vector2Int(0, 0);

224Vector2Int end = new Vector2Int(9, 9);

225

226List<Vector2Int> path = pathfinder.FindPath(start, end);

227

228if (path.Count > 0)

229{

230Console.WriteLine("Path found:");

231foreach (var pos in path)

232{

233Console.Write($"({pos.x},{pos.y}) -> ");

234}

235Console.WriteLine("End");

236}

237else

238{

239Console.WriteLine("Could not find a path.");

240}

241

242// Test unreachable destination

243Vector2Int unreachableEnd = new Vector2Int(5, 5); // Inside a wall

244List<Vector2Int> unreachablePath = pathfinder.FindPath(start, unreachableEnd);

245if (unreachablePath.Count == 0)

246{

247Console.WriteLine("Correctly identified unreachable destination.");

248}

249}

250}
Algorithm description viewbox

Pathfinding with A* Algorithm and Heuristic Functions

Algorithm description:

This C# code implements the A* pathfinding algorithm, a widely used technique for finding the shortest path between two points in a grid or graph. It utilizes a heuristic function to estimate the cost to the goal, guiding the search efficiently. The implementation manages open and closed sets of nodes, calculates movement costs, and reconstructs the path once the destination is reached. This is essential for AI agents in games to navigate complex environments.

Algorithm explanation:

The A* algorithm finds the shortest path by exploring nodes based on a cost function `F = G + H`, where `G` is the cost from the start node and `H` is the heuristic estimate to the end node. The `PathfindingGrid` represents the navigable space, and `PathNode` stores information about each grid cell. The `AStarPathfinder` uses an `openSet` (nodes to visit, prioritized by `F_Cost`) and a `closedSet` (nodes already visited). It iteratively selects the node with the lowest `F_Cost` from the `openSet`, adds it to the `closedSet`, and examines its neighbors. If a neighbor can be reached with a lower `G_Cost` than previously known, its parent and costs are updated, and it's added to the `openSet`. The path is reconstructed by backtracking from the goal node using parent pointers. Time complexity is typically O(E log V) or O(E + V log V) with a priority queue, where V is the number of nodes and E is the number of edges. Space complexity is O(V) for storing the open and closed sets. Edge cases include invalid start/end nodes, unreachable destinations, and obstacles, all handled by checking node walkability and the loop termination condition.

Pseudocode:

Class PathNode:
  GridPosition
  IsWalkable
  G_Cost (float, default infinity)
  H_Cost (float)
  F_Cost (calculated: G_Cost + H_Cost)
  ParentNode

Class PathfindingGrid:
  Grid (2D array of PathNode)
  Width, Height
  Constructor(width, height, obstacles):
    Initialize grid with PathNodes, setting IsWalkable based on obstacles
  Method GetNode(position):
    Return node at position if within bounds, else null
  Method GetNeighbours(node):
    Yield walkable neighbours of node

Class AStarPathfinder:
  Grid
  Constructor(grid):
    Set Grid

  Method CalculateHeuristic(from, to):
    Return Manhattan distance (or other heuristic)

  Method FindPath(startPos, endPos):
    Get startNode and endNode from Grid
    If startNode or endNode is null or not walkable, return empty list

    openSet (List of PathNode, sorted by F_Cost)
    closedSet (HashSet of PathNode)

    Initialize startNode: G_Cost = 0, H_Cost = CalculateHeuristic(startPos, endPos)
    Add startNode to openSet

    While openSet is not empty:
      Sort openSet by F_Cost
      currentNode = first element of openSet
      Remove currentNode from openSet

      If currentNode is endNode:
        Return ReconstructPath(currentNode)

      Add currentNode to closedSet

      For each neighbour of currentNode:
        If neighbour is in closedSet or not walkable, continue

        tentativeGCost = currentNode.G_Cost + distance(currentNode, neighbour)

        If tentativeGCost < neighbour.G_Cost:
          neighbour.ParentNode = currentNode
          neighbour.G_Cost = tentativeGCost
          neighbour.H_Cost = CalculateHeuristic(neighbour.GridPosition, endPos)

          If neighbour is not in openSet:
            Add neighbour to openSet

    Return empty list (path not found)

  Method ReconstructPath(endNode):
    path list
    currentNode = endNode
    While currentNode is not null:
      Add currentNode.GridPosition to path list
      currentNode = currentNode.ParentNode
    Reverse path list
    Return path list