Example #1
0
 def __init__(self):
     self.ants = []
     self.food = Circle(19, 18, 0.2)
     self.home = Circle(10, 10, 0.2)
     self.home_trail = QuadTree(AREA, capacity=QUADTREE_CAPACITY)
     self.food_trail = QuadTree(AREA, capacity=QUADTREE_CAPACITY)
     self.iteration = -1
Example #2
0
    def _build_dynamic_location_index(self):
        """
        Initialize a quadtree index and insert
        all the stored locations 
        """   
        sys.stdout.write("Generating QuadTree location index...\n ")
        sys.stdout.flush()
        location_index = QuadTree(self.bounding_box)
        # iterate through all trips

        for trip in self.all_trips:
            #Iterate through all locations
            current_cell = None
            #Loop for adding trajectory information
            for location in trip.locations:
                current_cell = location_index.insert(location)

        location_index.traverse() #Populate leaves[]

        _node_id = 0
        #Store a reference to all the quadtree LEAF cells
        for cell in location_index.leaves:
            location = cell._center_of_mass()
            cell.id = (location.latitude, location.longitude)
            self.all_nodes[(location.latitude, location.longitude)] = cell
        
        sys.stdout.write("Done...\n ")
        sys.stdout.flush()
        
        return location_index
Example #3
0
    def extend(self, item_list):
        """ Add the given items to the world.

            @param item_list: [ ... polygon ... ]
        """
        assert isinstance(item_list, (type(()), type([])))
        # Identify the bounds of the new list
        l = min(item.left for item in item_list)
        t = max(item.top for item in item_list)
        r = max(item.right for item in item_list)
        b = min(item.bottom for item in item_list)
        bounding_box = BBox(l, t, r, b)
        if self.item_list:
            self.item_list.extend(item_list)
        else:
            self.item_list = item_list
        # Compare with index
        if self.index is None:
            self.index = QuadTree(self.item_list)            
        else:
            # Check that new items are inside the existing index
            i_l, i_t, i_r, i_b = self.index.box_as_tuple
            if l >= i_l and t <= i_t and r <= i_r and b >= i_b:
                self.index.extend(item_list)
            else:
                # Rebuild to allow for extra space 
                self.index = QuadTree(self.item_list)
Example #4
0
class World:

  def __init__(self, rect):
    self.objects = []
    self.objects.append(Player())
    for i in range(100):
      self.objects.append(Hunter())

    self.qtree = QuadTree(rect, maxDepth=8)
    self.qtree.insert(self.objects)

  def update(self, delta_time):
    for obj in self.objects:
      obj.update(delta_time)

    update_collisions(self.qtree, self.objects)

  def draw(self, canvas):
    canvas.set_source_rgb(1, 1, 1)
    canvas.paint()

    draw_qtree(canvas, self.qtree)

    for obj in self.objects:
      mtx = canvas.get_matrix()
      obj.draw(canvas)
      canvas.set_matrix(mtx)
Example #5
0
class World:
    def __init__(self, rect):
        self.objects = []
        self.objects.append(Player())
        for i in range(100):
            self.objects.append(Hunter())

        self.qtree = QuadTree(rect, maxDepth=8)
        self.qtree.insert(self.objects)

    def update(self, delta_time):
        for obj in self.objects:
            obj.update(delta_time)

        update_collisions(self.qtree, self.objects)

    def draw(self, canvas):
        canvas.set_source_rgb(1, 1, 1)
        canvas.paint()

        draw_qtree(canvas, self.qtree)

        for obj in self.objects:
            mtx = canvas.get_matrix()
            obj.draw(canvas)
            canvas.set_matrix(mtx)
Example #6
0
class Level:
    def __init__(self, x=0, y=0, w=960, h=540):
        self.x = x
        self.y = y
        self.w = w
        self.h = h
        
        self.plans = set()
        self.blocks = set()
        
        self.planTree = QuadTree(boundingRect=(x, y, x+w, y+h))
        self.quadTree = QuadTree(boundingRect=(x, y, x+w, y+h))
    
    def save(self):
        pass
    
    def load(self):
        self.startingBalls = 12
        self.blocks |= set(Block(Circle(480 - 192 + 384/16*i, 250 + i%2*24, 16.0)) for i in range(16))
        
        n = 9
        r0 = 48.
        r1 = 48. + 16.
        self.blocks |= set(Block(Arc(300., 128., r0, r1, i*pi/n, pi/n*(i+.9))) for i in range(n))
        self.blocks |= set(Block(Arc(480., 80.,  r0, r1, i*pi/n, pi/n*(i+.9))) for i in range(n))
        self.blocks |= set(Block(Arc(660., 128., r0, r1, i*pi/n, pi/n*(i+.9))) for i in range(n))
        
        self.blocks |= set(Block(Capsule(32.+48.*i, 360., 48.+48.*i, 360., 8.)) for i in range(12))
        self.blocks |= set(Block(Rectangle(32.+48.*i, 400., 24., 16.)) for i in range(13))
        
        for block in self.blocks:
            self.quadTree.insert(block)
Example #7
0
    def _clear_all(self):
        self.find_points = []
        self.find_frm = None
        self.find_to = None
        self.kd_find_result = None
        self.kd_find_steps = []
        self.find_step_no = 0
        self.find_stage = NO_FIND
        self.q_find_steps = []
        self.q_find_result = []
        self._q_found_points = []

        self._q_graph = nx.DiGraph()
        self._kd_graph = nx.DiGraph()

        self._qt = QuadTree((0, 0), (X_SIZE, Y_SIZE),
                            lambda x: self.add_q_node(x),
                            lambda x, y: self.connect_q_node(x, y))
        self._kdt = KDTree(
            (0, 0), (X_SIZE, Y_SIZE),
            lambda x: self.add_kd_node(x),
            lambda x, y: self.connect_kd_node(x, y),
            lambda x, y, z=(False, True): self.highlight_kd_nodes(x, y, z))

        self._refresh_graphs()
Example #8
0
    def __check_collisions(self):
        """Update proximity sensors and detect collisions between objects"""

        collisions = []
        checked_robots = []

        if self.__qtree is None:
            self.__qtree = QuadTree(self.__obstacles)

        if len(self.__robots) > 1:
            rqtree = QuadTree(self.__robots)
        else:
            rqtree = None

        # check each robot
        for robot in self.__robots:

            # update proximity sensors
            for sensor in robot.get_external_sensors():
                sensor.get_world_envelope(True)
                rect = Rect(sensor.get_bounding_rect())
                sensor.update_distance()
                # distance to obstacles
                for obstacle in self.__qtree.find_items(rect):
                    sensor.update_distance(obstacle)
                # distance to other robots
                if rqtree is None:
                    continue
                for other in rqtree.find_items(rect):
                    if other is not robot:
                        sensor.update_distance(other)

            rect = Rect(robot.get_bounding_rect())

            # against nearest obstacles
            for obstacle in self.__qtree.find_items(rect):
                if robot.has_collision(obstacle):
                    collisions.append((robot, obstacle))

            # against other robots
            if rqtree is not None:
                for other in rqtree.find_items(rect):
                    if other is robot:
                        continue
                    if other in checked_robots:
                        continue
                    if robot.has_collision(other):
                        collisions.append((robot, other))

            checked_robots.append(robot)

        if len(collisions) > 0:
            # Test code - print out collisions
            for (robot, obstacle) in collisions:
                self.log("Collision with {}".format(
                    obstacle), obj=robot, color=robot.get_color())
            # end of test code
            return True

        return False
    def _build_location_index(self):   
        sys.stdout.write("Generating QuadTree location index... ")
        sys.stdout.flush()
        location_index = QuadTree(MAX_DEPTH, self.bounding_box)
        # iterate through all trips

        for trip in self.all_trips:
            #Iterate through all locations
            current_node = None
            #Loop for adding trajectory information
            for previous, location, next in self.previous_and_next(trip.locations):
                current_node = location_index.insert(location)

                # First location 
                if previous is not None and next is not None:
                    #get the node where the next location would fall
                    self.update_trajectory(previous, location, next, current_node)


        location_index.traverse() #Populate leaves[]

        _node_id = 0
        #Hash with all leaves
        for leave in location_index.leaves:
            location = leave._center_of_mass()
            leave.id = (location.latitude, location.longitude)
            self.all_nodes[(location.latitude, location.longitude)] = leave

        return location_index
Example #10
0
    def __init__(self, rect):
        self.objects = []
        self.objects.append(Player())
        for i in range(100):
            self.objects.append(Hunter())

        self.qtree = QuadTree(rect, maxDepth=8)
        self.qtree.insert(self.objects)
Example #11
0
    def remove(self, item):
        """ Remove a single item from the world

            @param item: PolyGon or similar
        """
        self.item_list.remove(item)
        # Oops. Rebuilding quadtrees is not that easy...
        self.index = QuadTree(self.item_list)
