Pathfinding Algorithms: BFS, DFS, A*, and Uniform Cost

Posted on Apr 4, 2025 in Computer Engineering

Breadth-First Search (BFS) and Depth-First Search (DFS)

from collections import deque
def bfs(maze, start, end):
  rows = len(maze)
  cols = len(maze[0])
  directions = [(0, 1), (1, 0), (0, -1), (-1, 0)]
  queue = deque([(start, [start])])
  visited = set()
  visited.add(start)
  node_visited = 0
  while queue:
    current, path = queue.popleft()
    node_visited += 1
    if current == end:
      return path, node_visited
    for r, c in directions:
      x = current[0] + r
      y = current[1] + c
      if 0 <= x < rows and 0 <= y < cols and maze[x][y] == 1 and (x, y) not in visited:
        visited.add((x, y))
        queue.append(((x, y), path + [(x, y)]))
  return [], node_visited
def dfs(maze, start, end):
  rows = len(maze)
  cols = len(maze[0])
  p = []
  directions = [(0, 1), (1, 0), (0, -1), (-1, 0)] #right,down,left,up
  stack = [(start, [start])]
  visited = set()
  visited.add(start)
  node_visited = 0
  while stack:
    current, path = stack.pop()
    node_visited += 1
    if current == end:
      p.append((path, node_visited))
      visited.remove(end)
    for r, c in directions:
      x, y = current[0] + r, current[1] + c
      if 0 <= x < rows and 0 <= y < cols and maze[x][y] == 1 and (x, y) not in visited:
        visited.add((x, y))
        stack.append(((x, y), path + [(x, y)]))



Bidirectional BFS

from collections import deque
def checkVisited(v1, v2):
    for x in v1:
        for y in v2:
            if x == y:
                return True
    return False
def mergePath(path1, path2):
    path1.pop()
    return path1 + path2, path2[0]
def bidirectionalBFS(maze, start, end):
    if start == end:
        return [start], 0, 0
    rows = len(maze)
    cols = len(maze[0])
    directions = [(0, 1), (1, 0), (0, -1), (-1, 0)]  # right down left up
    queue1 = deque([(start, [start])])
    queue2 = deque([(end, [end])])
    visited1 = set()
    visited2 = set()
    visited1.add(start)
    visited2.add(end)
    node_visited = 0
    while queue1 and queue2:
        curr1, path1 = queue1.popleft()
        curr2, path2 = queue2.popleft()
        isFound = checkVisited(visited1, visited2)
        if isFound:
            mergedPath, meeting_point = mergePath(path1, path2)
            return mergedPath, node_visited, meeting_point
        for r, c in directions:
            x, y = curr1[0] + r, curr1[1] + c
            if 0 <= x < rows and 0 <= y < cols and maze[x][y] == 1 and (x, y) not in visited1:
                node_visited += 1
                visited1.add((x, y))
                queue1.append(((x, y), path1 + [(x, y)]))
        isFound = checkVisited(visited1, visited2)
        if isFound:
            mergedPath, meeting_point = mergePath(path1, path2)


  return mergedPath, node_visited, meeting_point
        for r, c in directions:
            x, y = curr2[0] + r, curr2[1] + c
            if 0 <= x < rows and 0 <= y < cols and maze[x][y] == 1 and (x, y) not in visited2:
                node_visited += 1
                visited2.add((x, y))
                queue2.append(((x, y), [(x, y)] + path2))
return [], node_visited, 0 #BFS and DFS Code 

Greedy Best-First Search

from queue import PriorityQueue
import numpy as np
def reconstructPath(path, start, goal):#Implement the algorithm to always move to the most 
  resultPath = [] #promising cell first
  current = goal
  while current != start:
    resultPath.append(current)
    current = path[current]
  resultPath.append(start)
  resultPath.reverse()
  return resultPath
def greedyBestFirstSearch(grid, start, goal):
  rows, cols = len(grid), len(grid[0])
  pq = PriorityQueue()
  directions = [(-1, 0), (1, 0), (0, 1), (0, -1)]
  pq.put((grid[start[0]][start[1]], start))
  path = {start: None}
  visited = set()
  while not pq.empty():
    pos, current = pq.get()
    visited.add(current)
    if grid[current[0]][current[1]] == goal:
      return reconstructPath(path, start, current)
    for a, b in directions:
      x, y = current[0] + a, current[1] + b
      if 0 <= x < rows and 0 <= y < cols and (x, y) not in visited:
        pq.put((grid[x][y], (x, y)))
        visited.add((x, y))
        path[(x, y)] = current
  return "No Path Found"


