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