Example #12
0
def main():
        #Load all files and initilize Simple Tracks
    os.chdir("/home/moyano/Projects/CreateTracks/trips/")
    all_points = []
    for trip in os.listdir("."):
            trip_data = load_file(trip)
            all_points += trip_data
            #tracks.append(Track(trip_data))
            
    boundries = max_bounding_rect(all_points)
   # depth = depth(boundries, 0.00035)
    #print "Nesting Level: %i" % depth
    qtree = QuadTree(10, boundries)
    #Make the QTree
    for coord in all_points:
        qtree.add_point(coord)
    qtree.traverse()
    nodes = qtree.leaves

    #Load Trips
    trips = []
    for trip in os.listdir("."):
        if not trip.startswith('.'):

            gps_data = load_file(trip)
            trips.append(Trip(gps_data,trip))

    routes(trips, qtree)

    #Weighted Points
    os.chdir("/home/moyano/Projects/CreateTracks/edges")
    test_file = open("edges.txt", "w")
    test_file.write("latitude, longitude, ocurrences, color")
    # print children
    for node in nodes:
        p = node._center_of_mass()
        count = len(node.items)
        if count > 2:
            test_file.write("\n")  
            test_file.write(str(p.latitude) + "," + str(p.longitude) + "," + str(count) + "," + get_color(count))

    #All points
    os.chdir("/home/moyano/Projects/CreateTracks/edges")
    test_file = open("edges2.csv", "w")
    test_file.write("latitude, longitude")
    test_file.write("#")  
    # print children
    xpoints = []
    for node in nodes:
        p = node._center_of_mass()
        count = len(node.items)
        if count > 100:

            for _ in xrange(count):
                xpoints.append((p.latitude, p.longitude))
                test_file.write(str(p.latitude) + ", " + str(p.longitude))
                test_file.write("#")  
Example #13
0
class QuadTreeFunctionTest(unittest.TestCase):
    def setUp(self) -> None:
        self.quadtree = QuadTree(Bounds(0, 0, 400, 400),
                                 max_objects=4,
                                 max_level=5)

    def test_points_insert(self):
        with open('./tests/example_data.json', 'r') as f:
            json_data = json.loads(f.read())
        points = json_data['data']
        for point in points:
            self.quadtree.insert(Point(point['x'], point['y'], point['value']))

    def test_retrieve(self):
        objects = self.quadtree.retrieve(Bounds(200, 200, 50, 50))

    def test_retrieve_intersections(self):
        objects = self.quadtree.retrieve_intersections(Bounds(
            200, 200, 50, 50))

    def test_nearest_neighbors(self):
        points = self.quadtree.nearest_neighbors(Point(225, 225), radius=25)

    def test_visualization(self):
        self.quadtree.visualize()

    def tearDown(self) -> None:
        self.quadtree.clear()
Example #14
0
    def __init__(self):
        super(GUI, self).__init__('Simple example 1')

        self.find_stage = False
        self.find_points = []
        self.find_frm = None
        self.find_to = None
        self.kd_find_steps = []
        self.kd_find_result = None
        self.find_step_no = 0
        self.q_find_steps = []
        self.q_find_result = []
        self._q_found_points = []

        self._q_structure = ControlMatplotlib('Quad tree structure')
        self._q_plain = ControlMatplotlib('Quad tree plain')
        self._kd_structure = ControlMatplotlib('KD tree structure')
        self._kd_plain = ControlMatplotlib('KD tree plain')
        self._find_button = ControlButton('Find')
        self._step_button = ControlButton('Step')
        self._clear_button = ControlButton('Clear')
        self._find_button.value = self._on_find
        self._step_button.value = self._on_step
        self._clear_button.value = self._clear_all
        self._formset = [
            '_find_button', '||', '_step_button', '||', "_clear_button", '=', {
                'Quad tree': ['_q_structure', '||', '_q_plain'],
                'KD tree': ['_kd_structure', '||', '_kd_plain']
            }
        ]

        self._q_graph = nx.DiGraph()
        self._kd_graph = nx.DiGraph()

        self._q_graph_fig = self._q_structure.fig
        self._q_plain_fig = self._q_plain.fig
        self._kd_graph_fig = self._kd_structure.fig
        self._kd_plain_fig = self._kd_plain.fig

        self._kd_plain_fig.canvas.mpl_connect(
            'button_press_event', lambda x: self._add_point_action(x))
        self._q_plain_fig.canvas.mpl_connect(
            'button_press_event', lambda x: self._add_point_action(x))

        self._qt = QuadTree((0, 0), (X_SIZE, Y_SIZE),
                            lambda x: self.add_q_node(x),
                            lambda x, y: self.connect_q_node(x, y))
        self._kdt = KDTree(
            (0, 0), (X_SIZE, Y_SIZE),
            lambda x: self.add_kd_node(x),
            lambda x, y: self.connect_kd_node(x, y),
            lambda x, y, z=(False, True): self.highlight_kd_nodes(x, y, z))

        self._refresh_graphs()
Example #15
0
    def __init__(self, item_list=None):
        """ Set up the solid world

            @param item_list: [ ... polygon ... ]
        """
        self.item_list = None
        if item_list:
            self.item_list = item_list

        self.index = None
        if item_list:
            self.index = QuadTree(self.item_list)
Example #16
0
    def __init__(self, qt: QuadTree, scale=100):

        self.w, self.h = qt.get_boundary().w, qt.get_boundary().h
        self.qt = qt
        scaled = False

        if self.w < 100:
            self.w *= scale
            scaled = True

        if self.h < 100:
            self.h *= scale
            scaled = True
Example #17
0
def main():
    capacite = input(
        "Veuillez entrer la capacitée maximale que QuadTree peut contenir ")
    pygame.init()
    pygame.mixer.init()
    pygame.display.set_caption('QuadTree Project')
    ecran = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT))
    refresh(ecran)

    e = ensemble_points(0, 512, 512)  # création de 8 points aléatoires
    # dans un univers de taille
    # 512x512
    region = Rectangle((0, 0), (512, 512))
    quadTree = QuadTree(region, e, int(capacite))

    while True:  # boucle principale
        try:
            handleEvents(quadTree)
            drawApp(ecran, quadTree)
            pygame.display.update()

        except Quitte:
            break

    pygame.quit()
    sys.exit(0)
Example #18
0
  def updateQuads(self, v=None):
    isValid = True
    try:
      c = map(float, [self.lineEdit_xmin.text(), self.lineEdit_ymin.text(), self.lineEdit_xmax.text(), self.lineEdit_ymax.text()])
    except:
      isValid = False

    if isValid:
      # create quad rubber bands
      rect = QgsRectangle(c[0], c[1], c[2], c[3])
      quadtree = QuadTree(self.dialog.iface.mapCanvas().extent())
      quadtree.buildTreeByRect(rect, self.spinBox_Height.value())
      self.dialog.createRubberBands(quadtree.quads(), rect.center())
      self.dialog.setWindowState(self.windowState() & ~Qt.WindowMinimized | Qt.WindowActive)
    else:
      self.dialog.clearRubberBands()
Example #19
0
    def rebuild_quad_tree(self):
        self.quad_tree = QuadTree( geometry.BoundingBox( 0, self.SCREEN_SIZE[0], 0, self.SCREEN_SIZE[1] ), max_elems=4 )
        for wall in self.walls:
            for wall_segment in pairwise( wall ):
                self.quad_tree.add( geometry.LineSegment( wall_segment[0], wall_segment[1] ) )

        self.quad_tree_rects = []
        def get_quadrants_rects( quadrant ):
            if quadrant.subquadrants is not None:
                for subquadrant in quadrant.subquadrants:
                    get_quadrants_rects( subquadrant )
            else:
                self.quad_tree_rects.append( pygame.Rect( quadrant.bounding_box.x_lower, quadrant.bounding_box.y_lower,
                                                          quadrant.bounding_box.x_upper - quadrant.bounding_box.x_lower + 1,
                                                          quadrant.bounding_box.y_upper - quadrant.bounding_box.y_lower + 1 ) )

        get_quadrants_rects( self.quad_tree.main_quadrant )
Example #20
0
  def __init__(self, rect):
    self.objects = []
    self.objects.append(Player())
    for i in range(100):
      self.objects.append(Hunter())

    self.qtree = QuadTree(rect, maxDepth=8)
    self.qtree.insert(self.objects)
Example #21
0
def test_constructor_complex():
    from quadtree import QuadTree

    q = QuadTree(complex(100, 100))

    assert abs(q.dimension.real - 100) < 0.0001
    assert abs(q.dimension.imag - 100) < 0.0001

    assert q.branches == [None, None, None, None]
    assert q.children == {}
Example #22
0
def test_constructor_distinct():
    from quadtree import QuadTree

    q = QuadTree(width=100, height=100)

    assert abs(q.dimension.real - 100) < 0.0001
    assert abs(q.dimension.imag - 100) < 0.0001

    assert q.branches == [None, None, None, None]
    assert q.children == {}
