python-wavefront/wavefront.py at master · czhanacek/python-wavefront · 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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
#!/usr/bin/env python
import time
import sys

def newSearch(mapOfWorld, goal, start):
    heap = []
    newheap = []
    x, y = goal
    lastwave = 3
    # Start out by marking nodes around G with a 3
    moves = [(x + 1, y), (x - 1, y), (x, y - 1), (x, y + 1)]

    for move in moves:
        if(mapOfWorld.positions[move] == ' '):
            mapOfWorld.positions[move] = 3
            heap.append(move)
    for currentwave in range(4, 10000):
        lastwave = lastwave + 1
        while(heap != []):
            position = heap.pop()
            (x, y) = position
            moves = [(x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)]
            #x, y = position
            for move in moves:
                if(mapOfWorld.positions[move] != 'W'):
                    if(mapOfWorld.positions[move] == ' ' and mapOfWorld.positions[position] == currentwave - 1):
                        mapOfWorld.positions[move] = currentwave
                        newheap.append(move)
                    if(move == start):
                        return mapOfWorld, lastwave

        time.sleep(0.25)
        mapOfWorld.display()
        #print heap
        if(newheap == []):
            print "Goal is unreachable"
            return 1
        heap = newheap
        newheap = []

def printf(format, *args):
    sys.stdout.write(format % args)
class Map(object):

    def __init__(self, xdim, ydim, positions):
        self.xdim = xdim
        self.ydim = ydim
        self.positions = positions
    def display(self):
        printf("  ")
        for i in range(self.ydim):
            printf("%3s", str(i))
        print
        for x in range(self.xdim):
            printf("%2s", str(x))
            for y in range(self.ydim):
                printf("%3s", str(self.positions[(x, y)]))
            print
    # Navigate though the number-populated maze
    def nav(self, start, current):
        self.pos = start
        finished = False

        while(finished == False): # Run this code until we're at the goal
            x, y = self.pos
            self.positions[self.pos] = 'R' # Set the start on the map (this USUALLY keeps start the same)
            #         SOUTH        NORTH         WEST      EAST
            #           v           v             v          v
            moves = [(x + 1, y), (x - 1, y), (x, y - 1), (x, y + 1)] # Establish our directions
            moveDirections = ["South", "North", "West", "East"] # Create a corresponding list of the cardinal directions
            """ We don't want least to be 0, because then nothing would be less than it.
                However, in order to make our code more robust, we set it to one of the values,
                so that we're comparing least to an actual value instead of an arbitrary number (like 10).
            """
            # Do the actual comparing, and give us the least index so we know which move was the least
            for w in range(len(moves)):
                move = moves[w]

                # If the position has the current wave - 1 in it, move there.
                if(self.positions[move] == current - 1):
                    self.least = self.positions[move]
                    leastIndex = w
                # Or, if the position is the goal, stop the loop
                elif(self.positions[move] == 'G'):
                    finished = True
                    leastIndex = w
            # Decrement the current number so we can look for the next number
            current = current - 1
            self.positions[self.pos] = ' '
            print "Moved " + moveDirections[leastIndex]
            self.pos = moves[leastIndex] # This will be converted to "move robot in x direction"

            time.sleep(0.25)
            self.display()
        # Change the goal position (or wherever we stop) to an "!" to show that we've arrived.
        self.positions[self.pos] = '!'
        self.display()
# Find the goal, given the map
def findGoal(mapOfWorld):
    positions = mapOfWorld.positions
    for x in range(mapOfWorld.xdim):
        for y in range(mapOfWorld.ydim):
            if(mapOfWorld.positions[(x, y)] == 'G'):
                return (x, y)
# Find the start, given the map
def findStart(mapOfWorld):
    positions = mapOfWorld.positions
    for x in range(mapOfWorld.xdim):
        for y in range(mapOfWorld.ydim):
            if(mapOfWorld.positions[(x, y)] == 'R'):

                return (x, y)

def convertMap(mapOfWorld):
    positions = {}
    xdim = len(mapOfWorld)
    ydim = len(mapOfWorld[1])
    for y in range(ydim):
        for x in range(xdim):
            positions[(x, y)] = mapOfWorld[x][y]

    return Map(xdim, ydim, positions)

mapOfWorld = [['W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W'],
              ['W', ' ', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'G', 'W'],
              ['W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W', 'W'],
              ['W', 'W', 'W', 'W', 'W', 'W', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', ' ', ' ', ' ', 'W', ' ', ' ', 'W', 'W', 'W', ' ', 'W'],
              ['W', ' ', 'W', 'W', ' ', 'W', ' ', 'W', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', 'W', 'W', 'W', 'W', ' ', 'W', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', 'W', ' ', 'W', ' ', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', 'W', ' ', 'W', 'W', 'W', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', 'W', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', ' ', ' ', ' ', ' ', ' ', 'W', 'W', 'W', 'W', 'W', 'W'],
              ['W', ' ', 'W', 'W', ' ', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', 'W', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', ' ', 'W', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', 'W', 'W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', ' ', 'W', 'W', ' ', 'W', 'W', 'W', 'W', 'W', 'W', ' ', 'W'],
              ['W', ' ', ' ', ' ', ' ', 'W', 'W', ' ', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W', ' ', 'W', ' ', 'W'],
              ['W', ' ', 'W', ' ', 'W', 'W', 'W', 'W', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W', ' ', 'W'],
              ['W', ' ', 'W', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'W'],
              ['W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W', 'W'],]

mapOfLand = convertMap(mapOfWorld)
mapOfLand.display()
mapOfLand, lastwave = newSearch(mapOfLand, findGoal(mapOfLand), findStart(mapOfLand))
mapOfLand.nav(findStart(mapOfLand), lastwave)

#currentSearch(mapOfWorld, findGoal(mapOfWorld), findStart(mapOfWorld))