summaryrefslogtreecommitdiff
path: root/host/simu/model
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu/model')
-rw-r--r--host/simu/model/distance_sensor_sensopart.py2
-rw-r--r--host/simu/model/rectangular_obstacle.py63
-rw-r--r--host/simu/model/round_obstacle.py17
-rw-r--r--host/simu/model/switch.py8
-rw-r--r--host/simu/model/table.py4
-rw-r--r--host/simu/model/table_eurobot2011.py13
-rw-r--r--host/simu/model/table_eurobot2012.py93
-rw-r--r--host/simu/model/test/test_table.py212
8 files changed, 399 insertions, 13 deletions
diff --git a/host/simu/model/distance_sensor_sensopart.py b/host/simu/model/distance_sensor_sensopart.py
index 16676e5d..26ed604f 100644
--- a/host/simu/model/distance_sensor_sensopart.py
+++ b/host/simu/model/distance_sensor_sensopart.py
@@ -59,8 +59,8 @@ class DistanceSensorSensopart (Observable):
self.link = link
self.scheduler = scheduler
self.value = None
- self.register (self.__update)
self.evaluate ()
+ self.register (self.__update)
def evaluate (self):
# Compute real distance.
diff --git a/host/simu/model/rectangular_obstacle.py b/host/simu/model/rectangular_obstacle.py
new file mode 100644
index 00000000..71cfdc8b
--- /dev/null
+++ b/host/simu/model/rectangular_obstacle.py
@@ -0,0 +1,63 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Obstacle with a rectangular shape."""
+from utils.observable import Observable
+from simu.utils.vector import vector
+from math import pi
+import simu.utils.intersect
+
+class RectangularObstacle (Observable):
+
+ def __init__ (self, dim, level = 0):
+ Observable.__init__ (self)
+ self.pos = None
+ self.angle = 0
+ self.dim = dim
+ self.level = level
+
+ 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
+
diff --git a/host/simu/model/round_obstacle.py b/host/simu/model/round_obstacle.py
index 6658ed19..e749440b 100644
--- a/host/simu/model/round_obstacle.py
+++ b/host/simu/model/round_obstacle.py
@@ -24,6 +24,7 @@
"""Obstacle with a round shape."""
from math import pi, cos, sin, sqrt
from utils.observable import Observable
+from simu.utils.vector import vector
class RoundObstacle (Observable):
@@ -38,17 +39,19 @@ class RoundObstacle (Observable):
to intersection point, else, return None."""
if self.pos is None:
return None
- ab = sqrt ((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2) # distance AB.
- n = ((b[0] - a[0]) / ab, (b[1] - a[1]) / ab) # vector of length 1.
- o = self.pos # obstacle center.
+ 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[0] - a[0], o[1] - a[1]) # vector AO.
- # dot product, (-n[1], n[0]) is perpendicular to n.
- doc = abs (vao[0] * -n[1] + vao[1] * n[0])
+ 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[0] * n[0] + vao[1] * n[1]
+ m = vao * n
f = sqrt (self.radius ** 2 - doc ** 2)
if m - f > 0 and m - f < ab:
return m - f
diff --git a/host/simu/model/switch.py b/host/simu/model/switch.py
index 30cc1466..6a06a954 100644
--- a/host/simu/model/switch.py
+++ b/host/simu/model/switch.py
@@ -26,13 +26,17 @@ from utils.observable import Observable
class Switch (Observable):
- def __init__ (self, link):
+ def __init__ (self, link, invert = False):
Observable.__init__ (self)
self.link = link
self.state = None
+ self.invert = invert
self.register (self.__update)
def __update (self):
- self.link.state = self.state
+ if not self.invert:
+ self.link.state = self.state
+ else:
+ self.link.state = not self.state
self.link.notify ()
diff --git a/host/simu/model/table.py b/host/simu/model/table.py
index 41767ac4..d79f7758 100644
--- a/host/simu/model/table.py
+++ b/host/simu/model/table.py
@@ -22,6 +22,7 @@
#
# }}}
"""Table model."""
+from utils.observable import Observable
class Intersect:
@@ -29,9 +30,10 @@ class Intersect:
self.obstacle = obstacle
self.distance = distance
-class Table:
+class Table (Observable):
def __init__ (self):
+ Observable.__init__ (self)
self.obstacles = [ ]
def intersect (self, a, b, level = None, comp = None):
diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py
index c9c1d193..f200028e 100644
--- a/host/simu/model/table_eurobot2011.py
+++ b/host/simu/model/table_eurobot2011.py
@@ -31,8 +31,11 @@ class Table (simu.model.table.Table):
def __init__ (self, cards = None):
simu.model.table.Table.__init__ (self)
# Draw cards.
- if cards is None:
- cards = [ randrange (20) for i in xrange (3) ]
+ cards = [ ]
+ while len (cards) != 3:
+ card = randrange (20)
+ if card not in cards:
+ cards.append (card)
self.cards = cards
def pos (card):
king_pos = card // 4
@@ -81,3 +84,9 @@ class Table (simu.model.table.Table):
self.pawns.append (pawn)
# Add everything to obstacles.
self.obstacles += self.pawns
+
+ def add_pawn (self, pawn):
+ self.pawns.append (pawn)
+ self.obstacles.append (pawn)
+ self.notify ()
+
diff --git a/host/simu/model/table_eurobot2012.py b/host/simu/model/table_eurobot2012.py
new file mode 100644
index 00000000..38960513
--- /dev/null
+++ b/host/simu/model/table_eurobot2012.py
@@ -0,0 +1,93 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Table model for Eurobot 2012."""
+import simu.model.table
+from simu.model.round_obstacle import RoundObstacle
+from simu.model.rectangular_obstacle import RectangularObstacle
+from math import pi
+import math
+import random
+
+class Table (simu.model.table.Table):
+
+ def __init__ (self, cards = None):
+ simu.model.table.Table.__init__ (self)
+ # Well, this is a boring write only code which create every elements.
+ # Add coins.
+ self.coins = [ ]
+ def add_coin (pos, angle, level = 1):
+ coin = RoundObstacle (60, level)
+ coin.pos = pos
+ coin.angle = angle
+ coin.value = 1
+ self.coins.append (coin)
+ def add_coin_circle (center, radius, start, step, n, level = 1):
+ angle = start
+ for i in xrange (n):
+ pos = (center[0] + radius * math.cos (angle),
+ center[1] + radius * math.sin (angle))
+ add_coin (pos, angle, level)
+ angle += step
+ add_coin ((1000, 1500), 0)
+ add_coin ((2000, 1500), pi)
+ add_coin ((450, 300), pi)
+ add_coin ((3000 - 450, 300), 0)
+ add_coin_circle ((1500, 300), 90, 0, pi / 2, 4)
+ add_coin_circle ((1500 - 400, 1000), 300 - 60, pi / 4, pi / 4, 7)
+ add_coin_circle ((1500 + 400, 1000), 300 - 60, pi + pi / 4, pi / 4, 7)
+ add_coin_circle ((1500 - 400, 1000), 115, pi / 4, pi / 2, 4)
+ add_coin_circle ((1500 + 400, 1000), 115, pi / 4, pi / 2, 4)
+ add_coin_circle ((1500 - 400, 1000), 105, pi / 4, pi / 2, 4, 3)
+ add_coin_circle ((1500 + 400, 1000), 105, pi / 4, pi / 2, 4, 3)
+ # Add gold bars.
+ self.gold_bars = [ ]
+ def add_gold_bar (pos, angle, level = 1):
+ gold_bar = RectangularObstacle ((150, 70), level)
+ gold_bar.pos = pos
+ gold_bar.angle = angle
+ gold_bar.value = 3
+ self.gold_bars.append (gold_bar)
+ add_gold_bar ((1500, 647), 0)
+ add_gold_bar ((1500 - 400, 1000 + 125 - 35), 0, 2)
+ add_gold_bar ((1500 + 400, 1000 + 125 - 35), 0, 2)
+ add_gold_bar ((1500 - 400, 1000 - 125 + 35), 0, 2)
+ add_gold_bar ((1500 + 400, 1000 - 125 + 35), 0, 2)
+ ba = pi / 2 - 0.04995839
+ cba = math.cos (ba)
+ sba = math.sin (ba)
+ gbx = 400 - (285 + 75) * cba + 35
+ gby = 1500 - (285 + 75) * sba
+ add_gold_bar ((gbx, gby), ba)
+ add_gold_bar ((3000 - gbx, gby), pi - ba)
+ # Set random black coins.
+ nblack = 0
+ while nblack < 4:
+ coin = random.choice (self.coins[2:])
+ if coin.value:
+ coin.value = 0
+ nblack += 1
+ # Add everything to obstacles.
+ self.obstacles += self.coins
+ self.obstacles += self.gold_bars
+
diff --git a/host/simu/model/test/test_table.py b/host/simu/model/test/test_table.py
new file mode 100644
index 00000000..9595f256
--- /dev/null
+++ b/host/simu/model/test/test_table.py
@@ -0,0 +1,212 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Test table model and obstacles."""
+from simu.model.table import Table
+from simu.model.round_obstacle import RoundObstacle
+from simu.model.rectangular_obstacle import RectangularObstacle
+from simu.inter.drawable import Drawable, DrawableCanvas
+from simu.utils.vector import vector
+from Tkinter import *
+from math import pi
+
+class Area (Drawable):
+
+ def __init__ (self, onto):
+ Drawable.__init__ (self, onto)
+ self.table = Table ()
+ self.a = (0, 0)
+ self.b = (10, 10)
+ self.result = None
+
+ def draw (self):
+ self.reset ()
+ for o in self.table.obstacles:
+ if o.pos is None:
+ continue
+ if isinstance (o, RoundObstacle):
+ self.draw_circle (o.pos, o.radius)
+ elif isinstance (o, RectangularObstacle):
+ self.trans_push ()
+ self.trans_translate (o.pos)
+ self.trans_rotate (o.angle)
+ self.draw_circle ((0, 0), 0.2)
+ self.draw_rectangle ((o.dim[0] / 2, o.dim[1] / 2),
+ (-o.dim[0] / 2, -o.dim[1] / 2), fill = '')
+ self.trans_pop ()
+ else:
+ raise TypeError ("unknown obstacle")
+ if self.a is not None:
+ self.draw_circle (self.a, 0.2, fill = 'green')
+ if self.b is not None:
+ self.draw_circle (self.b, 0.2, fill = 'red')
+ if self.a is not None and self.b is not None:
+ self.draw_line (self.a, self.b, arrow = 'last')
+ if self.result is not None:
+ self.draw_circle (self.result, 0.2, fill = 'yellow')
+
+ 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 = (b - a).unit () * i.distance
+ elif test == 'nearest':
+ n = self.table.nearest (self.b, **kwargs)
+ if n is not None:
+ self.result = n.pos
+
+class AreaView (DrawableCanvas):
+
+ def __init__ (self, master = None):
+ DrawableCanvas.__init__ (self, 22, 22, 0, 0, master, borderwidth = 1,
+ relief = 'sunken', background = 'white')
+ self.area = Area (self)
+
+ def draw (self):
+ self.area.draw ()
+
+class TestTable (Frame):
+
+ def __init__ (self, master = None):
+ Frame.__init__ (self, master)
+ self.pack (expand = True, fill = 'both')
+ self.create_widgets ()
+ self.move = None
+
+ def create_widgets (self):
+ self.right_frame = Frame (self)
+ self.right_frame.pack (side = 'right', fill = 'y')
+ self.quit_button = Button (self.right_frame, text = 'Quit', command =
+ self.quit)
+ self.quit_button.pack (side = 'top', fill = 'x')
+ self.test_var = StringVar ()
+ self.test_var.set ('intersect')
+ self.test_intersect = Radiobutton (self.right_frame,
+ variable = self.test_var, command = self.update,
+ text = 'Intersect', value = 'intersect')
+ self.test_intersect.pack (side = 'top')
+ self.test_nearest = Radiobutton (self.right_frame,
+ variable = self.test_var, command = self.update,
+ text = 'Nearest', value = 'nearest')
+ self.test_nearest.pack (side = 'top')
+ self.new_obstacle_round_frame = LabelFrame (self.right_frame)
+ self.new_obstacle_round_frame.pack (side = 'top', fill = 'x')
+ self.new_obstacle_radius = Scale (self.new_obstacle_round_frame,
+ label = 'Radius', orient = 'horizontal', from_ = 0.5, to = 10,
+ resolution = 0.5)
+ self.new_obstacle_radius.pack (side = 'top')
+ self.new_obstacle_round = Button (self.new_obstacle_round_frame,
+ text = 'New round obstacle',
+ command = self.new_round_obstacle)
+ self.new_obstacle_round_frame.configure (
+ labelwidget = self.new_obstacle_round)
+ self.new_obstacle_rect_frame = LabelFrame (self.right_frame)
+ self.new_obstacle_rect_frame.pack (side = 'top', fill = 'x')
+ self.new_obstacle_dim0 = Scale (self.new_obstacle_rect_frame,
+ label = 'Dim[0]', orient = 'horizontal', from_ = 1, to = 10,
+ resolution = 1)
+ self.new_obstacle_dim0.pack (side = 'top')
+ self.new_obstacle_dim1 = Scale (self.new_obstacle_rect_frame,
+ label = 'Dim[1]', orient = 'horizontal', from_ = 1, to = 10,
+ resolution = 1)
+ self.new_obstacle_dim1.pack (side = 'top')
+ self.new_obstacle_rect = Button (self.new_obstacle_rect_frame,
+ text = 'New rectangular obstacle',
+ command = self.new_rectangular_obstacle)
+ self.new_obstacle_rect_frame.configure (
+ labelwidget = self.new_obstacle_rect)
+ self.area_view = AreaView (self)
+ self.area_view.pack (expand = True, fill = 'both')
+ self.area_view.bind ('<1>', self.click)
+ self.area_view.bind ('<3>', self.rotate)
+
+ def update (self, draw = True):
+ self.area_view.area.update (self.test_var.get ())
+ if draw:
+ self.area_view.draw ()
+
+ 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 new_round_obstacle (self):
+ o = RoundObstacle (self.new_obstacle_radius.get ())
+ o.pos = (5, -5)
+ self.area_view.area.table.obstacles.append (o)
+ self.update ()
+
+ def new_rectangular_obstacle (self):
+ o = RectangularObstacle ((float (self.new_obstacle_dim0.get ()),
+ float (self.new_obstacle_dim1.get ())))
+ o.pos = (5, -5)
+ o.angle = 0
+ self.area_view.area.table.obstacles.append (o)
+ self.update ()
+
+if __name__ == '__main__':
+ app = TestTable ()
+ o = RoundObstacle (2)
+ o.pos = (5, 5)
+ app.area_view.area.table.obstacles.append (o)
+ app.update (False)
+ app.mainloop ()