Example #23
0
    def __init__(self):
        self.root = QuadTree()
        self.radius = 0.125
        self.searching = None
        self.found = []

        self.app = QtWidgets.QApplication(sys.argv)
        super().__init__()

        self.app.setStyle("Fusion")
        palette = QtGui.QPalette()
        palette.setColor(QtGui.QPalette.Window, self.background)
        self.app.setPalette(palette)

        self.setWindowTitle('QuadTree')
        self.resize(600, 600)
        self.show()
        # main event loop
        sys.exit(self.app.exec_())
Example #24
0
 def __init__(self, x=0, y=0, w=960, h=540):
     self.x = x
     self.y = y
     self.w = w
     self.h = h
     
     self.plans = set()
     self.blocks = set()
     
     self.planTree = QuadTree(boundingRect=(x, y, x+w, y+h))
     self.quadTree = QuadTree(boundingRect=(x, y, x+w, y+h))
def createQuadTree(extent, p):
  """
  args:
    p -- demProperties
  """
  try:
    cx, cy, w, h = map(float, [p["lineEdit_centerX"], p["lineEdit_centerY"], p["lineEdit_rectWidth"], p["lineEdit_rectHeight"]])
  except ValueError:
    return None

  # normalize
  c = extent.normalizePoint(cx, cy)
  hw = 0.5 * w / extent.width()
  hh = 0.5 * h / extent.height()

  quadtree = QuadTree()
  if not quadtree.buildTreeByRect(QgsRectangle(c.x() - hw, c.y() - hh, c.x() + hw, c.y() + hh), p["spinBox_Height"]):
    return None

  return quadtree
Example #26
0
def build_tree(text):
    query = query_broadcast.value

    root = None
    geometric_centroid_ra = geometric_centroid_dec = None
    centroid = None
    cent_min_dist = float("inf")
    voxel = None
    for lines in text:
        for line in lines[1].split("\n"):
            split = line.split(",")
            if len(split) == 4:
                min_ra, max_ra, min_dec, max_dec = split
                voxel = Voxel(float(min_ra), float(max_ra), float(min_dec),
                              float(max_dec))
                geometric_centroid_ra, geometric_centroid_dec = voxel.getVoxelCentroid(
                )
                root = Node(voxel)
            elif line:
                border = False if split[13].lower() == "false" else True
                '''star = Element(int(split[0]), float(split[1]), float(split[2]), float(split[3]), float(split[4]),
                               float(split[5]), float(split[6]), float(split[7]), float(split[8]),
                               float(split[9]), float(split[10]), float(split[11]), float(split[12]), 0, border)'''

                star = Element(int(split[0]), float(split[1]), float(split[2]),
                               float(split[3]), 0, border)

                root.addElement(star)

                if star.border is False:
                    dist = EucDist(star.getRa(), geometric_centroid_ra,
                                   star.getDec(), geometric_centroid_dec)
                    if dist < cent_min_dist:
                        centroid = star
                        cent_min_dist = dist

    root.setSize(len(root.getElements()))
    root.addCentroid(centroid)

    level = compute_level(voxel.getSideSize(), voxel.getHeightSize(),
                          query.getMaxDistance())
    tree = QuadTree(root, level)

    print("\n**** Data Descriptions *****")
    print("Sky Voxel: %s,%s,%s,%s" %
          (voxel.x_left, voxel.x_right, voxel.y_left, voxel.y_right))
    print("Sky Diagonal: %s" % voxel.getDiagonal())
    print("Tree Level: %s" % level)
    print("Tree Elements: %s" % root.size)
    print("Tree Leaf nodes: %s" % len(tree.nodes))
    print("**** End Data Descriptions *****\n")

    return [tree]
    def setup(self):
        self.quadtree = QuadTree(8, self.width, self.height)

        self.buffer = 15

        self.numRectangles = 100
        self.numPoints = 100

        self.rectangles = []
        self.points = []

        self.rectColor = "#354F00"
        self.pointColor = "#567714"
        self.backColor = "#97A084"
        self.collRectColor = "#441154"

        self.maxRectSize = 100
        self.minRectSize = 20

        self.pointSize = 5

        self.collRectWidth = 150
        self.collRectHeight = 150

        self.collRect = RectData(0,0,self.collRectWidth,self.collRectHeight,"#F00")

        for i in range(self.numRectangles):
            vals = self.genRandomVals("rectangle")
            rect = RectData(vals.x, vals.y, vals.w, vals.h, self.rectColor)
            self.rectangles.append(rect)
            self.quadtree.add(rect)

        for i in range(self.numPoints):
            vals = self.genRandomVals("point")
            point = RectData(vals.x, vals.y, vals.w, vals.h, self.pointColor)
            self.points.append(point)
            self.quadtree.add(point)

        self.draw_rect(0, 0, self.width, self.height, "#000")
        self.draw_rect(self.collRect.x,self.collRect.y,self.collRect.w,self.collRect.h,self.collRect.data)
    def __check_collisions(self):
        """Update proximity sensors and detect collisions between objects"""

        collisions = []
        checked_robots = []

        if self.__qtree is None:
            self.__qtree = QuadTree(self.__obstacles)

        if len(self.__robots) > 1:
            rqtree = QuadTree(self.__robots)
        else:
            rqtree = None

        # check each robot
        for robot in self.__robots:

            # update proximity sensors
            for sensor in robot.get_external_sensors():
                sensor.get_world_envelope(True)
                rect = Rect(sensor.get_bounding_rect())
                sensor.update_distance()
                # distance to obstacles
                for obstacle in self.__qtree.find_items(rect):
                    sensor.update_distance(obstacle)
                # distance to other robots
                if rqtree is None: continue
                for other in rqtree.find_items(rect):
                    if other is not robot:
                        sensor.update_distance(other)

            rect = Rect(robot.get_bounding_rect())

            # against nearest obstacles
            for obstacle in self.__qtree.find_items(rect):
                if robot.has_collision(obstacle):
                    collisions.append((robot, obstacle))

            # against other robots
            if rqtree is not None:
                for other in rqtree.find_items(rect):
                    if other is robot: continue
                    if other in checked_robots: continue
                    if robot.has_collision(other):
                        collisions.append((robot, other))

            checked_robots.append(robot)

        if len(collisions) > 0:
            # Test code - print out collisions
            for (robot, obstacle) in collisions:
                self.log("Collision with {}".format(obstacle),
                         obj=robot,
                         color=robot.get_color())
            # end of test code
            return True

        return False
def build_tree(filename, query):
    start_time = time.time()

    root = None
    geometric_centroid_ra = geometric_centroid_dec = None
    centroid = None
    cent_min_dist = float("inf")
    voxel = None

    with open(filename) as f:
        for line in f:
            split = line.replace("\n", "").split(",")
            if len(split) == 4:
                min_ra, max_ra, min_dec, max_dec = split
                voxel = Voxel(float(min_ra), float(max_ra), float(min_dec), float(max_dec))
                geometric_centroid_ra, geometric_centroid_dec = voxel.getVoxelCentroid()
                root = Node(voxel)
            elif line:
                border = False # if split[13].lower() == "false" else True

                star = Element(int(split[0]), float(split[1]), float(split[2]), float(split[3]), float(split[4]),
                               float(split[5]), float(split[6]), float(split[7]), float(split[8]),
                               float(split[9]), float(split[10]), float(split[11]), float(split[12]), 0, border)

                root.addElement(star)

                if star.border is False:
                    dist = EucDist(star.getRa(), geometric_centroid_ra, star.getDec(), geometric_centroid_dec)
                    if dist < cent_min_dist:
                        centroid = star
                        cent_min_dist = dist

    root.setSize(len(root.getElements()))
    root.addCentroid(centroid)

    level = compute_level(voxel.getSideSize(), voxel.getHeightSize(), query.getMaxDistance())
    tree = QuadTree(root, level)

    end_time = time.time() - start_time
    print("BT - %s - %0.25f" % (filename, end_time))
    return tree
Example #30
0
def load(filename, zoom):
  with open(filename) as file:
    data = json.load(file)
  FILENAME = data['filename']
  startIter = data['iter']
  
  tree = QuadTree( (data['treeMin'][0]*zoom, data['treeMin'][1]*zoom), (data['treeMax'][0]*zoom, data['treeMax'][1]*zoom))
  Node.tree = tree
  Road.tree = tree
  for node in data['nodes']:
    Node((node['coord'][0]*zoom, node['coord'][1]*zoom), id=node['id'])
  Node.nodeId = max(Node.nodeSet.keys())+1
  
  for road in data['roads']:
    newRoad = Road.getClass(road['type'])(Node.nodeSet[road['start']], Node.nodeSet[road['end']], level=road['level'])
    if type(newRoad) is SpecialRoad:
      newRoad.create(**road['data'])
    newRoad.add(road['id'])
    newRoad.reset()
  Road.roadId = max(Road.roadSet.keys())+1 
  return FILENAME, startIter
