Maze-Solver/Pathfinding.py at main · Blubby24/Maze-Solver · 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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
import math
from random import random
from PriorityQueue import PriorityQueue


def breadthFirst(graph, start, end , screen):
    queue = [start]
    vistedNodes = set()
    while len(queue) > 0:
        vistedNodes.add(queue[0])
        if queue[0] == end:
            break
        for i, a in enumerate(graph[queue.pop(0)]):
            if a not in vistedNodes:
                queue.append(a)
        screen.drawPath(vistedNodes)


def depthFirst(graph, start, end, screen):
    stack = [start]
    vistedNodes = set()
    while len(stack) > 0:
        if stack[0] in vistedNodes:
            stack.pop(0)
            continue
        if stack[len(stack) - 1] == end:
            break
        vistedNodes.add(stack[len(stack) - 1])
        for i, a in enumerate(graph[stack.pop(len(stack) - 1)]):
            if a in vistedNodes:
                continue
            stack.append(a)
        screen.drawPath(vistedNodes)


def checkPath(graph, start, target):
    stack = [start]
    vistedNodes = set()
    while len(stack) > 0:
        vistedNodes.add(stack[len(stack) - 1])
        if stack[len(stack) - 1] == target:
            break
        for i, a in enumerate(graph[stack.pop(len(stack) - 1)]):
            if a in vistedNodes:
                continue
            stack.append(a)
        # screen.drawPath(vistedNodes)


def shortestPath(m, start, destination, screen):
    screen.drawStartandEnd(start, destination)
    table = {}
    unvisted = set()
    visted = set()
    stack = [start]
    for vertex in m:
        table[vertex] = [math.inf, '']
        unvisted.add(vertex)
    table[start] = [0, '']
    # I should make this a breadth first search it should in theory be faster, but it will look less cool
    while len(stack) > 0:
        # This is depth first
        # current = stack.pop(len(stack) - 1)
        # This is breath first
        current = stack.pop(0)
        visted.add(current)
        unvisted.remove(current)
        screen.drawPath(visted)
        for i, a in enumerate(m[current]):
            if a in unvisted:
                if table[a][0] > table[current][0] + 1:
                    table[a][0] = table[current][0] + 1
                    table[a][1] = current
                stack.append(a)
        if current == destination:
            break
    curr = destination
    path = set()
    while curr != start:
        path.add(curr)
        curr = table[curr][1]
    screen.drawStartandEnd(start, destination)
    return path

    # checkPath(m, start, destination)


def distanceFromNode(vertex, location):
    destX, destY, startX, startY = vertex[1], vertex[0], location[1], location[0]
    return abs(startX - destX) + abs(startY - destY)


def aStar(m, start, destination, screen):
    screen.drawStartandEnd(start, destination)
    table = {}
    unvisted = set()
    visted = set()
    queue = PriorityQueue()
    queue.put(start, 0)
    for vertex in m:
        table[vertex] = [math.inf, '']
        unvisted.add(vertex)
    table[start] = [0, '']
    while queue.size() > 0:
        current = queue.get()
        visted.add(current)
        unvisted.remove(current)
        screen.drawPath(visted)
        if current == destination:
            break
        for i, a in enumerate(m[current]):
            if a in unvisted:
                if table[a][0] > table[current][0] + 1:
                    table[a][0] = table[current][0] + 1
                    table[a][1] = current
                queue.put(a, distanceFromNode(current, destination))
    curr = destination
    path = set()
    while curr != start:
        path.add(curr)
        curr = table[curr][1]
    screen.drawStartandEnd(start, destination)
    return path


def pickRandomStartandEnd(m):
    startX = int(random() * len(m))
    startY = int(random() * len(m[0]))
    while m[startY][startX] == 1:
        startX = int(random() * len(m))
        startY = int(random() * len(m[0]))
    endX = int(random() * len(m))
    endY = int(random() * len(m[0]))
    while m[endY][endX] == 1:
        endX = int(random() * len(m))
        endY = int(random() * len(m[0]))
    return [startY, startX, endY, endX]