From 104bee8c4abe624303fe8e1b8d58fdb66c450b3d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 13 Apr 2012 21:44:54 +0200 Subject: host/simu/model: add exclude callback for table search --- host/simu/model/distance_sensor.py | 6 ++++-- host/simu/model/distance_sensor_sensopart.py | 6 +++--- host/simu/model/distance_sensor_sharps.py | 4 ++-- host/simu/model/table.py | 10 ++++++---- 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/host/simu/model/distance_sensor.py b/host/simu/model/distance_sensor.py index bdc8531d..e6c2c8b7 100644 --- a/host/simu/model/distance_sensor.py +++ b/host/simu/model/distance_sensor.py @@ -27,7 +27,8 @@ from simu.utils.trans_matrix import TransMatrix class DistanceSensor: - def __init__ (self, table, pos, angle, range, into = None, level = 0): + def __init__ (self, table, pos, angle, range, into = None, level = 0, + exclude = None): self.table = table self.pos = pos self.angle = angle @@ -36,6 +37,7 @@ class DistanceSensor: pos[1] + sin (angle) * range) self.into = into or () self.level = level + self.exclude = exclude self.distance = None def evaluate (self): @@ -51,7 +53,7 @@ class DistanceSensor: pos, target = m.apply (pos, target) # Find intersection. i = self.table.intersect (pos, target, level = self.level, - comp = lambda a, b: a < b) + comp = lambda a, b: a < b, exclude = self.exclude) if i is not None: self.distance = i.distance else: diff --git a/host/simu/model/distance_sensor_sensopart.py b/host/simu/model/distance_sensor_sensopart.py index c97cf54e..bb8e17c7 100644 --- a/host/simu/model/distance_sensor_sensopart.py +++ b/host/simu/model/distance_sensor_sensopart.py @@ -46,16 +46,16 @@ class DistanceSensorSensopart (Observable): OMAX = 250 * 0.020 def __init__ (self, link, scheduler, table, pos, angle, into = None, - level = 0): + level = 0, exclude = None): Observable.__init__ (self) self.rays = [ ] range = self.RANGE * self.QUALITY self.rays.append (DistanceSensorSensopartRay (table, pos, angle, - range, into, level)) + range, into, level, exclude)) for s in self.SECONDARY: for i in (-1, 1): self.rays.append (DistanceSensorSensopartRay (table, pos, - angle + s[0] * i, range * s[1], into, level)) + angle + s[0] * i, range * s[1], into, level, exclude)) self.link = link self.scheduler = scheduler self.value = None diff --git a/host/simu/model/distance_sensor_sharps.py b/host/simu/model/distance_sensor_sharps.py index 88bee49f..c9b524cf 100644 --- a/host/simu/model/distance_sensor_sharps.py +++ b/host/simu/model/distance_sensor_sharps.py @@ -32,10 +32,10 @@ class DistanceSensorSharps (Observable, DistanceSensor): B = 177.26 def __init__ (self, link, scheduler, table, pos, angle, into = None, - level = 0): + level = 0, exclude = exclude): Observable.__init__ (self) DistanceSensor.__init__ (self, table, pos, angle, self.RANGE, into, - level) + level, exclude) self.link = link self.scheduler = scheduler self.value = None diff --git a/host/simu/model/table.py b/host/simu/model/table.py index d79f7758..23b1fcb4 100644 --- a/host/simu/model/table.py +++ b/host/simu/model/table.py @@ -36,10 +36,11 @@ class Table (Observable): Observable.__init__ (self) self.obstacles = [ ] - def intersect (self, a, b, level = None, comp = None): + def intersect (self, a, b, level = None, comp = None, exclude = None): i = None for o in self.obstacles: - if level is None or level == o.level: + if ((level is None or level == o.level) + and (exclude is None or not exclude (o))): d = o.intersect (a, b) if d is not None and (i is None or comp (d, i.distance)): i = Intersect (o, d) @@ -47,12 +48,13 @@ class Table (Observable): return i return i - def nearest (self, pos, level = None, max = None): + def nearest (self, pos, level = None, max = None, exclude = None): """Return nearest object.""" no = None nds = None for o in self.obstacles: - if o.pos is not None and (level is None or level == o.level): + if (o.pos is not None and (level is None or level == o.level) + and (exclude is None or not exclude (o))): ds = (pos[0] - o.pos[0]) ** 2 + (pos[1] - o.pos[1]) ** 2 if (max is None or ds < max ** 2) and (nds is None or ds < nds): no = o -- cgit v1.2.3