draw.py

# ------------------------------------------------------------------------
# coding=utf-8
# ------------------------------------------------------------------------
#
#  Created by Martin J. Laubach on 2011-11-15
#  Modified by Joseph Tosey on 2011-11-16
#
# ------------------------------------------------------------------------

import turtle
import random

turtle.tracer(50000, delay=0)
turtle.register_shape("dot", ((-3,-3), (-3,3), (3,3), (3,-3)))

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))
                    if block == 2:
                        self.beacons.extend(((x, 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()

    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 clear(self):
        turtle.clearstamps()

    def update(self):
        turtle.update()

    def show_particles(self, particles):
        turtle.shape('dot')

        for p in particles:
            turtle.setposition(*p.xy)
            turtle.setheading(p.h)
            turtle.color(self.weight_to_color(p.w))
            turtle.stamp()

    def show_robot(self, robot):
        turtle.color("green")
        turtle.shape('turtle')
        turtle.setposition(*robot.xy)
        turtle.setheading(robot.h)
        turtle.stamp()

    def show_estimate(self, estimate):
        turtle.color("red")
        turtle.shape('turtle')
        turtle.setposition(*estimate.xy)
        turtle.setheading(estimate.h)
        turtle.stamp()

    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_beacon(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