示例#1
0
 def __get_robot_matrix (self):
     """Return robot transformation matrix."""
     m = TransMatrix ()
     m.rotate (-self.robot_position.angle)
     m.translate ((-self.robot_position.pos[0],
         -self.robot_position.pos[1]))
     return m
示例#2
0
文件: clamp.py 项目: TLoebner/apbteam
 def __robot_position_notified (self):
     # Compute robot direction.
     direction = self.__get_robot_direction ()
     # Update bottom slots.
     changed = False
     for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM):
         slot = self.slots[sloti]
         if direction == slot.side or direction is None:
             # If pushing, can take new elements.
             if slot.pawn is None:
                 p = self.__get_floor_elements (slot.side)
                 if p is not None:
                     slot.pawn = p
                     p.pos = None
                     p.notify ()
                     changed = True
         else:
             # Else, can drop elements.
             if slot.pawn is not None and slot.door_motor.angle:
                 m = TransMatrix ()
                 m.translate (self.robot_position.pos)
                 m.rotate (self.robot_position.angle)
                 xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[slot.side]
                 slot.pawn.pos = m.apply ((xoffset, 0))
                 slot.pawn.notify ()
                 slot.pawn = None
                 changed = True
     if changed:
         self.update_contacts ()
         self.notify ()
示例#3
0
 def __get_front_elements (self):
     """Return a list of elements in front of the robot, between clamp."""
     elements = [ ]
     if (self.robot_position is None
             or self.clamp_pos[0] is None
             or self.clamp_pos[1] is None):
         return elements
     # Matrix to transform an obstacle position into robot coordinates.
     m = TransMatrix ()
     m.rotate (-self.robot_position.angle)
     m.translate ((-self.robot_position.pos[0],
         -self.robot_position.pos[1]))
     # Look up elements.
     # This could be used if clamp blocking is handled or elements are
     # pushed:
     #ymin = -self.CLAMP_WIDTH / 2 + self.clamp_pos[1]
     #ymax = self.CLAMP_WIDTH / 2 - self.clamp_pos[0]
     ymin = -self.CLAMP_WIDTH + 20
     ymax = self.CLAMP_WIDTH - 20
     for o in self.table.obstacles:
         if o.level == 1 and o.pos is not None:
             pos = m.apply (o.pos)
             if (pos[0] > self.FRONT_ZONE_X_MIN
                     and pos[0] < self.FRONT_ZONE_X_MAX
                     and pos[1] > ymin and pos[1] < ymax):
                 elements.append (o)
     return elements
示例#4
0
 def __transform (self, pos):
     m = TransMatrix ()
     for i in self.into:
         if i.pos is None:
             return None
         m.translate (i.pos)
         m.rotate (i.angle)
     return m.apply (pos)
示例#5
0
 def __gate_notified (self):
     self.gate_angle = self.gate_link.angle
     if self.gate_angle is not None and self.robot_position is not None:
         # If gate is high, drop elements.
         if self.load and self.gate_angle > self.GATE_STROKE / 2:
             m = TransMatrix ()
             m.translate (self.robot_position.pos)
             m.rotate (self.robot_position.angle)
             pos = m.apply ((-250, 0))
             for e in self.load:
                 e.pos = pos
                 e.notify ()
             self.load = [ ]
     self.notify ()
示例#6
0
 def __pot_notified (self):
     self.firing = self.pot.wiper[0] > .5
     if self.cherries and self.firing:
         m = TransMatrix ()
         m.translate (self.robot_position.pos)
         m.rotate (self.robot_position.angle)
         hit = vector (*m.apply (self.cannon_hit))
         for c in self.cherries:
             c.pos = hit + vector.polar (random.uniform (-pi, pi),
                     random.uniform (0, 50))
             c.notify ()
         self.table.cherries.cherries.extend (self.cherries)
         self.table.cherries.notify ()
         self.cherries = [ ]
     self.notify ()
示例#7
0
 def evaluate (self):
     # Transform in the table base.
     pos, target = self.pos, self.target
     m = TransMatrix ()
     for i in self.into:
         if i.pos is None:
             self.distance = None
             return
         m.translate (i.pos)
         m.rotate (i.angle)
     pos, target = m.apply (pos, target)
     # Find intersection.
     i = self.table.intersect (pos, target, level = self.level,
             comp = lambda a, b: a < b, exclude = self.exclude)
     if i is not None:
         self.distance = i.distance
     else:
         self.distance = None