Example #31
0
def build_tree(text):
    #query = query_broadcast.value

    root = None
    geometric_centroid_ra = geometric_centroid_dec = None
    centroid = None
    cent_min_dist = float("inf")
    voxel = None
    for i in range(1, len(text)):  # skip first line
        #for line in lines[1].split("\n"):
        split = text[i].split(",")
        if len(split) == 4:
            min_ra, max_ra, min_dec, max_dec = split
            voxel = Voxel(float(min_ra), float(max_ra), float(min_dec),
                          float(max_dec))
            geometric_centroid_ra, geometric_centroid_dec = voxel.getVoxelCentroid(
            )
            root = Node(voxel)
        elif text[i]:

            star = Element(int(split[0]), float(split[1]), float(split[2]),
                           float(split[3]), 0, split[4])

            root.addElement(star)

            dist = EucDist(star.getRa(), geometric_centroid_ra, star.getDec(),
                           geometric_centroid_dec)
            if dist < cent_min_dist:
                centroid = star
                cent_min_dist = dist

    root.setSize(len(root.getElements()))
    root.addCentroid(centroid)

    level = compute_level(voxel.getSideSize(), voxel.getHeightSize(),
                          0.00416667)
    tree = QuadTree(root, level)

    return tree
Example #32
0
def test_constructor_wrong():
    from quadtree import QuadTree

    with pytest.raises(ValueError):
        QuadTree()

    with pytest.raises(ValueError):
        QuadTree(width=100)

    with pytest.raises(ValueError):
        QuadTree(height=100)

    with pytest.raises(ValueError):
        QuadTree(complex(100, 100), width=100)

    with pytest.raises(ValueError):
        QuadTree(complex(100, 100), height=100)

    with pytest.raises(ValueError):
        QuadTree(complex(100, 100), width=100, height=100)
	for d in node.data:
		pygame.draw.rect(surface, d.data, pygame.Rect(d.x, d.y, d.w, d.h))
			
	
if __name__ == "__main__":
	display = pygame.display.set_mode((640, 640))
	clock = pygame.time.Clock()
	
	mdown = False
	mx1 = 0
	my1 = 0
	mx2 = 0
	my2 = 0
	selected = []
	
	quadtree = QuadTree(8, 640, 640)
	
	keep_going = True
	while keep_going:
		for event in pygame.event.get():
			if event.type == pygame.QUIT:
				keep_going = False
				
			if event.type == pygame.KEYDOWN:
				if event.key == pygame.K_RETURN:
					x = random.randint(0,600)
					y = random.randint(0,600)
					w = random.randint(3,40)
					h = random.randint(3,40)
					color = (random.randint(64,250), random.randint(64,250), random.randint(64,250))
					quadtree.add(RectData(x, y, w, h, color))
