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)