Minimum Distance Operator

docs/reference/operators.rst

@@ -35,6 +35,14 @@ distance [from *vector*] to *vector*
 -------------------------------------
 The distance to the given position from ego (or the position provided with the optional from vector)
 
+.. _minimum distance [from {Object}] to {Object}:
+.. _minimum distance from:
+
+minimum distance [from *Object*] to *Object*
+--------------------------------------------
+The minimum distance to the given Object from ego (or the Object provided with the optional from Object). Unlike :ref:`distance from`, this operator takes into account the Objects' shapes, sizes, etc...
+
+
 .. _angle [from {vector}] to {vector}:
 
 angle [from *vector* ] to *vector*

src/scenic/core/object_types.py

@@ -1168,6 +1168,20 @@ def distanceTo(self, point):
         """The minimal distance from the space this object occupies to a given point"""
         return self.occupiedSpace.distanceTo(point)
 
+    @cached_method
+    def minimumDistanceTo(self, other):
+        """The minimal distance between this object and another."""
+        if not isinstance(other, Object):
+            raise RuntimeError(
+                f"Cannot compute minimum distance between Object and {type(other)} "
+            )
+
+        # 2D fast path
+        if self._isPlanarBox and other._isPlanarBox and self.z == other.z:
+            return self._boundingPolygon.distance(other._boundingPolygon)
+
+        return self.occupiedSpace.minimumDistanceTo(other.occupiedSpace)
+
     @cached_method
     def intersects(self, other):
         """Whether or not this object intersects another object or region"""

src/scenic/core/regions.py

@@ -1759,6 +1759,22 @@ def distanceTo(self, point):
 
         return abs(dist)
 
+    @distributionFunction
+    def minimumDistanceTo(self, other):
+        """Get the minimum distance between this region and another.
+
+        Currently only supports other as a `MeshVolumeRegion`, and is
+        primarily used for computing minimum distance between objects.
+        """
+        if not isinstance(other, MeshVolumeRegion):
+            raise NotImplementedError(
+                f"Cannot compute distance between MeshVolumeRegion and {type(other)}"
+            )
+
+        selfObj = fcl.CollisionObject(*self._fclData)
+        otherObj = fcl.CollisionObject(*other._fclData)
+        return fcl.distance(selfObj, otherObj)
+
     @cached_property
     @distributionFunction
     def inradius(self):

src/scenic/syntax/ast.py

@@ -446,6 +446,12 @@ class DistanceFromOp(AST):
     base: Optional[ast.AST] = None
 
 
+class MinDistanceFromOp(AST):
+    # because `to` and `from` are symmetric, the first operand will be `target` and the second will be `base`
+    target: ast.AST
+    base: Optional[ast.AST] = None
+
+
 class DistancePastOp(AST):
     target: ast.AST
     base: Optional[ast.AST] = None

src/scenic/syntax/compiler.py

@@ -1705,6 +1705,17 @@ def visit_DistanceFromOp(self, node: s.DistanceFromOp):
             ),
         )
 
+    def visit_MinDistanceFromOp(self, node: s.MinDistanceFromOp):
+        return ast.Call(
+            func=ast.Name(id="MinDistanceFrom", ctx=loadCtx),
+            args=[self.visit(node.target)],
+            keywords=(
+                [ast.keyword(arg="Y", value=self.visit(node.base))]
+                if node.base is not None
+                else []
+            ),
+        )
+
     def visit_DistancePastOp(self, node: s.DistancePastOp):
         return ast.Call(
             func=ast.Name(id="DistancePast", ctx=loadCtx),

src/scenic/syntax/scenic.gram

@@ -1853,6 +1853,8 @@ scenic_prefix_operators:
     # distance past
     | "distance" "past" e1=expression 'of' e2=scenic_prefix_operators { s.DistancePastOp(target=e1, base=e2, LOCATIONS) }
     | "distance" "past" e1=scenic_prefix_operators { s.DistancePastOp(target=e1, LOCATIONS) }
+    # minimum distance from/to
+    | &"minimum" scenic_min_distance_from_op
     # angle from/to
     | &"angle" scenic_angle_from_op
     # altitude from/to
@@ -1868,6 +1870,11 @@ scenic_distance_from_op:
     | "distance" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.DistanceFromOp(target=e1, base=e2, LOCATIONS) }
     | "distance" ('to'|'from') e1=scenic_prefix_operators { s.DistanceFromOp(target=e1, LOCATIONS) }
 
+scenic_min_distance_from_op:
+    | "minimum" "distance" 'from' e1=expression 'to' e2=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, base=e2, LOCATIONS) }
+    | "minimum" "distance" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, base=e2, LOCATIONS) }
+    | "minimum" "distance" ('to'|'from') e1=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, LOCATIONS) }
+
 scenic_angle_from_op:
     | "angle" 'from' e1=expression 'to' e2=scenic_prefix_operators { s.AngleFromOp(base=e1, target=e2, LOCATIONS) }
     | "angle" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.AngleFromOp(target=e1, base=e2, LOCATIONS) }