Example #34
0
class Simulator(threading.Thread):
    """The simulator manages simobjects and their collisions, commands supervisors
       and draws the world using the supplied *renderer*.
       
       The simulator runs in a separate thread. None of its functions are thread-safe,
       and should never be called directly from other objects (except for the functions
       inherited from `threading.Thread`). The communication with the simulator
       should be done through its *in_queue* and *out_queue*. See :ref:`ui-sim-queue`.
       
       :param renderer: The renderer that will be used to draw the world.
                        The simulator will assume control of the renderer.
                        The renderer functions also have to be considered thread-unsafe.
       :type renderer: :class:`~renderer.Renderer`
       :param in_queue: The queue that is used to send events to the simulator.
       :type in_queue: :class:`Queue.Queue`
    """
    
    __nice_colors = (0x55AAEE, 0x66BB22, 0xFFBB22, 0xCC66AA,
                     0x77CCAA, 0xFF7711, 0xFF5555, 0x55CC88)

    def __init__(self, renderer, in_queue):
        """Create a simulator with *renderer* and *in_queue*
        """
        super(Simulator, self).__init__()

        #Attributes
        self.__stop = False
        self.__state = PAUSE
        self.__renderer = renderer
        self.__center_on_robot = False
        self.__orient_on_robot = False
        self.__show_sensors = True
        self.__draw_supervisors = False
        self.__show_tracks = True
        
        self.__in_queue = in_queue
        self._out_queue = queue.Queue()

        # Zoom on scene - Move to read_config later
        self.__time_multiplier = 1.0
        self.__time = 0.0

        # Plots
        self.plot_expressions = []

        # World objects
        self.__robots = []
        self.__trackers = []
        self.__obstacles = []
        self.__supervisors = []
        self.__background = []
        self.__zoom_default = 1

        self.__world = None
        
        self.__log_queue = deque()
        
        # Internal objects
        self.__qtree = None

    def read_config(self, filename):
        '''Load in the objects from the world XML file '''

        self.log('reading initial configuration')
        try:
            self.__world = XMLReader(filename, 'simulation').read()
        except Exception as e:
            raise Exception('[Simulator.read_config] Failed to parse ' + filename \
                + ': ' + str(e))
        else:
            self.__supervisor_param_cache = None
            self.__center_on_robot = False
            self.__construct_world()

    def __construct_world(self):
        """Creates objects previously loaded from the world xml file.
           
           This function uses the world in ``self.__world``.
           
           All the objects will be created anew, including robots and supervisors.
           All of the user's code is reloaded.
        """
        if self.__world is None:
            return

        helpers.unload_user_modules()

        self.__state = DRAW_ONCE            
            
        self.__robots = []
        self.__obstacles = []
        self.__supervisors = []
        self.__background = []
        self.__trackers = []
        self.__qtree = None
        
        for thing in self.__world:
            thing_type = thing[0]
            if thing_type == 'robot':
                robot_type, supervisor_type, robot_pose, robot_color  = thing[1:5]
                try:
                    # Create robot
                    robot_class = helpers.load_by_name(robot_type,'robots')
                    robot = robot_class(pose.Pose(robot_pose))
                    if robot_color is not None:
                        robot.set_color(robot_color)
                    elif len(self.__robots) < 8:
                        robot.set_color(self.__nice_colors[len(self.__robots)])
                        
                    # Create supervisor
                    sup_class = helpers.load_by_name(supervisor_type,'supervisors')
                    
                    info = robot.get_info()
                    info.color = robot.get_color()
                    supervisor = sup_class(robot.get_pose(), info)
                    supervisor.set_logqueue(self.__log_queue)
                    name = "Robot {}: {}".format(len(self.__robots)+1, sup_class.__name__)
                    if self.__supervisor_param_cache is not None and \
                       len(self.__supervisor_param_cache) > len(self.__supervisors):
                        supervisor.set_parameters(self.__supervisor_param_cache[len(self.__supervisors)])
                    self._out_queue.put(("make_param_window",
                                            (robot, name,
                                             supervisor.get_ui_description())))
                    self.__supervisors.append(supervisor)
                    
                    # append robot after supervisor for the case of exceptions
                    self.__robots.append(robot)
                    
                    # Create trackers
                    self.__trackers.append(simobject.Path(robot.get_pose(),robot))
                    self.__trackers[-1].set_color(robot.get_color())
                except:
                    self.log("[Simulator.construct_world] Robot creation failed!")
                    raise
                    #raise Exception('[Simulator.construct_world] Unknown robot type!')
            elif thing_type == 'obstacle':
                obstacle_pose, obstacle_coords, obstacle_color = thing[1:4]
                if obstacle_color is None:
                    obstacle_color = 0xFF0000
                self.__obstacles.append(
                    simobject.Polygon(pose.Pose(obstacle_pose),
                                      obstacle_coords,
                                      obstacle_color))
            elif thing_type == 'marker':
                obj_pose, obj_coords, obj_color = thing[1:4]
                if obj_color is None:
                    obj_color = 0x00FF00
                self.__background.append(
                    simobject.Polygon(pose.Pose(obj_pose),
                                      obj_coords,
                                      obj_color))
            else:
                raise Exception('[Simulator.construct_world] Unknown object: '
                                + str(thing_type))
                                
        self.__time = 0.0
        if not self.__robots:
            raise Exception('[Simulator.construct_world] No robot specified!')
        else:
            self.__recalculate_default_zoom()
            if not self.__center_on_robot:
                self.focus_on_world()
            self.__supervisor_param_cache = None
            self.step_simulation()
            
        self._out_queue.put(('reset',()))

    def __recalculate_default_zoom(self):
        """Calculate the zoom level that will show the robot at about 10% its size
        """
        maxsize = 0
        for robot in self.__robots:
            xmin, ymin, xmax, ymax = robot.get_bounds()
            maxsize = max(maxsize,math.sqrt(float(xmax-xmin)**2 + float(ymax-ymin)**2))
        if maxsize == 0:
            self.__zoom_default = 1
        else:
            self.__zoom_default = max(self.__renderer.size)/maxsize/10
            
    def __reset_world(self):
        """Resets the world and objects to starting position.
        
           All the user's code will be reloaded.
        """
        if self.__world is None:
            return
        self.__supervisor_param_cache = [sv.get_parameters() for sv in self.__supervisors ]
        self.__construct_world()

    def run(self):
        """Start the thread. In the beginning there's no world, no obstacles
           and no robots.
           
           The simulator will try to draw the world undependently of the
           simulation status, so that the commands from the UI get processed.
        """
        self.log('starting simulator thread')

        time_constant = 0.02 # 20 milliseconds
        
        self.__renderer.clear_screen() #create a white screen
        self.__update_view()

        while not self.__stop:

            try:

                sleep(time_constant/self.__time_multiplier)

                self.__process_queue()

                if self.__state == RUN or \
                   self.__state == RUN_ONCE:

                    self.__time += time_constant

                    # First, move robots
                    for i, robot in enumerate(self.__robots):
                        robot.move(time_constant)
                        self.__trackers[i].add_point(robot.get_pose())

                    self.fwd_logqueue()

                    # Second, check for collisions and update sensors
                    if self.__check_collisions():
                        #print("Collision detected!")
                        self.__state = DRAW_ONCE

                    self.fwd_logqueue()

                    # Now calculate supervisor outputs for the new position
                    for i, supervisor in enumerate(self.__supervisors):
                        info = self.__robots[i].get_info()
                        inputs = supervisor.execute( info, time_constant)
                        self.__robots[i].set_inputs(inputs)
                        self.fwd_logqueue()

                    if self.plot_expressions:
                        self.announce_plotables()

                # Draw to buffer-bitmap
                # Note that if the robot moves immediately after calculation,
                # the supervisor would draw the previous state.
                if self.__state != PAUSE:
                    self.__draw()
                    
                if self.__state == DRAW_ONCE or \
                   self.__state == RUN_ONCE:
                    self.pause_simulation()

                self.fwd_logqueue()
            
            except Exception as e:
                self._out_queue.put(("exception",sys.exc_info()))
                self.pause_simulation()
                self.fwd_logqueue()

    def __draw(self):
        """Draws the world and items in it.
        
           This will draw the markers, the obstacles,
           the robots, their tracks and their sensors
        """
        
        if self.__robots and self.__center_on_robot:
            # Temporary fix - center onto first robot
            robot = self.__robots[0]
            if self.__orient_on_robot:
                self.__renderer.set_screen_center_pose(robot.get_pose())
            else:
                self.__renderer.set_screen_center_pose(pose.Pose(robot.get_pose().x, robot.get_pose().y, 0.0))

        self.__renderer.clear_screen()

        if self.__draw_supervisors:
            for supervisor in self.__supervisors:
                supervisor.draw_background(self.__renderer)

        for bg_object in self.__background:
            bg_object.draw(self.__renderer)
        for obstacle in self.__obstacles:
            obstacle.draw(self.__renderer)

        # Draw the robots, trackers and sensors after obstacles
        if self.__show_tracks:
            for tracker in self.__trackers:
                tracker.draw(self.__renderer)
        for robot in self.__robots:
            robot.draw(self.__renderer)
            if self.__show_sensors:
                robot.draw_sensors(self.__renderer)

        if self.__draw_supervisors:
            for supervisor in self.__supervisors:
                supervisor.draw_foreground(self.__renderer)

        # update view
        self.__update_view()

    def __update_view(self):
        """Signal the UI that the drawing process is finished,
           and it is safe to access the renderer.
        """
        self._out_queue.put(('update_view',()))
        self._out_queue.join() # wait until drawn

    def __draw_once(self):
        if self.__state == PAUSE:
            self.__state = DRAW_ONCE
            
    def refresh(self):
        self.__draw_once()
        
    def focus_on_world(self):
        """Scale the view to include all of the world (including robots)"""
        def include_bounds(bounds, o_bounds):
            xl, yb, xr, yt = bounds
            xlo, ybo, xro, yto = o_bounds
            if xlo < xl: xl = xlo
            if xro > xr: xr = xro
            if ybo < yb: yb = ybo
            if yto > yt: yt = yto
            return xl, yb, xr, yt
        
        def bloat_bounds(bounds, factor):
            xl, yb, xr, yt = bounds
            w = xr-xl
            h = yt-yb
            factor = (factor-1)/2.0
            return xl - w*factor, yb - h*factor, xr + w*factor, yt + h*factor
            
        self.__center_on_robot = False
        bounds = self.__robots[0].get_bounds()
        for robot in self.__robots:
            bounds = include_bounds(bounds, bloat_bounds(robot.get_bounds(),4))
        for obstacle in self.__obstacles:
            bounds = include_bounds(bounds, obstacle.get_bounds())
        xl, yb, xr, yt = bounds
        self.__renderer.set_view_rect(xl,yb,xr-xl,yt-yb)
        self.__draw_once()

    def focus_on_robot(self, rotate = True):
        """Center the view on the (first) robot and follow it.
        
           If *rotate* is true, also follow the robot's orientation.
        """
        self.__center_on_robot = True
        self.__orient_on_robot = rotate
        self.__draw_once()

    def show_sensors(self, show = True):
        """Show or hide the robots' sensors on the simulation view
        """
        self.__show_sensors = show
        self.__draw_once()

    def show_tracks(self, show = True):
        """Show/hide tracks for every robot on simulator view"""
        self.__show_tracks = show
        self.__draw_once()

    def show_supervisors(self, show = True):
        """Show/hide the information from the supervisors"""
        self.__draw_supervisors = show
        self.__draw_once()

    def show_grid(self, show=True):
        """Show/hide gridlines on simulator view"""
        self.__renderer.show_grid(show)
        self.__draw_once()

    def adjust_zoom(self,factor):
        """Zoom the view by *factor*"""
        self.__renderer.set_zoom_level(self.__zoom_default*factor)
        self.__draw_once()
        
    def apply_parameters(self,robot,parameters):
        """Apply *parameters* to the supervisor of *robot*.
        
        The parameters have to correspond to the requirements of the supervisor,
        as specified in :meth:`supervisor.Supervisor.get_ui_description`
        """
        index = self.__robots.index(robot)
        if index < 0:
            self.log("Robot not found")
        else:
            self.__supervisors[index].set_parameters(parameters)
        self.__draw_once()

    def add_plotable(self,expression):
        """A plotable is an expression that yields a numerical
           value. It is evaluated every cycle and the values are announced by the
           simulator in the ``plot_update`` signal.
           
           The expression has access to the following variables:
           ``robot`` - the first robot in the scene
           ``supervisor`` - the supervisor of this robot
           ``math`` - the math module
           """
        if expression is not None and expression not in self.plot_expressions:
            self.plot_expressions.append(expression)

    def announce_plotables(self):
        plots = {'time':self.__time}
        for expr in self.plot_expressions:
            try:
                plots[expr] = \
                    eval(expr,{},
                         {'robot':self.__robots[0],
                          'supervisor':self.__supervisors[0],
                          'math':math})
            except Exception as e:
                self.log(str(e))
                plots[expr] = 0
        self._out_queue.put(('plot_update',(plots,)))

    def plotables(self):
        """ Returns a list with some examples of plotables"""
        return {
            "Robot's X coordinate":"robot.get_pose().x",
            "Robot's Y coordinate":"robot.get_pose().y",
            "Robot's orientation":"robot.get_pose().theta",
            "Robot's orientation (degrees)":"robot.get_pose().theta*57.29578",
            "Estimated X coordinate":"supervisor.pose_est.x",
            "Estimated Y coordinate":"supervisor.pose_est.y",
            "Estimated orientation":"supervisor.pose_est.theta",
            "Estimated orientation (degrees)":"supervisor.pose_est.theta*57.29578",
            "Left wheel speed":"robot.ang_velocity[0]",
            "Right wheel speed":"robot.ang_velocity[1]",
            "Linear velocity":"robot.diff2uni(robot.ang_velocity)[0]",
            "Angular velocity":"robot.diff2uni(robot.ang_velocity)[1]"
            # The possibilities are infinite
            #"":"",
            }

    # Stops the thread
    def stop(self):
        """Stop the simulator thread when the entire program is closed"""
        self.log('stopping simulator thread')
        self.__stop = True
        self._out_queue.put(('stopped',()))

    def start_simulation(self):
        """Start/continue the simulation"""
        if self.__robots:
            self.__state = RUN
            self._out_queue.put(('running',()))

    def pause_simulation(self):
        """Pause the simulation"""
        self.__state = PAUSE
        self._out_queue.put(('paused',()))

    def step_simulation(self):
        """Do one step"""
        if self.__state != RUN:
            self.__state = RUN_ONCE
        #self._out_queue.put(('paused',()))

    def reset_simulation(self):
        """Reset the simulation to the start position"""
        self.__state = DRAW_ONCE
        self.__reset_world()

    def set_time_multiplier(self,multiplier):
        """Shorten the interval between evaluation cycles by *multiplier*,
           speeding up the simulation"""
        self.__time_multiplier = multiplier

### FIXME Those two functions are not thread-safe
    def get_time(self):
        """Get the internal simulator time."""
        return self.__time

    def is_running(self):
        """Get the simulation state as a `bool`"""
        return self.__state == RUN