grid = [
  [7, 5, 3, 2, 4],
  [8, 5, 5, 2, 2],
  [5, 6, 2, 1, 3],
  [4, 3, 3, 2, 5]
]
start = (0, 0), goal = 1
path = greedyBestFirstSearch(grid, start, goal)
print("path:", path)

Manhattan Distance Heuristic

# Task 2: Use Manhattan distance as a heuristic function
def manHattanDistance(cell, goal):
  return abs(cell[0] - goal[0]) + abs(cell[1] - goal[1])
def withManhattan(grid, start, goal):
  rows, cols = len(grid), len(grid[0])
  directions = [(-1, 0), (1, 0), (0, 1), (0, -1)]
  pq = PriorityQueue()
  pq.put((manHattanDistance(start, goal), start))
  path = {start: None}
  visited = set()
  while not pq.empty():
    pos, current = pq.get()
    visited.add(current)
    if current == goal:
      return reconstructPath(path, start, goal)
    for a, b in directions:
      x, y = current[0] + a, current[1] + b
      if 0 <= x < rows and 0 <= y < cols and (x, y) not in visited and grid[x][y] != 0:
        pq.put((manHattanDistance((x, y), goal), (x, y)))
        visited.add((x, y))
        path[(x, y)] = current
  return "No Path Found"
grid = [ [1, 1, 0, 1, 1],[1, 1, 1, 1, 0],[0, 1, 0, 1, 1],[1, 1, 1, 1, 1]]
start = (0, 0), goal = (2, 3)
path = withManhattan(grid, start, goal)
print("path:", path)


Uniform Cost Search

from queue import PriorityQueue
from collections import deque
def reconstructPath(visited, start, goal):
    path = []
    current = goal
    while(current != start):
        path.append(current)
current = visited[current][1]
    path.append(start)
    path.reverse()
    return path
def uniformCostSearch(graph, start, goal):
    pq = PriorityQueue()
    pq.put((0, start))
    visited = {start: (0, None)}
    while not pq.empty():
        curr_cost, curr_node = pq.get()
        if curr_node == goal:
            return curr_cost, reconstructPath(visited, start, goal)
        for neigh, cost in graph[curr_node]:
            total_cost = curr_cost + cost
            if neigh not in visited or total_cost < visited[neigh][0]:
                visited[neigh] = (total_cost, curr_node)
                pq.put((total_cost, neigh))
    return "No Path Found!"
graph = { 'A' : [('B', 1), ('C', 4)],'B' : [('A', 1), ('C', 2), ('D', 5)], 'C' : [('A', 4), ('B', 2), ('D', 1)],
    'D' : [('B', 5), ('C', 1)]}
start = 'A' , goal = 'D'
cost, path = uniformCostSearch(graph, start, goal)
print("Path: ", path, "Cost: ", cost)


A* Search Algorithm on a 2D Grid

#A* -> 2d Grid 

import heapq

def heuristic(node, goal):

    # Manhattan Distance (for grid-based pathfinding)

    return abs(node[0] - goal[0]) + abs(node[1] - goal[1])

def reconstruct_path(parent, current):

    path = []

    while current in parent:

        path.append(current)

        current = parent[current]

    path.append(current)  # Add the start node

    return path[::-1]  # Reverse path

def a_star_search(start, goal, neighbors):

    open_set = []

    heapq.heappush(open_set, (0, start))  # (fScore, node)

    closed_set = set()

    parent = {}

    g_score = {start: 0}  # Cost from start to current node

    f_score = {start: heuristic(start, goal)}

    while open_set:

        _, current = heapq.heappop(open_set)

        if current == goal:

            return reconstruct_path(parent, current)

        closed_set.add(current)

        for neighbor in neighbors(current):

            if neighbor in closed_set:

                continue

            tentative_g_score = g_score[current] + 1  # Assuming uniform cost of 1

            if tentative_g_score < g_score.get(neighbor, float('inf')):

parent[neighbor] = current

                  g_score[neighbor] = tentative_g_score

                  f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal)

                  if neighbor not in [n for _, n in open_set]:

                      heapq.heappush(open_set, (f_score[neighbor], neighbor))

    return []  # No path found

def get_neighbors(node):

    x, y = node

    return [(x+1, y), (x-1, y), (x, y+1), (x, y-1)]  # 4-way movement

start = (0, 0), goal = (2, 2)

path = a_star_search(start, goal, get_neighbors)

print("Optimal Path:", path)