3d driving

examples/driving/car.scenic

@@ -10,4 +10,4 @@ param map = localPath('../../assets/maps/CARLA/Town01.xodr')
 
 model scenic.domains.driving.model
 
-ego = new Car
+ego = new Car

src/scenic/__main__.py

@@ -279,7 +279,10 @@ def runSimulation(scene, sceneCount):
                     successCount += 1
             else:
                 successCount += 1
-                if mode2D:
+                use2DMap = getattr(
+                    getattr(scene.workspace, "network", None), "use2DMap", False
+                )
+                if use2DMap:
                     if delay is None:
                         scene.show2D(zoom=args.zoom)
                     else:

src/scenic/core/regions.py

@@ -563,6 +563,7 @@ def evaluateInner(self, context):
     def containsPoint(self, point):
         return any(region.containsPoint(point) for region in self.footprint.regions)
 
+    # TODO: Need to implement this
     def containsObject(self, obj):
         raise NotImplementedError
 
@@ -2041,6 +2042,33 @@ def intersects(self, other, triedReversed=False):
 
         return super().intersects(other, triedReversed)
 
+    def union(self, other, triedReversed=False):
+        """Get a `Region` representing the union of this region with another.
+
+        This function handles union computation for `MeshSurfaceRegion` with:
+            - `MeshSurfaceRegion`
+        """
+        # If one of the regions isn't fixed, fall back on default behavior
+        if isLazy(self) or isLazy(other):
+            return super().union(other, triedReversed)
+
+        # If other region is represented by a mesh, we can extract the mesh to
+        # perform boolean operations on it
+        if isinstance(other, MeshSurfaceRegion):
+            other_mesh = other.mesh
+
+            # Compute union using Trimesh
+            new_mesh = trimesh.util.concatenate(self.mesh, other_mesh)
+
+            return MeshSurfaceRegion(
+                new_mesh,
+                tolerance=min(self.tolerance, other.tolerance),
+                centerMesh=False,
+            )
+
+        # Don't know how to compute this union, fall back to default behavior.
+        return super().union(other, triedReversed)
+
     @distributionFunction
     def containsPoint(self, point):
         """Check if this region's surface contains a point."""
@@ -2052,18 +2080,18 @@ def containsPoint(self, point):
 
     def containsObject(self, obj):
         # A surface cannot contain an object, which must have a volume.
-        return False
+        # Use footprint
+        return self.boundingPolygon.containsObject(obj)
 
     def containsRegionInner(self, reg, tolerance):
-        if tolerance != 0:
-            warnings.warn(
-                "Nonzero tolerances are ignored for containsRegionInner on MeshSurfaceRegion"
-            )
-
         if isinstance(reg, MeshSurfaceRegion):
-            diff_region = reg.difference(self)
-
-            return isinstance(diff_region, EmptyRegion)
+            if self.mesh.is_empty:
+                return False
+            elif reg.mesh.is_empty:
+                return True
+            return self.boundingPolygon.polygons.buffer(tolerance).contains(
+                reg.boundingPolygon.polygons
+            )
 
         raise NotImplementedError
 
@@ -2076,9 +2104,7 @@ def distanceTo(self, point):
         point = toVector(point, f"Could not convert {point} to vector.")
 
         pq = trimesh.proximity.ProximityQuery(self.mesh)
-
         dist = abs(pq.signed_distance([point.coordinates])[0])
-
         return dist
 
     @property
@@ -2767,11 +2793,22 @@ def __init__(
             numpy.linalg.norm(b - a, axis=1), (len(a), 1)
         )
 
+    def intersects(self, other, triedReversed=False):
+        if isinstance(other, PathRegion):
+            self_polyline = PolylineRegion(points=[(v.x, v.y) for v in self.vertices])
+            other_polyline = PolylineRegion(points=[(v.x, v.y) for v in other.vertices])
+            poly = toPolygon(other_polyline)
+            if poly is not None:
+                return self_polyline.lineString.intersects(poly)
+            return self_polyline.intersects(other, triedReversed)
+        else:
+            return super().intersects(other, triedReversed)
+
     def containsPoint(self, point):
         return self.distanceTo(point) < self.tolerance
 
     def containsObject(self, obj):
-        return False
+        raise NotImplementedError
 
     def containsRegionInner(self, reg, tolerance):
         raise NotImplementedError
@@ -2820,6 +2857,27 @@ def AABB(self):
             tuple(numpy.amax(self.vertices, axis=0)),
         )
 
+    @distributionMethod
+    def signedDistanceTo(self, point) -> float:
+        """Compute the signed distance of the PathRegion to a point.
+
+        The distance is positive if the point is left of the nearest segment,
+        and negative otherwise.
+        """
+        dist = self.distanceTo(point)
+        start, end = self.nearestSegmentTo(point)
+        rp = point - start
+        tangent = end - start
+        return dist if tangent.angleWith(rp) >= 0 else -dist
+
+    def __getitem__(self, i) -> Vector:
+        """Get the ith point along this path.
+
+        If the region consists of multiple polylines, this order is linear
+        along each polyline but arbitrary across different polylines.
+        """
+        return self.vertices[i]
+
     def uniformPointInner(self):
         # Pick an edge, weighted by length, and extract its two points
         edge = random.choices(population=self.edges, weights=self.edge_lengths, k=1)[0]
@@ -3080,6 +3138,16 @@ def unionAll(regions, buf=0):
         z = 0 if z is None else z
         return PolygonalRegion(polygon=union, orientation=orientation, z=z)
 