###------------------

    def __check_collisions(self):
        """Update proximity sensors and detect collisions between objects"""
        
        collisions = []
        checked_robots = []
        
        if self.__qtree is None:
            self.__qtree = QuadTree(self.__obstacles)
            
        if len(self.__robots) > 1:
            rqtree = QuadTree(self.__robots)
        else: rqtree = None
        
        # check each robot
        for robot in self.__robots:
                
            # update proximity sensors
            for sensor in robot.get_external_sensors():
                sensor.get_world_envelope(True)
                rect = Rect(sensor.get_bounding_rect())
                sensor.update_distance()
                # distance to obstacles
                for obstacle in self.__qtree.find_items(rect):
                    sensor.update_distance(obstacle)
                # distance to other robots
                if rqtree is None: continue
                for other in rqtree.find_items(rect):
                    if other is not robot:
                        sensor.update_distance(other)
            
            rect = Rect(robot.get_bounding_rect())
            
            # against nearest obstacles
            for obstacle in self.__qtree.find_items(rect):
                if robot.has_collision(obstacle):
                    collisions.append((robot, obstacle))
            
            # against other robots
            if rqtree is not None:
                for other in rqtree.find_items(rect):
                    if other is robot: continue
                    if other in checked_robots: continue
                    if robot.has_collision(other):
                        collisions.append((robot, other))

            checked_robots.append(robot)
            
        if len(collisions) > 0:
            # Test code - print out collisions
            for (robot, obstacle) in collisions:
                self.log("Collision with {}".format(obstacle), obj = robot)
            # end of test code
            return True
                
        return False

    def __process_queue(self):
        """Process external calls
        """
        while not self.__in_queue.empty():
            tpl = self.__in_queue.get()
            if isinstance(tpl,tuple) and len(tpl) == 2:
                name, args = tpl
                if name in self.__class__.__dict__:
                    try:
                        self.__class__.__dict__[name](self,*args)
                    except TypeError:
                        self.log("Wrong simulator event parameters {}{}".format(name,args))
                        self._out_queue.put(("exception",sys.exc_info()))
                    except Exception as e:
                        self._out_queue.put(("exception",sys.exc_info()))
                else:
                    self.log("Unknown simulator event '{}'".format(name))
            else:
                self.log("Wrong simulator event format '{}'".format(tpl))
            self.__in_queue.task_done()
    
    def log(self, message, obj=None):
        if obj is None:
            obj = self
        print("{}: {}".format(obj.__class__.__name__,message))
        self._out_queue.put(("log",(message,obj.__class__.__name__,None)))
        
    def fwd_logqueue(self):
        while self.__log_queue:
            obj, message = self.__log_queue.popleft()
            
            color = None
            # Get the color
            if isinstance(obj,simobject.SimObject):
                color = obj.get_color()
            elif isinstance(obj,supervisor.Supervisor):
                color = obj.robot_color
                
            self._out_queue.put(("log",(message,obj.__class__.__name__,color)))
Example #35
0
import numpy as np
import matplotlib.pyplot as plt
from quadtree import Point, Rect, QuadTree
from matplotlib import gridspec

DPI = 72
np.random.seed(13)

width, height = 600, 400

N = 5000
coords = np.random.randn(N, 2) * height / 3 + (width / 2, height / 2)
points = [Point(*coord) for coord in coords]

domain = Rect(width / 2, height / 2, width, height)
qtree = QuadTree(domain, 3)
for point in points:
    qtree.insert(point)

print('Number of points in the domain =', len(qtree))

fig = plt.figure(figsize=(700 / DPI, 500 / DPI), dpi=DPI)
ax = plt.subplot()
ax.set_xlim(0, width)
ax.set_ylim(0, height)
qtree.draw(ax)