示例#8
0
 def __robot_position_notified (self):
     if self.robot_position.pos is None:
         return
     m = TransMatrix ()
     m.translate (self.robot_position.pos)
     m.rotate (self.robot_position.angle)
     x = -back
     y = (50 - 35, -50 - 35)
     for i, c in enumerate (self.contacts):
         s = True
         sensor_pos = m.apply ((x, y[i]))
         for o in self.table.obstacles:
             if (o.pos is not None and o.pos[1] > 0
                     and hasattr (o, 'inside') and o.inside (sensor_pos)):
                 s = False
                 break
         if s != self.contacts[i].state:
             self.contacts[i].state = s
             self.contacts[i].notify ()
示例#9
0
 def intersect (self, a, b):
     """If the segment [AB] intersects the obstacle, return distance from a
     to intersection point, else, return None."""
     if self.pos is None or self.angle is None:
         return None
     # Find intersection with each segments.  Return the nearest.
     m = TransMatrix ()
     m.translate (self.pos)
     m.rotate (self.angle)
     p = m.apply (*self.points)
     found = None
     def iter_seg (p):
         for i in xrange (len (p) - 1):
             yield p[i], p[i + 1]
         yield p[-1], p[0]
     for c, d in iter_seg (p):
         i = simu.utils.intersect.segment_segment (a, b, c, d)
         if i is not None:
             if found is not None:
                 found = min (found, i)
                 break
             else:
                 found = i
     return found
示例#10
0
文件: clamp.py 项目: TLoebner/apbteam
 def draw (self):
     self.reset ()
     # Draw base from side.
     self.trans_translate ((0, -165))
     self.draw_line ((-150, 0), (150, 0), fill = GREY)
     self.draw_line ((-20, 60), (20, 60), fill = GREY)
     self.draw_line ((-20, 120), (20, 120), fill = GREY)
     self.draw_line ((0, 0), (0, 330), fill = GREY)
     # Draw slots.
     for slot in self.model.slots:
         if slot.pawn is not None:
             self.draw_pawn ((-slot.x, slot.z), slot.pawn)
     # Draw clamp.
     if self.model.rotation is not None:
         self.trans_push ()
         self.trans_translate ((0, self.model.elevation))
         m = TransMatrix ()
         m.rotate (pi + self.model.rotation)
         # 2D projection of a 3D circular clamp.
         ltip = m.apply ((150, 103))[0]
         lbase = m.apply ((47, 0))[0]
         lcenter = m.apply ((150, 0))[0]
         m.rotate (- self.model.clamping / 43)
         rtip = m.apply ((150, -103))[0]
         rbase = m.apply ((47, 0))[0]
         rcenter = m.apply ((150, 0))[0]
         s, c = sin (self.model.rotation), cos (self.model.rotation)
         dattr = dict (outline = BLACK, fill = DGREY)
         attr = dict (outline = BLACK, fill = GREY)
         if c >= 0:
             if s >= 0:
                 self.draw_rectangle ((lbase, 10), (lcenter + 103, 40), **dattr)
                 self.draw_rectangle ((rtip, 10), (rbase, 40), **dattr)
                 self.draw_pawn ((lcenter, 0), self.model.load)
                 self.draw_rectangle ((ltip, 10), (lcenter + 103, 40), **attr)
             else:
                 self.draw_rectangle ((rtip, 10), (rcenter + 103, 40), **dattr)
                 self.draw_pawn ((lcenter, 0), self.model.load)
                 self.draw_rectangle ((ltip, 10), (lbase, 40), **attr)
                 self.draw_rectangle ((rbase, 10), (rcenter + 103, 40), **attr)
         else:
             if s >= 0:
                 self.draw_rectangle ((lbase, 10), (ltip, 40), **dattr)
                 self.draw_rectangle ((rbase, 10), (rcenter - 103, 40), **dattr)
                 self.draw_pawn ((lcenter, 0), self.model.load)
                 self.draw_rectangle ((rtip, 10), (rcenter - 103, 40), **attr)
             else:
                 self.draw_rectangle ((ltip, 10), (lcenter - 103, 40), **dattr)
                 self.draw_pawn ((lcenter, 0), self.model.load)
                 self.draw_rectangle ((lbase, 10), (lcenter - 103, 40), **attr)
                 self.draw_rectangle ((rbase, 10), (rtip, 40), **attr)
         self.trans_pop ()
     # Draw doors.
     for slot in self.model.slots:
         if slot.door_motor is not None:
             self.trans_push ()
             self.trans_translate ((-slot.x, slot.z + 50))
             self.trans_rotate (-0.5 * pi + slot.door_motor.angle)
             self.draw_line ((0, 0), (40, 0))
             self.trans_pop ()
