forked from mjl/particle_filter_demo
/
draw.py
96 lines (81 loc) · 2.74 KB
/
draw.py
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
# ------------------------------------------------------------------------
# coding=utf-8
# ------------------------------------------------------------------------
#
# Created by Martin J. Laubach on 2011-11-15
#
# ------------------------------------------------------------------------
import turtle
import random
turtle.tracer(5000, delay=0)
class Maze(object):
def __init__(self, maze):
self.maze = maze
self.width = len(maze[0])
self.height = len(maze)
turtle.setworldcoordinates(0, 0, self.width, self.height)
self.blocks = []
self.beacons = []
for y, line in enumerate(self.maze):
for x, block in enumerate(line):
if block:
nb_y = self.height - y - 1
self.blocks.append((x, nb_y))
self.beacons.extend(((x, nb_y), (x+1, nb_y), (x, nb_y+1), (x+1, nb_y+1)))
def draw(self):
for x, y in self.blocks:
turtle.up()
turtle.setposition(x, y)
turtle.down()
turtle.setheading(90)
turtle.begin_fill()
for _ in range(0, 4):
turtle.fd(1)
turtle.right(90)
turtle.end_fill()
turtle.up()
turtle.color("#00ffff")
for x, y in self.beacons:
turtle.setposition(x, y)
turtle.dot()
turtle.update()
def weight_to_color(self, weight):
return "#%02x00%02x" % (int(weight * 255), int((1 - weight) * 255))
def is_in(self, x, y):
if x < 0 or y < 0 or x > self.width or y > self.height:
return False
return True
def is_free(self, x, y):
if not self.is_in(x, y):
return False
yy = self.height - int(y) - 1
xx = int(x)
return self.maze[yy][xx] == 0
def show_particles(self, particles):
turtle.clearstamps()
for p in particles:
turtle.setposition(*p.xy)
turtle.setheading(p.h)
turtle.color(self.weight_to_color(p.w))
turtle.stamp()
turtle.update()
def show_robot(self, robot):
turtle.color("green")
turtle.setposition(*robot.xy)
turtle.setheading(robot.h)
turtle.stamp()
turtle.update()
def random_free_place(self):
while True:
x = random.random() * self.width
y = random.random() * self.height
if self.is_free(x, y):
return x, y
def distance_to_nearest_corner(self, x, y):
d = 99999
for c_x, c_y in self.beacons:
distance = (c_x - x) ** 2 + (c_y - y) ** 2
if distance < d:
d = distance
d_x, d_y = c_x, c_y
return d