MAC0318/ParticleFilter/python/map.py

from graphics import *
import time
import numpy as np
import math

class Map:
    def __init__(self, width, height, file):
        self.width, self.height = width, height
        self.circle = []
        self.poly = []
        with open(file) as f:
            for line in f:
                line = line.split(" ")
                shape = line[0]
                info = line[1:-1]
                info = filter(lambda a: a != ']' and a != '[', info)
                info = np.array(list(info))
                info = info.astype(np.float)

                if shape == "REC" or shape == "TRIANGLE":
                    vertices = info.reshape(-1, 2)
                    self.poly.append(vertices)
                elif shape == "CIRCLE":
                    self.circle.append(info)

    def draw(self, win, zoom):
        for c  in self.circle:
            c = c/zoom
            circle = Circle(Point(c[0],c[1]), c[2])
            circle.setFill("white")
            circle.setOutline('white')
            circle.draw(win)

        for vertices  in self.poly:
            vertices = vertices/zoom
            vertices = list(map(self.__toPoint__, vertices))
            poly = Polygon(vertices)
            poly.setFill('white')
            poly.setOutline('white')
            poly.draw(win)

    def white(self, x, y):
        for c  in self.circle:
            dx = c[0]-x
            dy = c[1]-y
            if dx*dx+dy*dy <= c[2]*c[2]:
                return True

        for vertices  in self.poly:
            if self.__inside_polygon__(x, y, vertices):
                return True

        return False

    def inside(self, x, y):
        if x > self.width or x < 0:
            return False
        if y > self.height or y < 0:
            return False
        return True

    def __inside_polygon__(self, x, y, points):
        """
        Return True if a coordinate (x, y) is inside a polygon defined by
        a list of verticies [(x1, y1), (x2, x2), ... , (xN, yN)].

        Reference: http://www.ariel.com.au/a/python-point-int-poly.html
        """
        n = len(points)
        inside = False
        p1x, p1y = points[0]
        for i in range(1, n + 1):
            p2x, p2y = points[i % n]
            if y > min(p1y, p2y):
                if y <= max(p1y, p2y):
                    if x <= max(p1x, p2x):
                        if p1y != p2y:
                            xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
                        if p1x == p2x or x <= xinters:
                            inside = not inside
            p1x, p1y = p2x, p2y
        return inside

    def __toPoint__(self, pair):
        return Point(pair[0],pair[1])