TDT4136-Assignment2/tempCodeRunnerFile.py at main · axelegkor/TDT4136-Assignment2 · 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
from Map import Map_Obj

## Function for calculation h(s), distance from s to goal
def h(s, goal_pos):
    return (abs(goal_pos[0] - s[0])**2 + abs(goal_pos[1] - s[1])**2)**0.5

## Function for calculation g(s), distance from start to s
def find_path(map_obj: Map_Obj):

    ## Initialize the open list. This will be a list of list, with each list containing a node and the path to that node
    open_list = []

    ## Initialize the closed list
    closed_list = []

    ## Add the start node to the open list
    open_list.append([map_obj.get_start_pos(), []])

    ## Loop until the open list is empty
    while open_list:

        ## Get the node with the lowest cost and the path to that node
        current = open_list.pop(0)

        ## Get the node with the lowest cost
        currentNode = current[0]

        ## Get the path to the node with the lowest cost
        currentHistory = current[1]

        ## Add the node with the lowest cost to the closed list
        closed_list.append(currentNode)

        ## If-statement to check if the current node is the goal node
        if currentNode == map_obj.get_goal_pos():

            ## Add the goal node to the path
            currentHistory.append(currentNode)

            ## Set the color of all nodes along the path to black
            for i in currentHistory[1:len(currentHistory)-1]:
                map_obj.replace_map_values(i, 4, map_obj.get_goal_pos())

            ## Return the path
            return currentHistory

        ## Get the walkable neighbors of the current node
        for i in map_obj.get_walkable_neighbors(currentNode):
            ## Ignore nodes that are already in the closed list
            if i not in closed_list:
                ## Color the node grey
                map_obj.replace_map_values(i, 2, map_obj.get_goal_pos())

                ## Create a list with the node and the path to that node
                new = [i, currentHistory + [currentNode]]

                ## Add the list to the right place in the open list
                j = 0
                while j < len(open_list) and len(open_list[j][1]) + h(open_list[j][0], map_obj.get_goal_pos()) < len(new[1]) + h(new[0], map_obj.get_goal_pos()):
                    j += 1
                if j == len(open_list):
                    open_list.append(new)
                else:
                    open_list.append(open_list[len(open_list) - 1])
                    for k in range(len(open_list) - 1, j, -1):
                        open_list[k] = open_list[k - 1]
                    open_list[j] = new

    ## If the open list is empty, and the goal node is not found, return None
    return None