+    @cached_property
+    def _boundingPolygon(self):
+        return self._polygon
+
+    @cached_property
+    @distributionFunction
+    def boundingPolygon(self):
+        """A PolygonalRegion returning self"""
+        return self
+
     @property
     @distributionFunction
     def points(self):

src/scenic/core/requirements.py

@@ -15,6 +15,7 @@
 from scenic.core.errors import InvalidScenarioError
 from scenic.core.lazy_eval import needsLazyEvaluation
 from scenic.core.propositions import Atomic, PropositionNode
+from scenic.core.regions import MeshSurfaceRegion, PolygonalRegion
 import scenic.syntax.relations as relations
 
 

src/scenic/domains/driving/__init__.py

@@ -1,6 +1,6 @@
 """Domain for driving scenarios.
 
-This domain must currently be used in `2D compatibility mode`.
+This domain supports both 2D and 3D maps. This can be toggled by setting the `2D Compatibility mode` option.
 
 The :doc:`world model <scenic.domains.driving.model>` defines Scenic classes for cars,
 pedestrians, etc., actions for dynamic agents which walk or drive, as well as simple

src/scenic/domains/driving/behaviors.scenic

@@ -10,7 +10,10 @@ import scenic.domains.driving.model as _model
 from scenic.domains.driving.roads import ManeuverType
 
 def concatenateCenterlines(centerlines=[]):
-    return PolylineRegion.unionAll(centerlines)
+    if isinstance(centerlines[0], PathRegion):
+        return PathRegion(polylines=centerlines)
+    else:
+        return PolylineRegion.unionAll(centerlines)
 
 behavior ConstantThrottleBehavior(x):
     while True:
@@ -131,9 +134,7 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra
             target_speed = original_target_speed
             _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self)
 
-        nearest_line_points = current_centerline.nearestSegmentTo(self.position)
-        nearest_line_segment = PolylineRegion(nearest_line_points)
-        cte = nearest_line_segment.signedDistanceTo(self.position)
+        cte = current_centerline.signedDistanceTo(self.position)
         if is_oppositeTraffic:
             cte = -cte
 
@@ -213,7 +214,7 @@ behavior TurnBehavior(trajectory, target_speed=6):
     it will terminate if the vehicle is outside of an intersection.
     """
 
-    if isinstance(trajectory, PolylineRegion):
+    if isinstance(trajectory, PolylineRegion) or isinstance(trajectory, PathRegion):
         trajectory_centerline = trajectory
     else:
         trajectory_centerline = concatenateCenterlines([traj.centerline for traj in trajectory])

src/scenic/domains/driving/model.scenic

@@ -17,12 +17,18 @@ If you are writing a generic scenario that supports multiple maps, you may leave
 ``map`` parameter undefined; then running the scenario will produce an error unless the
 user uses the :option:`--param` command-line option to specify the map.
 
-The ``use2DMap`` global parameter determines whether or not maps are generated in 2D. Currently
-3D maps are not supported, but are under development. By default, this parameter is `False`
-(so that future versions of Scenic will automatically use 3D maps), unless
-:ref:`2D compatibility mode` is enabled, in which case the default is `True`. The parameter
-can be manually set to `True` to ensure 2D maps are used even if the scenario is not compiled
-in 2D compatibility mode.
+Both 2D and 3D road networkd are now fully supported. The ``use2DMap`` global parameter 
+determines whether or not maps are generated in 2D or 3D. By default, this parameter is 
+`False` (so that 3D maps are used by default), unless :ref:`2D compatibility mode` is enabled, 
+in which case the default is `True`. The parameter can be manually set to `True` to ensure 2D 
+maps are used even if the scenario is not compiled in 2D compatibility mode.
+
+.. note::
+
+    3D road network support is now fully implemented. When ``use2DMap`` is `False`, road
+    geometry, object placement, and region queries will use full 3D coordinates, including
+    elevation (Z) values. This allows for more realistic scenarios with varying terrain
+    heights and complex road structures.
 
 
 .. note::
@@ -57,21 +63,16 @@ param use2DMap = True if is2DMode() else False
 if is2DMode() and not globalParameters.use2DMap:
     raise RuntimeError('in 2D mode, global parameter "use2DMap" must be True')
 
-# Note: The following should be removed when 3D maps are supported
-if not globalParameters.use2DMap:
-    raise RuntimeError('3D maps not supported at this time.'
-        '(to use 2D maps set global parameter "use2DMap" to True)')
-
 ## Load map and set up workspace
 
 if 'map' not in globalParameters:
     raise RuntimeError('need to specify map before importing driving model '
                        '(set the global parameter "map")')
 param map_options = {}
-
+options = {**globalParameters.map_options, "use2DMap": globalParameters.use2DMap}
 #: The road network being used for the scenario, as a `Network` object.
-network : Network = Network.fromFile(globalParameters.map, **globalParameters.map_options)
-
+#network : Network = Network.fromFile(globalParameters.map, **globalParameters.map_options)
+network : Network = Network.fromFile(globalParameters.map, **options)
 workspace = DrivingWorkspace(network)
 
 ## Various useful objects and regions
@@ -282,13 +283,14 @@ class Vehicle(DrivingObject):
             distribution derived from car color popularity statistics; see
             :obj:`Color.defaultCarColor`.
     """
-    regionContainedIn: roadOrShoulder
+    regionContainedIn: roadOrShoulder.boundingPolygon # Change region to bounding polygon region
     position: new Point on road
     parentOrientation: (roadDirection at self.position) + self.roadDeviation
     roadDeviation: 0
     viewAngle: 90 deg
     width: 2
     length: 4.5
+    height: 2.5 # default height;
     color: Color.defaultCarColor()
 
     @property