src/scenic/syntax/veneer.py

@@ -68,6 +68,7 @@
     "ApparentHeading",
     "RelativePosition",
     "DistanceFrom",
+    "MinDistanceFrom",
     "DistancePast",
     "Follow",
     "AngleTo",
@@ -1310,6 +1311,18 @@ def DistanceFrom(X, Y=None):
     return X.distanceTo(Y)
 
 
+def MinDistanceFrom(X, Y=None):
+    """The :grammar:`minimum distance from <object> [to <object>]` operator.
+
+    If the :grammar:`to <object>` is omitted, the ego is used.
+    """
+    X = toTypes(X, (Object,), '"minimum distance from X to Y" with X not an Object')
+    if Y is None:
+        Y = ego()
+    Y = toTypes(Y, (Object,), '"minimum distance from X to Y" with Y not an Object')
+    return X.minimumDistanceTo(Y)
+
+
 def DistancePast(X, Y=None):
     """The :grammar:`distance past <vector> of <oriented point>` operator.
 

tests/syntax/test_compiler.py

@@ -2107,6 +2107,22 @@ def test_distance_to_op_from(self):
             case _:
                 assert False
 
+    def test_min_distance_to_op(self):
+        node, _ = compileScenicAST(MinDistanceFromOp(Name("X"), None))
+        match node:
+            case Call(Name("MinDistanceFrom"), [Name("X")], []):
+                assert True
+            case _:
+                assert False
+
+    def test_min_distance_to_op_from(self):
+        node, _ = compileScenicAST(MinDistanceFromOp(Name("X"), Name("Y")))
+        match node:
+            case Call(Name("MinDistanceFrom"), [Name("X")], [keyword("Y", Name("Y"))]):
+                assert True
+            case _:
+                assert False
+
     def test_distance_past_op(self):
         node, _ = compileScenicAST(DistancePastOp(Name("X")))
         match node:

tests/syntax/test_operators.py

@@ -188,6 +188,57 @@ def test_distance_to_region():
     assert p == pytest.approx(2)
 
 
+# Minimum Distance
+def test_minimum_distance():
+    p = sampleParamPFrom(
+        """
+        ego = new Object at (1.5, 2, 2.5),
+            with width 1, with length 2, with height 3
+        other = new Object at (-10, -10, -10),
+            with width 2, with length 2, with height 2
+        param p = minimum distance to other
+        """
+    )
+    assert p == pytest.approx(math.hypot(10, 10, 10))
+
+
+def test_minimum_distance_from():
+    p = sampleParamPFrom(
+        """
+        foo = new Object at (1.5, 2, 2.5),
+            with width 1, with length 2, with height 3
+        bar = new Object at (-10, -10, -10),
+            with width 2, with length 2, with height 2
+        param p = minimum distance from foo to bar
+        """
+    )
+    assert p == pytest.approx(math.hypot(10, 10, 10))
+
+
+def test_minimum_distance_no_ego():
+    with pytest.raises(InvalidScenarioError):
+        sampleParamPFrom(
+            """
+            other = new Object at (-10, -10, -10),
+                with width 2, with length 2, with height 2
+            param p = minimum distance to other
+            """
+        )
+
+
+def test_minimum_distance_2d():
+    p = sampleParamPFrom(
+        """
+        ego = new Object at (1.5, 2),
+            with width 1, with length 2
+        other = new Object at (-10, -10),
+            with width 2, with length 2
+        param p = minimum distance to other
+        """
+    )
+    assert p == pytest.approx(math.hypot(10, 10))
+
+
 # Distance past
 
 

tests/syntax/test_parser.py

@@ -2337,6 +2337,33 @@ def test_distance_to_from(self):
             case _:
                 assert False
 
+    def test_min_distance_to(self):
+        mod = parse_string_helper("minimum distance to x")
+        stmt = mod.body[0]
+        match stmt:
+            case Expr(MinDistanceFromOp(Name("x"), None)):
+                assert True
+            case _:
+                assert False
+
+    def test_min_distance_from_to(self):
+        mod = parse_string_helper("minimum distance from x to y")
+        stmt = mod.body[0]
+        match stmt:
+            case Expr(MinDistanceFromOp(Name("x"), Name("y"))):
+                assert True
+            case _:
+                assert False
+
+    def test_min_distance_to_from(self):
+        mod = parse_string_helper("minimum distance to x from y")
+        stmt = mod.body[0]
+        match stmt:
+            case Expr(MinDistanceFromOp(Name("x"), Name("y"))):
+                assert True
+            case _:
+                assert False
+
     @pytest.mark.parametrize(
         "code,expected",
         [