def click (self, ev):
def move (o, pos):
if callable (o):
o (pos)
else:
o.pos = pos
pos = vector (self.area_view.screen_coord ((ev.x, ev.y)))
if self.move is None:
def move_a (pos):
self.area_view.area.a = pos
def move_b (pos):
self.area_view.area.b = pos
objs = [ [ self.area_view.area.a, 0.2, move_a ],
[ self.area_view.area.b, 0.2, move_b ] ]
for o in self.area_view.area.table.obstacles:
objs.append ([ o.pos, getattr (o, 'radius', 0.2), o ])
for obj in objs:
opos = vector (obj[0])
radius = obj[1]
if abs (opos - pos) < radius:
self.move = obj[2]
break
if self.move is not None:
move (self.move, None)
else:
move (self.move, pos)
self.move = None
self.update ()
def rotate (self, ev):
pos = vector (self.area_view.screen_coord ((ev.x, ev.y)))
for o in self.area_view.area.table.obstacles:
if o.pos is None or not hasattr (o, 'angle'):
continue
opos = vector (o.pos)
if abs (opos - pos) < 0.2:
o.angle += pi / 4
self.update ()
def inside (self, a):
"""If A is inside obstacle, return True."""
# Map point in obstacle coordinates.
u = vector.polar (self.angle, 1)
o = vector (self.pos)
a = vector (a)
oa = a - o
x = oa * u / (.5 * self.dim[0])
y = oa * u.normal () / (.5 * self.dim[1])
return x > -1 and x < 1 and y > -1 and y < 1
def update (self, test, **kwargs):
self.result = None
if self.a is not None and self.b is not None:
if test == 'intersect':
def nearer (a, b): return a < b
i = self.table.intersect (self.a, self.b, comp = nearer,
**kwargs)
if i is not None:
a, b = vector (self.a), vector (self.b)
self.result = a + (b - a).unit () * i.distance
elif test == 'nearest':
n = self.table.nearest (self.b, **kwargs)
if n is not None:
self.result = n.pos
def segment_segment (a, b, c, d):
"""Find intersection between [AB] and [CD] line segments. Return i such
that A + i * (B - A).unit () gives this intersection. Return None if no
intersection found.
If line segments are parallel or collinear, None is returned.
"""
a, b, c, d = vector (a), vector (b), vector (c), vector (d)
# For each point P on the line segment [AB], there is a real u in [0, 1]
# for which P = A + u (B - A)
#
# An intersection point must be on both line segments:
#
# A + u (B - A) = C + v (D - C)
#
# xa + u (xb - xa) = xc + v (xd - xc)
# ya + u (yb - ya) = yc + v (yd - yc)
#
# (xc - xa) (yd - yc) - (xd - xc) (yc - ya)
# u = -----------------------------------------
# (xb - xa) (yd - yc) - (xd - xc) (yb - ya)
# (xc - xa) (yb - ya) - (xb - xa) (yc - ya)
# v = -----------------------------------------
# (xb - xa) (yd - yc) - (xd - xc) (yb - ya)
#
# u = (vac.normal() . vcd) / (vab.normal() . vcd)
# v = (vac.normal() . vab) / (vab.normal() . vcd)
#
# If vab.normal() . vcd is 0, AB and CD are parallel.
vab = b - a
vcd = d - c
vac = c - a
# Cannot test for 0 because we are using float, cannot test for a very
# small number because we do not know what is small enough, therefore,
# compare with numerator.
den = vab.normal () * vcd
unum = vac.normal () * vcd
if abs (den) <= 1e-6 * abs (unum):
return None
else:
u = unum / den
if u >= 0 and u <= 1:
v = vac.normal () * vab / den
if v >= 0 and v <= 1:
return u * vab.norm ()
return None
def __arm_notified (self):
if self.arm_cyl.pos > .9:
push_point = (vector (self.robot_position.pos)
+ vector.polar (self.robot_position.angle - pi / 2, side + 100)
- vector.polar (self.robot_position.angle, 100))
gift = self.table.nearest (push_point, level = 0, max = 150)
if gift is not None and hasattr (gift, 'state'):
gift.state = True
gift.notify ()
self.notify ()
def __door_notified (self):
self.door = self.door_cylinder.pos # 1. is open.
if self.door > 0.5 and self.load:
for e in self.load:
e.pos = (vector (self.robot_position.pos)
- vector.polar (self.robot_position.angle
+ random.uniform (-pi/8, pi/8),
200 + random.uniform (0, 70)))
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 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:
return None
a, b = vector(a), vector(b)
vab = b - a
ab = abs(vab) # distance AB.
n = vab.unit() # vector of length 1.
o = vector(self.pos) # obstacle center.
# To check if the line (AB) intersects the circle, compute distance
# from circle center to line using a dot product.
vao = o - a # vector AO.
# abs of dot product.
doc = abs(vao * n.normal())
if doc < self.radius:
# Line intersects, check if segment intersects.
m = vao * n
f = sqrt(self.radius ** 2 - doc ** 2)
if m - f > 0 and m - f < ab:
return m - f
elif m + f > 0 and m + f < ab:
return m + f
return None
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 rectangle segments. There is at most
# two intersections, return the nearest.
u = vector.polar (self.angle, self.dim[0] / 2.)
v = vector.polar (self.angle + pi / 2, self.dim[1] / 2.)
o = vector (self.pos)
p1 = o + u + v
p2 = o - u + v
p3 = o - u - v
p4 = o + u - v
found = None
for c, d in ((p1, p2), (p2, p3), (p3, p4), (p4, p1)):
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 __plate_drop_point (self):
return (vector (self.robot_position.pos)
+ vector (-back - 85, -35).rotate (self.robot_position.angle))
den = vab.normal () * vcd
unum = vac.normal () * vcd
if abs (den) <= 1e-6 * abs (unum):
return None
else:
u = unum / den
if u >= 0 and u <= 1:
v = vac.normal () * vab / den
if v >= 0 and v <= 1:
return u * vab.norm ()
return None
if __name__ == '__main__':
import sys
import math
v00 = vector (0, 0)
v02 = vector (0, 2)
v20 = vector (2, 0)
v22 = vector (2, 2)
v44 = vector (4, 4)
failed = 0
verbose = True
def check (test, result):
r = eval (test)
if r != result:
print test, 'is', r, 'but expected', result
return 1
elif verbose:
print test, 'is', r, 'as expected'
return 0
v2 = math.sqrt (2)