Path-Planning-Visualization/planner.py at main · AdevLog/Path-Planning-Visualization · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
import heapq
from collections import deque


# ── shared ────────────────────────────────────────────────────
def _manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def _reconstruct(came_from, start, goal):
    path, node = [], goal
    while node != start:
        path.append(node)
        node = came_from[node]
    path.append(start)
    path.reverse()
    return path


# ── A* ────────────────────────────────────────────────────────
def astar(grid):
    start = grid.start
    goal  = grid.goal

    open_set = []
    heapq.heappush(open_set, (0, start))

    came_from = {}
    g_score   = {start: 0}
    closed    = set()
    explored_order = []

    while open_set:
        _, current = heapq.heappop(open_set)

        if current in closed:
            continue
        closed.add(current)
        explored_order.append(current)

        if current == goal:
            return explored_order, _reconstruct(came_from, start, goal)

        for nb in grid.get_neighbors(*current):
            tentative_g = g_score[current] + 1
            if nb not in g_score or tentative_g < g_score[nb]:
                came_from[nb] = current
                g_score[nb]   = tentative_g
                f              = tentative_g + _manhattan(nb, goal)
                heapq.heappush(open_set, (f, nb))

    return explored_order, []


# ── BFS ───────────────────────────────────────────────────────
def bfs(grid):
    """
    Breadth-First Search — expands nodes level by level (FIFO queue).
    Guarantees shortest path on an unweighted grid, but explores blindly.
    """
    start = grid.start
    goal  = grid.goal

    queue     = deque([start])
    came_from = {start: None}
    explored_order = []

    while queue:
        current = queue.popleft()
        explored_order.append(current)

        if current == goal:
            return explored_order, _reconstruct(came_from, start, goal)

        for nb in grid.get_neighbors(*current):
            if nb not in came_from:
                came_from[nb] = current
                queue.append(nb)

    return explored_order, []