RaycastingGame/pathfinding.py at main · pslawx/RaycastingGame · 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
from collections import deque
from settings import MAX_DEPTH

class PathFinding:
    def __init__(self, game):
        self.game = game
        self.map = game.map.mini_map
        self.ways = [-1, 0], [0, -1], [1, 0], [0, 1], [-1, -1], [1, -1], [1, 1], [-1, 1]
        self.graph = {}
        self.get_graph()

    def get_path (self, start, goal):
        distance = self.calculated_distance(start, goal)
        # print(distance > (MAX_DEPTH + 50))
        # print(distance)
        if distance > (MAX_DEPTH + 3):
            return start
        # else:
        #     print(distance > (MAX_DEPTH + 3))
        #     print(distance)
        self.visited = self.bfs(start, goal, self.graph)
        path = [goal]
        step = self.visited.get(goal, start)

        while step and step != start:
            path.append(step)
            step = self.visited[step]
        return path[-1]

    # def bfs(self, start, goal, graph):
    #     queue = deque([start])
    #     visited = {start: None}

    #     while queue:
    #         cur_node = queue.popleft()
    #         if cur_node == goal:
    #             break
    #         next_nodes = graph[cur_node]

    #         for next_node in next_nodes:
    #             if next_node not in visited:
    #                 queue.append(next_node)
    #                 visited[next_node] = cur_node
    #     return visited

    def calculated_distance(self, start, goal):
        # print('start', start, 'start[0]', start[0])
        return ((start[0] - goal[0]) ** 2 + (start[1] - goal[1]) ** 2) ** 0.5

    def bfs(self, start, goal, graph):
        queue = deque([start])
        visited = {start: None}

        while queue:
            cur_node = queue.popleft()
            if cur_node == goal:
                break
            if cur_node in graph:  # Check if the node exists in the graph
                next_nodes = graph[cur_node]
                for next_node in next_nodes:
                    if next_node not in visited and next_node not in self.game.object_handler.npc_positions:
                        queue.append(next_node)
                        visited[next_node] = cur_node
        return visited

    def get_next_nodes(self, x, y):
        return [(x + dx, y + dy) for dx, dy in self.ways if (x + dx, y + dy) not in self.game.map.world_map]

    def get_graph(self):
        for y, row in enumerate(self.map):
            for x, col in enumerate(row):
                if not col:
                    self.graph[(x, y)] = self.graph.get((x, y), []) + self.get_next_nodes(x, y)