ax.scatter([p.x for p in points], [p.y for p in points], s=4)
ax.set_xticks([])
ax.set_yticks([])
class Simulator(threading.Thread):
    """The simulator manages simobjects and their collisions, commands supervisors
       and draws the world using the supplied *renderer*.
       
       The simulator runs in a separate thread. None of its functions are thread-safe,
       and should never be called directly from other objects (except for the functions
       inherited from `threading.Thread`). The communication with the simulator
       should be done through its *in_queue* and *out_queue*. See :ref:`ui-sim-queue`.
       
       :param renderer: The renderer that will be used to draw the world.
                        The simulator will assume control of the renderer.
                        The renderer functions also have to be considered thread-unsafe.
       :type renderer: :class:`~renderer.Renderer`
       :param in_queue: The queue that is used to send events to the simulator.
       :type in_queue: :class:`Queue.Queue`
    """

    __nice_colors = (0x55AAEE, 0x66BB22, 0xFFBB22, 0xCC66AA, 0x77CCAA,
                     0xFF7711, 0xFF5555, 0x55CC88)

    def __init__(self, renderer, in_queue):
        """Create a simulator with *renderer* and *in_queue*
        """
        super(Simulator, self).__init__()

        #Attributes
        self.__stop = False
        self.__state = PAUSE
        self.__renderer = renderer
        self.__center_on_robot = False
        self.__orient_on_robot = False
        self.__show_sensors = True
        self.__draw_supervisors = False
        self.__show_tracks = True

        self.__in_queue = in_queue
        self._out_queue = queue.Queue()

        # Zoom on scene - Move to read_config later
        self.__time_multiplier = 1.0
        self.__time = 0.0

        # World objects
        self.__robots = []
        self.__trackers = []
        self.__obstacles = []
        self.__supervisors = []
        self.__background = []
        self.__zoom_default = 1

        self.__world = None

        self.__log_queue = deque()

        # Internal objects
        self.__qtree = None

    def read_config(self, filename):
        '''Load in the objects from the world XML file '''

        self.log('reading initial configuration')
        try:
            self.__world = XMLReader(filename, 'simulation').read()
        except Exception as e:
            raise Exception('[Simulator.read_config] Failed to parse ' + filename \
                + ': ' + str(e))
        else:
            self.__supervisor_param_cache = None
            self.__center_on_robot = False
            self.__construct_world()

    def __construct_world(self):
        """Creates objects previously loaded from the world xml file.
           
           This function uses the world in ``self.__world``.
           
           All the objects will be created anew, including robots and supervisors.
           All of the user's code is reloaded.
        """
        if self.__world is None:
            return

        helpers.unload_user_modules()

        self.__state = DRAW_ONCE

        self.__robots = []
        self.__obstacles = []
        self.__supervisors = []
        self.__background = []
        self.__trackers = []
        self.__qtree = None

        for thing in self.__world:
            thing_type = thing[0]
            if thing_type == 'robot':
                robot_type, supervisor_type, robot_pose, robot_color = thing[
                    1:5]
                try:
                    # Create robot
                    robot_class = helpers.load_by_name(robot_type, 'robots')
                    robot = robot_class(pose.Pose(robot_pose))
                    if robot_color is not None:
                        robot.set_color(robot_color)
                    elif len(self.__robots) < 8:
                        robot.set_color(self.__nice_colors[len(self.__robots)])

                    # Create supervisor
                    sup_class = helpers.load_by_name(supervisor_type,
                                                     'supervisors')

                    info = robot.get_info()
                    info.color = robot.get_color()
                    supervisor = sup_class(robot.get_pose(), info)
                    supervisor.set_logqueue(self.__log_queue)
                    name = "Robot {}: {}".format(
                        len(self.__robots) + 1, sup_class.__name__)
                    if self.__supervisor_param_cache is not None and \
                       len(self.__supervisor_param_cache) > len(self.__supervisors):
                        supervisor.set_parameters(
                            self.__supervisor_param_cache[len(
                                self.__supervisors)])
                    self._out_queue.put(
                        ("make_param_window",
                         (robot, name, supervisor.get_ui_description())))
                    self.__supervisors.append(supervisor)

                    # append robot after supervisor for the case of exceptions
                    self.__robots.append(robot)

                    # Create trackers
                    self.__trackers.append(
                        simobject.Path(robot.get_pose(), robot))
                    self.__trackers[-1].set_color(robot.get_color())
                except:
                    self.log(
                        "[Simulator.construct_world] Robot creation failed!")
                    raise
                    #raise Exception('[Simulator.construct_world] Unknown robot type!')
            elif thing_type == 'obstacle':
                obstacle_pose, obstacle_coords, obstacle_color = thing[1:4]
                if obstacle_color is None:
                    obstacle_color = 0xFF0000
                self.__obstacles.append(
                    simobject.Polygon(pose.Pose(obstacle_pose),
                                      obstacle_coords, obstacle_color))
            elif thing_type == 'marker':
                obj_pose, obj_coords, obj_color = thing[1:4]
                if obj_color is None:
                    obj_color = 0x00FF00
                self.__background.append(
                    simobject.Polygon(pose.Pose(obj_pose), obj_coords,
                                      obj_color))
            else:
                raise Exception(
                    '[Simulator.construct_world] Unknown object: ' +
                    str(thing_type))

        self.__time = 0.0
        if not self.__robots:
            raise Exception('[Simulator.construct_world] No robot specified!')
        else:
            self.__recalculate_default_zoom()
            if not self.__center_on_robot:
                self.focus_on_world()
            self.__supervisor_param_cache = None
            self.step_simulation()

        self._out_queue.put(('reset', ()))

    def __recalculate_default_zoom(self):
        """Calculate the zoom level that will show the robot at about 10% its size
        """
        maxsize = 0
        for robot in self.__robots:
            xmin, ymin, xmax, ymax = robot.get_bounds()
            maxsize = max(maxsize,
                          sqrt(float(xmax - xmin)**2 + float(ymax - ymin)**2))
        if maxsize == 0:
            self.__zoom_default = 1
        else:
            self.__zoom_default = max(self.__renderer.size) / maxsize / 10

    def __reset_world(self):
        """Resets the world and objects to starting position.
        
           All the user's code will be reloaded.
        """
        if self.__world is None:
            return
        self.__supervisor_param_cache = [
            sv.get_parameters() for sv in self.__supervisors
        ]
        self.__construct_world()

    def run(self):
        """Start the thread. In the beginning there's no world, no obstacles
           and no robots.
           
           The simulator will try to draw the world undependently of the
           simulation status, so that the commands from the UI get processed.
        """
        self.log('starting simulator thread')

        time_constant = 0.02  # 20 milliseconds

        self.__renderer.clear_screen()  #create a white screen
        self.__update_view()

        while not self.__stop:

            try:

                sleep(time_constant / self.__time_multiplier)

                self.__process_queue()

                if self.__state == RUN or \
                   self.__state == RUN_ONCE:

                    self.__time += time_constant

                    # First, move robots
                    for i, robot in enumerate(self.__robots):
                        robot.move(time_constant)
                        self.__trackers[i].add_point(robot.get_pose())

                    self.fwd_logqueue()

                    # Second, check for collisions and update sensors
                    if self.__check_collisions():
                        #print("Collision detected!")
                        self.__state = DRAW_ONCE

                    self.fwd_logqueue()

                    # Now calculate supervisor outputs for the new position
                    for i, supervisor in enumerate(self.__supervisors):
                        info = self.__robots[i].get_info()
                        inputs = supervisor.execute(info, time_constant)
                        self.__robots[i].set_inputs(inputs)
                        self.fwd_logqueue()

                # Draw to buffer-bitmap
                # Note that if the robot moves immediately after calculation,
                # the supervisor would draw the previous state.
                if self.__state != PAUSE:
                    self.__draw()

                if self.__state == DRAW_ONCE or \
                   self.__state == RUN_ONCE:
                    self.pause_simulation()

                self.fwd_logqueue()

            except Exception as e:
                self._out_queue.put(("exception", sys.exc_info()))
                self.pause_simulation()
                self.fwd_logqueue()

    def __draw(self):
        """Draws the world and items in it.
        
           This will draw the markers, the obstacles,
           the robots, their tracks and their sensors
        """

        if self.__robots and self.__center_on_robot:
            # Temporary fix - center onto first robot
            robot = self.__robots[0]
            if self.__orient_on_robot:
                self.__renderer.set_screen_center_pose(robot.get_pose())
            else:
                self.__renderer.set_screen_center_pose(
                    pose.Pose(robot.get_pose().x,
                              robot.get_pose().y, 0.0))

        self.__renderer.clear_screen()

        for bg_object in self.__background:
            bg_object.draw(self.__renderer)
        for obstacle in self.__obstacles:
            obstacle.draw(self.__renderer)

        # Draw the robots, trackers and sensors after obstacles
        if self.__show_tracks:
            for tracker in self.__trackers:
                tracker.draw(self.__renderer)
        for robot in self.__robots:
            robot.draw(self.__renderer)
            if self.__show_sensors:
                robot.draw_sensors(self.__renderer)

        if self.__draw_supervisors:
            for supervisor in self.__supervisors:
                supervisor.draw(self.__renderer)

        # update view
        self.__update_view()

    def __update_view(self):
        """Signal the UI that the drawing process is finished,
           and it is safe to access the renderer.
        """
        self._out_queue.put(('update_view', ()))
        self._out_queue.join()  # wait until drawn

    def __draw_once(self):
        if self.__state == PAUSE:
            self.__state = DRAW_ONCE

    def refresh(self):
        self.__draw_once()

    def focus_on_world(self):
        """Scale the view to include all of the world (including robots)"""
        def include_bounds(bounds, o_bounds):
            xl, yb, xr, yt = bounds
            xlo, ybo, xro, yto = o_bounds
            if xlo < xl: xl = xlo
            if xro > xr: xr = xro
            if ybo < yb: yb = ybo
            if yto > yt: yt = yto
            return xl, yb, xr, yt

        def bloat_bounds(bounds, factor):
            xl, yb, xr, yt = bounds
            w = xr - xl
            h = yt - yb
            factor = (factor - 1) / 2.0
            return xl - w * factor, yb - h * factor, xr + w * factor, yt + h * factor

        self.__center_on_robot = False
        bounds = self.__robots[0].get_bounds()
        for robot in self.__robots:
            bounds = include_bounds(bounds,
                                    bloat_bounds(robot.get_bounds(), 4))
        for obstacle in self.__obstacles:
            bounds = include_bounds(bounds, obstacle.get_bounds())
        xl, yb, xr, yt = bounds
        self.__renderer.set_view_rect(xl, yb, xr - xl, yt - yb)
        self.__draw_once()

    def focus_on_robot(self, rotate=True):
        """Center the view on the (first) robot and follow it.
        
           If *rotate* is true, also follow the robot's orientation.
        """
        self.__center_on_robot = True
        self.__orient_on_robot = rotate
        self.__draw_once()

    def show_sensors(self, show=True):
        """Show or hide the robots' sensors on the simulation view
        """
        self.__show_sensors = show
        self.__draw_once()

    def show_tracks(self, show=True):
        """Show/hide tracks for every robot on simulator view"""
        self.__show_tracks = show
        self.__draw_once()

    def show_supervisors(self, show=True):
        """Show/hide the information from the supervisors"""
        self.__draw_supervisors = show
        self.__draw_once()

    def show_grid(self, show=True):
        """Show/hide gridlines on simulator view"""
        self.__renderer.show_grid(show)
        self.__draw_once()

    def adjust_zoom(self, factor):
        """Zoom the view by *factor*"""
        self.__renderer.set_zoom_level(self.__zoom_default * factor)
        self.__draw_once()

    def apply_parameters(self, robot, parameters):
        """Apply *parameters* to the supervisor of *robot*.
        
        The parameters have to correspond to the requirements of the supervisor,
        as specified in :meth:`supervisor.Supervisor.get_ui_description`
        """
        index = self.__robots.index(robot)
        if index < 0:
            self.log("Robot not found")
        else:
            self.__supervisors[index].set_parameters(parameters)
        self.__draw_once()

    # Stops the thread
    def stop(self):
        """Stop the simulator thread when the entire program is closed"""
        self.log('stopping simulator thread')
        self.__stop = True
        self._out_queue.put(('stopped', ()))

    def start_simulation(self):
        """Start/continue the simulation"""
        if self.__robots:
            self.__state = RUN
            self._out_queue.put(('running', ()))

    def pause_simulation(self):
        """Pause the simulation"""
        self.__state = PAUSE
        self._out_queue.put(('paused', ()))

    def step_simulation(self):
        """Do one step"""
        if self.__state != RUN:
            self.__state = RUN_ONCE
        #self._out_queue.put(('paused',()))

    def reset_simulation(self):
        """Reset the simulation to the start position"""
        self.__state = DRAW_ONCE
        self.__reset_world()

    def set_time_multiplier(self, multiplier):
        """Shorten the interval between evaluation cycles by *multiplier*,
           speeding up the simulation"""
        self.__time_multiplier = multiplier

### FIXME Those two functions are not thread-safe

    def get_time(self):
        """Get the internal simulator time."""
        return self.__time

    def is_running(self):
        """Get the simulation state as a `bool`"""
        return self.__state == RUN
