A* Search Algorithm

  1#!/usr/bin/python
  2import sys
  3import random
  4
  5
  6RED = '\033[31m'
  7GREEN = '\033[32m'
  8GRAY = '\033[35m'
  9NC = '\033[0m'
 10
 11class Point(object):
 12    x = 0
 13    y = 0
 14    close = False
 15    open = False
 16    start = False
 17    end = False
 18    wall = False
 19    H = 99
 20    G = 99
 21    parent = None
 22
 23    def __init__(self, **kwargs):
 24        if 'x' in kwargs:
 25            self.x = kwargs['x']
 26
 27        if 'y' in kwargs:
 28            self.y = kwargs['y']
 29
 30        self.key = '{}-{}'.format(self.x, self.y)
 31
 32    def __str__(self):
 33        return '{},H{:2},G{:2},F{:2},P{:3}'.format(
 34                self.key,
 35                self.H if self.H != 99 else '',
 36                self.G if self.G != 99 else '',
 37                self.G + self.H if self.G != 99 else '',
 38                self.parent.key if self.parent else '')
 39
 40    def  __lt__(self, other):
 41        return self.x < other.x if self.y == other.y else self.y < other.y
 42
 43
 44def get_area(width, height):
 45    area = {}
 46    for i in range(width):
 47        for j in range(height):
 48            point = Point(x=i, y=j)
 49
 50            area[point.key] = point
 51    return area
 52
 53def show_result(area_hash):
 54    prev_y = -1
 55
 56    for point in sorted(area_hash.values()):
 57        if ( prev_y != point.y ):
 58            if (prev_y != -1):
 59                print('')
 60            prev_y = point.y
 61
 62        if point.start:
 63            format = RED + 'S' + NC
 64        elif point.end:
 65            format = RED + 'E' + NC
 66        elif point.wall:
 67            format = GRAY + 'W' + NC
 68        elif point.close:
 69            format = GREEN + '0' + NC
 70        else:
 71            format = ' '
 72        print((format + '({})  ').format(point), end='')
 73    print('')
 74
 75def set_preset(area, start, end, wall):
 76    area[start].start = True
 77    area[end].end = True
 78    for key in wall:
 79        area[key].wall = True
 80    return area
 81
 82def is_valid_point(point, width, height):
 83    if point.wall or point.close:
 84        return False
 85
 86    if point.x < 0 or point.x > width:
 87        return False
 88
 89    if point.y < 0 or point.y > height:
 90        return False
 91
 92    return True
 93
 94def get_around(area, point, width, height):
 95    n = '{}-{}'.format(point.x, point.y - 1)
 96    s = '{}-{}'.format(point.x, point.y + 1)
 97    l = '{}-{}'.format(point.x - 1, point.y)
 98    r = '{}-{}'.format(point.x + 1, point.y)
 99
100    res = {}
101    for key in [n, s, l, r]:
102        if key in area and is_valid_point(area[key], width, height):
103            area[key].parent = point
104            area[key].G = area[key].G if area[key].G != 99 else (point.G if point.G != 99 else 0) + 1
105            res[key] = area[key]
106
107    return res
108
109def get_H(point, end_point):
110    return abs(point.x - end_point.x) + abs(point.y - end_point.y)
111
112step = 0
113opened = {}
114
115def go(area, start, end, width, height):
116    global step, opened
117    start_point = area[start]
118    end_point= area[end]
119    start_point.close = True
120
121    if start_point.key in opened:
122        del opened[start_point.key]
123
124    points = get_around(area, start_point, width, height)
125    opened.update(points)
126
127    rnd = random.choice(list(opened))
128    next_point = area[rnd]
129    for key, point in opened.items():
130        point.H = get_H(point, end_point)
131
132        if point.H + point.G <= next_point.H + next_point.G:
133            next_point = point
134
135    print('step {}: {}'.format(step, next_point))
136    step += 1
137    if step >= 3:
138        pass
139
140    if next_point and next_point.key != end:
141        go(area, next_point.key, end, width, height)
142
143def get_result(area, start, end):
144    print('{}{}{} <- '.format(RED, area[end].key, NC), end='')
145    parent = area[end].parent
146    if(parent.key != start):
147        get_result(area, start, parent.key)
148    else:
149        print('{}{}{}'.format(RED, area[start].key, NC), end='')
150
151def main():
152    width = 10
153    height = 10
154    start = '1-3'
155    end = '8-4'
156    wall = ['2-2', '3-2', '3-3', '3-4', '3-5', '3-6', '5-4', '2-6', '1-6']
157
158    area = get_area(width, height)
159    area = set_preset(area, start, end, wall)
160    go(area, start, end, width,  height)
161    show_result(area)
162    get_result(area, start, end)
163
164if __name__ == '__main__':
165    main()