示例#11
0
 def draw (self):
     # Redraw.
     self.reset ()
     # Table.
     self.draw_rectangle ((-22, 0), (3000 + 22, 2100 + 22), fill = 'white')
     self.draw_rectangle ((-22, -10), (3000 + 22, 0), fill = PLEXI)
     self.draw_rectangle ((0, 0), (3000, 2100), fill = BLUE)
     self.draw_rectangle ((0, 2100 - 500), (500, 2100), fill = GREEN)
     self.draw_rectangle ((3000 - 500, 2100 - 500), (3000, 2100), fill = RED)
     # Axes.
     self.draw_line ((0, 200), (0, 0), (200, 0), arrow = 'both')
     # Beacons.
     self.draw_rectangle ((-22, 2100 + 22), (-22 - 80, 2100 + 22 + 80), fill = BLACK)
     self.draw_rectangle ((-22, 1050 - 40), (-22 - 80, 1050 + 40), fill = BLACK)
     self.draw_rectangle ((-22, -80 - 10), (-22 - 80, -10), fill = BLACK)
     self.draw_rectangle ((3000 + 22, 2100 + 22), (3000 + 22 + 80, 2100 + 22 + 80), fill = BLACK)
     self.draw_rectangle ((3000 + 22, 1050 - 40), (3000 + 22 + 80, 1050 + 40), fill = BLACK)
     self.draw_rectangle ((3000 + 22, -80 - 10), (3000 + 22 + 80, -10), fill = BLACK)
     # Building areas.
     self.draw_circle ((1500, 1050), 150, fill = BROWN)
     self.draw_rectangle ((1500 - 900, 0), (1500 + 900, 100), fill = BROWN)
     self.draw_rectangle ((1500 - 300, 0), (1500 + 300, 100), fill = BROWN)
     self.draw_rectangle ((1500 - 900 - 22, 0), (1500 - 900, 100), fill = 'white')
     self.draw_rectangle ((1500 + 900 + 22, 0), (1500 + 900, 100), fill = 'white')
     for bx in (-600, 0, +600):
         for dx in (-130 - 65, -65, +65, +130 + 65):
             x = 1500 + bx + dx
             self.draw_rectangle ((x - 7.5, 100), (x + 7.5, 100 + 250), fill = BLACK)
     # Lintel dispensers.
     for x in (1500 - 600, 1500 - 200, 1500 + 200, 1500 + 600):
         self.draw_rectangle ((x - 100, 2100), (x + 100, 2100 + 70 + 22), fill = BLACK)
         if x < 1500:
             color = GREEN
         else:
             color = RED
         self.draw_rectangle ((x - 100, 2100), (x + 100, 2100 + 70), fill = color)
         self.draw_rectangle ((x - 7.5, 2100), (x + 7.5, 2100 - 250), fill = BLACK)
     # Vertical dispensers.
     if self.model.dispensers_card == 1:
         ds = -250
     else:
         ds = +250
     for dpos, dangle, dcolor in (
             ((289, 40), pi / 2, RED),
             ((3000 - 289, 40), pi / 2, GREEN),
             ((40, 1050 + ds), 0, RED),
             ((3000 - 40, 1050 + ds), pi, GREEN),
             ):
         dtm = TransMatrix ()
         dtm.translate (dpos)
         dtm.rotate (dangle)
         if dpos[1] == 40:
             a = 55
         else:
             a = 67
         self.draw_rectangle (dtm.apply ((-a, -25)), dtm.apply ((0, 25)), fill = PLEXI)
         self.draw_rectangle (dtm.apply ((-40, -40)), dtm.apply ((40, 40)), fill = PLEXI)
         self.draw_circle (dtm.apply ((0, 0)), 35, fill = PLEXI, outline = dcolor)
         self.draw_rectangle (dtm.apply ((-40, -40)), dtm.apply ((40, 40)), fill = '')
     # Pucks.
     for m, color in ((-1, GREEN), (1, RED)):
         for x in (400, 650, 900):
             for y in (-125, 75, 275, 475):
                 self.draw_circle ((1500 + m * x, 1050 + y), 35, outline = color)
     # Children.
     Drawable.draw (self)