###------------------

    def __check_collisions(self):
        """Update proximity sensors and detect collisions between objects"""

        collisions = []
        checked_robots = []

        if self.__qtree is None:
            self.__qtree = QuadTree(self.__obstacles)

        if len(self.__robots) > 1:
            rqtree = QuadTree(self.__robots)
        else:
            rqtree = None

        # check each robot
        for robot in self.__robots:

            # update proximity sensors
            for sensor in robot.get_external_sensors():
                sensor.get_world_envelope(True)
                rect = Rect(sensor.get_bounding_rect())
                sensor.update_distance()
                # distance to obstacles
                for obstacle in self.__qtree.find_items(rect):
                    sensor.update_distance(obstacle)
                # distance to other robots
                if rqtree is None: continue
                for other in rqtree.find_items(rect):
                    if other is not robot:
                        sensor.update_distance(other)

            rect = Rect(robot.get_bounding_rect())

            # against nearest obstacles
            for obstacle in self.__qtree.find_items(rect):
                if robot.has_collision(obstacle):
                    collisions.append((robot, obstacle))

            # against other robots
            if rqtree is not None:
                for other in rqtree.find_items(rect):
                    if other is robot: continue
                    if other in checked_robots: continue
                    if robot.has_collision(other):
                        collisions.append((robot, other))

            checked_robots.append(robot)

        if len(collisions) > 0:
            # Test code - print out collisions
            for (robot, obstacle) in collisions:
                self.log("Collision with {}".format(obstacle), obj=robot)
            # end of test code
            return True

        return False

    def __process_queue(self):
        """Process external calls
        """
        while not self.__in_queue.empty():
            tpl = self.__in_queue.get()
            if isinstance(tpl, tuple) and len(tpl) == 2:
                name, args = tpl
                if name in self.__class__.__dict__:
                    try:
                        self.__class__.__dict__[name](self, *args)
                    except TypeError:
                        self.log(
                            "Wrong simulator event parameters {}{}".format(
                                name, args))
                        self._out_queue.put(("exception", sys.exc_info()))
                    except Exception as e:
                        self._out_queue.put(("exception", sys.exc_info()))
                else:
                    self.log("Unknown simulator event '{}'".format(name))
            else:
                self.log("Wrong simulator event format '{}'".format(tpl))
            self.__in_queue.task_done()

    def log(self, message, obj=None):
        if obj is None:
            obj = self
        print("{}: {}".format(obj.__class__.__name__, message))
        self._out_queue.put(("log", (message, obj.__class__.__name__, None)))

    def fwd_logqueue(self):
        while self.__log_queue:
            obj, message = self.__log_queue.popleft()

            color = None
            # Get the color
            if isinstance(obj, simobject.SimObject):
                color = obj.get_color()
            elif isinstance(obj, supervisor.Supervisor):
                color = obj.robot_color

            self._out_queue.put(
                ("log", (message, obj.__class__.__name__, color)))
Example #37
0
def main():
  QuadTree.maxSize = 1
  qt = QuadTree( (-160,-160), (160,160))
  Node.tree = qt

  assert qt.root.isLeaf
  assert qt.root.pointCount == 0
  print('Test 1 passed')
  pts = [[Node(i+10*random.random(),j+10*random.random()) for j in range(-170,180,20)] for i in range(-170,180,20)]
  for row in pts:
    for p in row:
      p.add()
  qt.addPoint(pts[1][1])

  assert qt.root.isLeaf
  assert qt.root.pointCount == 1
  print('Test 2 passed')
  qt.addPoint(pts[6][2])

  assert not qt.root.isLeaf
  assert qt.root.pointCount == 2
  print('Test 3 passed')
  qt.addPoint(pts[0][0])

  assert qt.root.minX < -16
  assert qt.root.pointCount == 3
  print('Test 4 passed')
  
  

  draw(qt, 'tree1.png')
  print('Tree1 complete')
  qt = QuadTree( (-155, -155), (155,155))
  Node.tree = qt
  points = [Node(x,y) for x,y in [(30,50),(30,70),(50,90),(70,90),(90,70),(90,50),(70,30),(50,30)]]
  p1 = Node(50,50)
  p1.add()
  p2 = Node(50,70)
  p2.add()
  [p.add() for p in points]
  pairs = [(a,b) for a in points for b in points if (a.coord-b.coord).sum() > 0]
  roads = [TransportRoad(a,b) for a,b in pairs]
  [r.add() for r in roads]
  for i in range(len(roads)):
    qt = QuadTree( (-155,-155), (155,155))
    Node.tree = qt
    QuadTree.roadNodes = {}

    qt.addPoint(p1)

    qt.addPoint(p2)
    [qt.addPoint(p) for p in points]
    qt.addRoad(roads[i])
    draw(qt, 'treeroad%d.png' % i)
    print('Treeroad%d complete' % i)

  QuadTree.roadNodes = {}
  qt = QuadTree( (-160,-160), (160,160))
  Node.tree = qt
  added = []
  for i in range(100):
    
    #(row,col) in random.sample(list(itertools.product(range(1,len(pts)-1), range(1,len(pts)-1))), 100):
    x = (random.random()-.5) * 320
    y = (random.random()-.5) * 320
    p = Node(x,y)
    p.add()
    assert qt.root.parent is None
    qt.addPoint(p)
    added.append(p)
    draw(qt, 'tree2-%d.png' % i)



  toCheck = [qt]
  while len(toCheck) > 0:
    t = toCheck.pop()
    for c in t.children:
      toCheck.append(c)
    assert t.isLeaf or t.pointCount > 0, str(t)

  print('Tree2 complete')

  for i in range(20):
    QuadTree.roadNodes = {}
    qt = QuadTree( (-160,-160), (160,160))
    Node.tree = qt
    added = []
    for _ in range(100):
      x = (random.random()-.5) * 320
      y = (random.random()-.5) * 320
      p = Node(x,y)
      p.add()
      assert qt.root.parent is None
      qt.addPoint(p)
      added.append(p)
    a,b = random.sample(added,2)
    road = TransportRoad(a,b)
    road.add()

    qt.addRoad(road)

    draw(qt, 'treeroadr%d.png' % i)
    print('Treeroadr%d complete' % i)
Example #38
0
 def __init__(self, nrof_creatures, world):
     super().__init__()
     self.nrof_creatures = nrof_creatures
     self.world = world
     self.creatures = QuadTree(600, 600)
Example #39
0
 def setUp(self) -> None:
     self.quadtree = QuadTree(Bounds(0, 0, 400, 400),
                              max_objects=4,
                              max_level=5)
Example #40
0
 def __init__(self, rootnode, minrect, circles):
     CQuadTree.circles = circles
     QuadTree.__init__(self, rootnode, minrect)
Example #41
0
# Test playground for functions
if __name__ == "__main__":
    import p5_vis
    import random
    import datetime
    from p5 import *
    from aabb import AABB
    from point2d import Point2D
    from quadtree import QuadTree

    # 1000 x 1000 QuadTree
    q = QuadTree(AABB(0, 0, 1000, 1000))

    random.seed(datetime.datetime.now().utcoffset())

    vis_qt = p5_vis.VisQuadTree(q)
    # Insert 100000 points into QuadTree with gaussian distribution
    for x in range(0, 100000):
        p = Point2D(random.gauss(500, 100), random.gauss(500, 100))
        q.insert(p)

    # p5py setup function
    def setup():
        vis_qt.setup()

    # p5py run function
    run(sketch_draw=vis_qt.draw())

    pts = q.get_all()

    xtotal = 0
Example #42
0
 def __init__(self, rootnode, minrect, circles):
     CQuadTree.circles = circles
     QuadTree.__init__(self, rootnode, minrect)
Example #43
0
class CreatureManager(Observable):
    def __init__(self, nrof_creatures, world):
        super().__init__()
        self.nrof_creatures = nrof_creatures
        self.world = world
        self.creatures = QuadTree(600, 600)

    def create_creatures(self, nrof_creatures):
        """create nrof_creatures creatures"""
        for i in range(nrof_creatures):
            self.create_creature()

    def create_creature(self,
                        position=None,
                        creature_information=CreatureInformation(0, None)):
        """create a creature with default parameters"""
        if (position is None):
            position = (int(random.uniform(0,
                                           400)), int(random.uniform(0, 400)))
        information = creature_information
        dna = DNA(100, 100, 1, 5)
        #print(f"x:{position[0]} y:{position[1]}")
        creature = Creature(position, dna, information, self.world)
        self.register(creature, position)
        return creature

    def register(self, creature, position):
        self.creatures.insert(creature, position[0], position[1])
        creature.reproduce.register(self.create_offspring)
        creature.death.register(self.handle_death)
        self.notify()

    def unregister(self, creature):
        creature.reproduce.unregister(self.create_offspring)
        creature.death.unregister(self.handle_death)
        self.creatures.remove(creature)

    def handle_death(self, creature):
        """handle the death of a creature by unregistering it"""
        try:
            self.unregister(creature)
            self.notify()
        except ValueError as e:
            print("Tried removing unregistered creature from creature_manager")

    def get_creatures(self):
        """get all currently registered creatures"""
        return self.creatures.all()

    def update_creatures(self, delta_time):
        if (len(self.get_creatures()) < self.nrof_creatures):
            self.create_creatures(1)
        for creature in self.get_creatures():
            self.get_close_creatures(creature)
            creature.update(delta_time)

    def get_close_creatures(self, creature):
        creature.close_creatures = len(
            self.creatures.get_neighbors(creature.position[0],
                                         creature.position[1],
                                         creature.dna.sense_distance))

    def get_creature_highest_generation(self):
        return sorted(self.get_creatures(),
                      key=lambda creature: creature.information.generation)[-1]

    def create_offspring(self, creature):
        creature_information = CreatureInformation(
            creature.information.generation + 1, creature)
        new_position = (creature.position[0] + 5, creature.position[1] + 5)
        offspring = self.create_creature(
            position=creature.position,
            creature_information=creature_information)
        offspring.brain = Brain.from_existing_brain(creature.brain)
        offspring.brain.mutate()