def nearestSegmentTo(self, point): dist = self.lineString.project(shapely.geometry.Point(point)) # TODO optimize? for segment, cumLen in zip(self.segments, self.cumulativeLengths): if dist <= cumLen: break # FYI, could also get here if loop runs to completion due to rounding error return (Vector(*segment[0]), Vector(*segment[1]))
def getProperties(self, obj, properties): vals = dict(position=obj.position, heading=obj.heading, velocity=Vector(0, 0), speed=0, angularSpeed=0) for prop in properties: if prop not in vals: vals[prop] = None return vals
def uniformPointInner(self): x, y = self.center heading, angle, maxDist = self.heading, self.angle, self.radius r = random.triangular(0, maxDist, maxDist) ha = angle / 2.0 t = random.uniform(-ha, ha) + (heading + (math.pi / 2)) return Vector(x + (r * cos(t)), y + (r * sin(t)))
def __getitem__(self, i): """Get the ith point along this polyline. If the region consists of multiple polylines, this order is linear along each polyline but arbitrary across different polylines. """ return Vector(*self.points[i])
def test_visible_from_point(): scenario = compileScenic( 'x = Point at 300@200, with visibleDistance 2\n' 'ego = Object visible from x' ) for i in range(30): scene = sampleScene(scenario, maxIterations=1) assert scene.egoObject.position.distanceTo(Vector(300, 200)) <= 2
def sampler(intRegion): o = intRegion.regions[1] center, radius = o.circumcircle possibles = (Vector(*self.kdTree.data[i]) for i in self.kdTree.query_ball_point(center, radius)) intersection = [p for p in possibles if o.containsPoint(p)] if len(intersection) == 0: raise RejectionException() return self.orient(random.choice(intersection))
class Point(Constructible): """Implementation of the Scenic class ``Point``. The default mutator for `Point` adds Gaussian noise to ``position`` with a standard deviation given by the ``positionStdDev`` property. Attributes: position (`Vector`): Position of the point. Default value is the origin. visibleDistance (float): Distance for ``can see`` operator. Default value 50. width (float): Default value zero (only provided for compatibility with operators that expect an `Object`). height (float): Default value zero. """ position: Vector(0, 0) width: 0 height: 0 visibleDistance: 50 mutationEnabled: False mutator: PropertyDefault({'positionStdDev'}, {'additive'}, lambda self: PositionMutator(self.positionStdDev)) positionStdDev: 1 def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.position = toVector(self.position, f'"position" of {self} not a vector') self.corners = (self.position, ) self.visibleRegion = CircularRegion(self.position, self.visibleDistance) def toVector(self): return self.position.toVector() def canSee(self, other): # TODO improve approximation? for corner in other.corners: if self.visibleRegion.containsPoint(corner): return True return False def sampleGiven(self, value): sample = super().sampleGiven(value) if self.mutationEnabled: for mutator in self.mutator: if mutator is None: continue sample, proceed = mutator.appliedTo(sample) if not proceed: break return sample # Points automatically convert to Vectors when needed def __getattr__(self, attr): if hasattr(Vector, attr): return getattr(self.toVector(), attr) else: raise AttributeError( f"'{type(self).__name__}' object has no attribute '{attr}'")
def coerceToVector(thing) -> Vector: if isinstance(thing, (tuple, list)): l = len(thing) if l != 2: raise CoercionFailure('expected 2D vector, got ' f'{type(thing).__name__} of length {l}') return Vector(*thing) else: return thing.toVector()
def uniformPointInner(self): pointA, pointB = random.choices(self.segments, cum_weights=self.cumulativeLengths)[0] interpolation = random.random() x, y = averageVectors(pointA, pointB, weight=interpolation) if self.orientation is True: return OrientedVector(x, y, headingOfSegment(pointA, pointB)) else: return self.orient(Vector(x, y))
def uniformPointInner(self): triangle, bounds = random.choices( self.trianglesAndBounds, cum_weights=self.cumulativeTriangleAreas)[0] minx, miny, maxx, maxy = bounds # TODO improve? while True: x, y = random.uniform(minx, maxx), random.uniform(miny, maxy) if triangle.intersects(shapely.geometry.Point(x, y)): return self.orient(Vector(x, y))
def __init__(self, position, heading, width, height): super().__init__('Rectangle', position, heading, width, height) self.position = position.toVector() self.heading = heading self.width = width self.height = height self.hw = hw = width / 2 self.hh = hh = height / 2 self.radius = hypot(hw, hh) # circumcircle; for collision detection self.corners = tuple(position.offsetRotated(heading, Vector(*offset)) for offset in ((hw, hh), (-hw, hh), (-hw, -hh), (hw, -hh))) self.circumcircle = (self.position, self.radius)
def test_visible_from_oriented_point(): scenario = compileScenic( 'op = OrientedPoint at 100 @ 200, facing 45 deg,\n' ' with visibleDistance 5, with viewAngle 90 deg\n' 'ego = Object visible from op') base = Vector(100, 200) for i in range(30): scene, iterations = scenario.generate(maxIterations=1) pos = scene.egoObject.position assert pos.distanceTo(base) <= 5 assert pos.x <= base.x assert pos.y >= base.y
def Beyond(pos, offset, fromPt=None): pos = toVector(pos, 'specifier "beyond X by Y" with X not a vector') dType = underlyingType(offset) if dType is float or dType is int: offset = Vector(0, offset) elif dType is not Vector: raise RuntimeParseError('specifier "beyond X by Y" with Y not a number or vector') if fromPt is None: fromPt = ego() fromPt = toVector(fromPt, 'specifier "beyond X by Y from Z" with Z not a vector') lineOfSight = fromPt.angleTo(pos) return Specifier('position', pos.offsetRotated(lineOfSight, offset))
def Behind(pos, dist=0): """The 'behind X [by Y]' polymorphic specifier. Specifies 'position', depending on 'length'. See other dependencies below. Allowed forms: behind <oriented point> [by <scalar/vector>] -- optionally specifies 'heading'; behind <vector> [by <scalar/vector>] -- depends on 'heading'. If the 'by <scalar/vector>' is omitted, zero is used. """ return leftSpecHelper('behind', pos, dist, 'length', lambda dist: (0, dist), lambda self, dx, dy: Vector(dx, -self.length / 2 - dy))
def __init__(self, position, heading, width, length, name=None): super().__init__(name, position, heading, width, length) self.position = position.toVector() self.heading = heading self.width = width self.length = length self.hw = hw = width / 2 self.hl = hl = length / 2 self.radius = hypot(hw, hl) # circumcircle; for collision detection self.corners = tuple( position.offsetRotated(heading, Vector(*offset)) for offset in ((hw, hl), (-hw, hl), (-hw, -hl), (hw, -hl))) self.circumcircle = (self.position, self.radius)
def RightSpec(pos, dist=0): """The 'right of X [by Y]' polymorphic specifier. Specifies 'position', depending on 'width'. See other dependencies below. Allowed forms: right of <oriented point> [by <scalar/vector>] -- optionally specifies 'heading'; right of <vector> [by <scalar/vector>] -- depends on 'heading'. If the 'by <scalar/vector>' is omitted, zero is used. """ return leftSpecHelper('right of', pos, dist, 'width', lambda dist: (dist, 0), lambda self, dx, dy: Vector(self.width / 2 + dx, dy))
def Ahead(pos, dist=0): """The 'ahead of X [by Y]' polymorphic specifier. Specifies 'position', depending on 'length'. See other dependencies below. Allowed forms: * ``ahead of`` <oriented point> [``by`` <scalar/vector>] -- optionally specifies 'heading'; * ``ahead of`` <vector> [``by`` <scalar/vector>] -- depends on 'heading'. If the 'by <scalar/vector>' is omitted, zero is used. """ return leftSpecHelper('ahead of', pos, dist, 'length', lambda dist: (0, dist), lambda self, dx, dy: Vector(dx, self.length / 2 + dy))
def readPropertiesFromWebots(self): for obj in self.objects: webotsObj = self.webotsObjects[obj] # webotsObject obj.webotsObject = webotsObj # position pos = webotsObj.getField('translation').getSFVec3f() x, y = utils.webotsToScenicPosition(pos) obj.position = Vector(x, y) # heading rot = webotsObj.getField('rotation').getSFRotation() heading = utils.webotsToScenicRotation(rot, tolerance2D=100) if heading is None: raise RuntimeError(f'{webotsObj} has non-planar orientation!') obj.heading = heading
class Point(Constructible): position: Vector(0, 0) width: 0 height: 0 visibleDistance: 50 mutationEnabled: False mutator: PropertyDefault({'positionStdDev'}, {'additive'}, lambda self: PositionMutator(self.positionStdDev)) positionStdDev: 1 def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.position = toVector(self.position, f'"position" of {self} not a vector') self.corners = (self.position, ) self.visibleRegion = CircularRegion(self.position, self.visibleDistance) def toVector(self): return self.position.toVector() def canSee(self, other): # TODO improve approximation? for corner in other.corners: if self.visibleRegion.containsPoint(corner): return True return False def sampleGiven(self, value): sample = super().sampleGiven(value) if self.mutationEnabled: for mutator in self.mutator: if mutator is None: continue sample, proceed = mutator.appliedTo(sample) if not proceed: break return sample # Points automatically convert to Vectors when needed def __getattr__(self, attr): if hasattr(Vector, attr): return getattr(self.toVector(), attr) else: raise AttributeError( f"'{type(self).__name__}' object has no attribute '{attr}'")
def segmentsOf(cls, lineString): if isinstance(lineString, shapely.geometry.LineString): segments = [] points = list(lineString.coords) if len(points) < 2: raise RuntimeError('LineString has fewer than 2 points') last = Vector(*points[0][:2]) for point in points[1:]: point = point[:2] segments.append((last, point)) last = point return segments elif isinstance(lineString, shapely.geometry.MultiLineString): allSegments = [] for line in lineString: allSegments.extend(cls.segmentsOf(line)) return allSegments else: raise RuntimeError('called segmentsOf on non-linestring')
def Beyond(pos, offset, fromPt=None): """The 'beyond X by Y [from Z]' polymorphic specifier. Specifies 'position', with no dependencies. Allowed forms: beyond <vector> by <number> [from <vector>] beyond <vector> by <vector> [from <vector>] If the 'from <vector>' is omitted, the position of ego is used. """ pos = toVector(pos, 'specifier "beyond X by Y" with X not a vector') dType = underlyingType(offset) if dType is float or dType is int: offset = Vector(0, offset) elif dType is not Vector: raise RuntimeParseError('specifier "beyond X by Y" with Y not a number or vector') if fromPt is None: fromPt = ego() fromPt = toVector(fromPt, 'specifier "beyond X by Y from Z" with Z not a vector') lineOfSight = fromPt.angleTo(pos) return Specifier('position', pos.offsetRotated(lineOfSight, offset))
def uniformPointInner(self): hw, hh = self.hw, self.hh rx = random.uniform(-hw, hw) ry = random.uniform(-hh, hh) pt = self.position.offsetRotated(self.heading, Vector(rx, ry)) return self.orient(pt)
def Behind(pos, dist=0): return leftSpecHelper('behind', pos, dist, 'height', lambda dist: (0, dist), lambda self, dx, dy: Vector(dx, -self.height / 2 - dy))
def Ahead(pos, dist=0): return leftSpecHelper('ahead of', pos, dist, 'height', lambda dist: (0, dist), lambda self, dx, dy: Vector(dx, self.height / 2 + dy))
def RightSpec(pos, dist=0): return leftSpecHelper('right of', pos, dist, 'width', lambda dist: (dist, 0), lambda self, dx, dy: Vector(self.width / 2 + dx, dy))
def LeftSpec(pos, dist=0): return leftSpecHelper('left of', pos, dist, 'width', lambda dist: (dist, 0), lambda self, dx, dy: Vector(-self.width / 2 - dx, dy))
def applyTo(self, obj, sim): vel = Vector(0, self.speed).rotatedBy(obj.heading) obj.setVelocity(vel)
def uniformPointInner(self): return self.orient(Vector(*random.choice(self.points)))
class Object(OrientedPoint, RotatedRectangle): width: 1 height: 1 allowCollisions: False requireVisible: True regionContainedIn: None cameraOffset: Vector(0, 0) def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) import scenic.syntax.veneer as veneer # TODO improve? veneer.registerObject(self) self.hw = hw = self.width / 2 self.hh = hh = self.height / 2 self.radius = hypot(hw, hh) # circumcircle; for collision detection self.inradius = min(hw, hh) # incircle; for collision detection self.left = self.relativePosition(-hw, 0) self.right = self.relativePosition(hw, 0) self.front = self.relativePosition(0, hh) self.back = self.relativePosition(0, -hh) self.frontLeft = self.relativePosition(-hw, hh) self.frontRight = self.relativePosition(hw, hh) self.backLeft = self.relativePosition(-hw, -hh) self.backRight = self.relativePosition(hw, -hh) self.corners = (self.frontRight.toVector(), self.frontLeft.toVector(), self.backLeft.toVector(), self.backRight.toVector()) camera = self.position.offsetRotated(self.heading, self.cameraOffset) self.visibleRegion = SectorRegion(camera, self.visibleDistance, self.heading, self.viewAngle) self._relations = [] def show(self, workspace, plt, highlight=False): if needsSampling(self): raise RuntimeError('tried to show() symbolic Object') pos = self.position mpos = workspace.langToMapCoords(pos) if highlight: # Circle around object rad = 1.5 * max(self.width, self.height) c = plt.Circle(mpos, rad, color='g', fill=False) plt.gca().add_artist(c) # View cone ha = self.viewAngle / 2.0 camera = self.position.offsetRotated(self.heading, self.cameraOffset) cpos = workspace.langToMapCoords(camera) for angle in (-ha, ha): p = camera.offsetRadially(20, self.heading + angle) edge = [cpos, workspace.langToMapCoords(p)] x, y = zip(*edge) plt.plot(x, y, 'b:') corners = [ workspace.langToMapCoords(corner) for corner in self.corners ] x, y = zip(*corners) color = self.color if hasattr(self, 'color') else (1, 0, 0) plt.fill(x, y, color=color) #plt.plot(x + (x[0],), y + (y[0],), color="g", linewidth=1) frontMid = averageVectors(corners[0], corners[1]) baseTriangle = [frontMid, corners[2], corners[3]] triangle = [averageVectors(p, mpos, weight=0.5) for p in baseTriangle] x, y = zip(*triangle) plt.fill(x, y, "w") plt.plot(x + (x[0], ), y + (y[0], ), color="k", linewidth=1)
def uniformPointInner(self): x, y = self.center r = random.triangular(0, self.radius, self.radius) t = random.uniform(-math.pi, math.pi) pt = Vector(x + (r * cos(t)), y + (r * sin(t))) return self.orient(pt)