└── dstar.py


/dstar.py:
--------------------------------------------------------------------------------
  1 | # coding=utf-8
  2 | 
  3 | import math
  4 | from sys import maxsize
  5 | 
  6 | 
  7 | class State(object):
  8 | 
  9 |     def __init__(self, x, y):
 10 |         self.x = x
 11 |         self.y = y
 12 |         self.parent = None
 13 |         self.state = "."
 14 |         self.t = "new"
 15 |         self.h = 0
 16 |         self.k = 0
 17 | 
 18 |     def cost(self, state):
 19 |         if self.state == "#" or state.state == "#":
 20 |             return maxsize
 21 |         return math.sqrt(math.pow((self.x - state.x), 2) +
 22 |                          math.pow((self.y - state.y), 2))
 23 | 
 24 |     def set_state(self, state):
 25 |         if state not in ["s", ".", "#", "e", "*"]:
 26 |             return
 27 |         self.state = state
 28 | 
 29 | 
 30 | class Map(object):
 31 | 
 32 |     def __init__(self, row, col):
 33 |         self.row = row
 34 |         self.col = col
 35 |         self.map = self.init_map()
 36 | 
 37 |     def init_map(self):
 38 |         map_list = []
 39 |         for i in range(self.row):
 40 |             tmp = []
 41 |             for j in range(self.col):
 42 |                 tmp.append(State(i, j))
 43 |             map_list.append(tmp)
 44 |         return map_list
 45 | 
 46 |     def print_map(self):
 47 |         for i in range(self.row):
 48 |             tmp = ""
 49 |             for j in range(self.col):
 50 |                 tmp += self.map[i][j].state + " "
 51 |             print(tmp)
 52 | 
 53 |     def get_neighbers(self, state):
 54 |         state_list = []
 55 |         for i in [-1, 0, 1]:
 56 |             for j in [-1, 0, 1]:
 57 |                 if i == 0 and j == 0:
 58 |                     continue
 59 |                 if state.x + i < 0 or state.x + i >= self.row:
 60 |                     continue
 61 |                 if state.y + j < 0 or state.y + j >= self.col:
 62 |                     continue
 63 |                 state_list.append(self.map[state.x + i][state.y + j])
 64 |         return state_list
 65 | 
 66 |     def set_obstacle(self, point_list):
 67 |         for x, y in point_list:
 68 |             if x < 0 or x >= self.row or y < 0 or y >= self.col:
 69 |                 continue
 70 |             self.map[x][y].set_state("#")
 71 | 
 72 | 
 73 | class Dstar(object):
 74 | 
 75 |     def __init__(self, maps):
 76 |         self.map = maps
 77 |         self.open_list = set()
 78 | 
 79 |     def process_state(self):
 80 |         x = self.min_state()
 81 |         if x is None:
 82 |             return -1
 83 |         k_old = self.get_kmin()
 84 |         self.remove(x)
 85 |         if k_old < x.h:
 86 |             for y in self.map.get_neighbers(x):
 87 |                 if y.h <= k_old and x.h > y.h + x.cost(y):
 88 |                     x.parent = y
 89 |                     x.h = y.h + x.cost(y)
 90 |         elif k_old == x.h:
 91 |             for y in self.map.get_neighbers(x):
 92 |                 if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \
 93 |                         or y.parent != x and y.h > x.h + x.cost(y):
 94 |                     y.parent = x
 95 |                     self.insert(y, x.h + x.cost(y))
 96 |         else:
 97 |             for y in self.map.get_neighbers(x):
 98 |                 if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y):
 99 |                     y.parent = x
100 |                     self.insert(y, x.h + x.cost(y))
101 |                 else:
102 |                     if y.parent != x and y.h > x.h + x.cost(y):
103 |                         self.insert(y, x.h)
104 |                     else:
105 |                         if y.parent != x and x.h > y.h + x.cost(y) \
106 |                                 and y.t == "close" and y.h > k_old:
107 |                             self.insert(y, y.h)
108 |         return self.get_kmin()
109 | 
110 |     def min_state(self):
111 |         if not self.open_list:
112 |             return None
113 |         min_state = min(self.open_list, key=lambda x: x.k)
114 |         return min_state
115 | 
116 |     def get_kmin(self):
117 |         if not self.open_list:
118 |             return -1
119 |         k_min = min([x.k for x in self.open_list])
120 |         return k_min
121 | 
122 |     def insert(self, state, h_new):
123 |         if state.t == "new":
124 |             state.k = h_new
125 |         elif state.t == "open":
126 |             state.k = min(state.k, h_new)
127 |         elif state.t == "close":
128 |             state.k = min(state.h, h_new)
129 |         state.h = h_new
130 |         state.t = "open"
131 |         self.open_list.add(state)
132 | 
133 |     def remove(self, state):
134 |         if state.t == "open":
135 |             state.t = "close"
136 |         self.open_list.remove(state)
137 | 
138 |     def modify_cost(self, x):
139 |         if x.t == "close":
140 |             self.insert(x, x.parent.h + x.cost(x.parent))
141 | 
142 |     def run(self, start, end):
143 |         self.open_list.add(end)
144 |         while True:
145 |             self.process_state()
146 |             if start.t == "close":
147 |                 break
148 |         start.set_state("s")
149 |         s = start
150 |         while s != end:
151 |             s.set_state("s")
152 |             s = s.parent
153 |         s.set_state("e")
154 |         self.map.print_map()
155 |         tmp = start
156 |         self.map.set_obstacle([(9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8)])
157 |         while tmp != end:
158 |             tmp.set_state("*")
159 |             self.map.print_map()
160 |             print("")
161 |             if tmp.parent.state == "#":
162 |                 self.modify(tmp)
163 |                 continue
164 |             tmp = tmp.parent
165 |         tmp.set_state("e")
166 | 
167 |     def modify(self, state):
168 |         self.modify_cost(state)
169 |         while True:
170 |             k_min = self.process_state()
171 |             if k_min >= state.h:
172 |                 break
173 | 
174 | 
175 | if __name__ == '__main__':
176 |     m = Map(20, 20)
177 |     m.set_obstacle([(4, 3), (4, 4), (4, 5), (4, 6), (5, 3), (6, 3), (7, 3)])
178 |     start = m.map[1][2]
179 |     end = m.map[17][11]
180 |     dstar = Dstar(m)
181 |     dstar.run(start, end)
182 |     m.print_map()
183 | 


--------------------------------------------------------------------------------