New closestPointTo function

Author: Eric-Vin

src/scenic/core/regions.py

@@ -118,16 +118,17 @@ def projectVector(self, point, onDirection):
         """Returns point projected onto this region along onDirection."""
         pass
 
+    @abstractmethod
+    def closestPointTo(self, target):
+        """Returns the closest point to target (also a point) that is contained in the region"""
+        pass
+
     @property
     @abstractmethod
     def AABB(self):
         """Axis-aligned bounding box for this `Region`."""
         pass
 
-    @cached_property
-    def midpoint(self):
-        return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2
-
     ## Overridable Methods ##
     # The following methods can be overriden to get better performance or if the region
     # has dependencies (in the case of sampleGiven).
@@ -267,6 +268,10 @@ def __repr__(self):
             s += f" {self.name}"
         return s + f" at {hex(id(self))}>"
 
+    @cached_property
+    def midpoint(self):
+        return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2
+
 
 class PointInRegionDistribution(VectorDistribution):
     """Uniform distribution over points in a Region"""
@@ -335,6 +340,9 @@ def distanceTo(self, point):
     def projectVector(self, point, onDirection):
         return point
 
+    def closestPointTo(self, target):
+        return toVector(target)
+
     @property
     def AABB(self):
         raise TypeError("AllRegion does not have a well defined AABB")
@@ -387,6 +395,9 @@ def distanceTo(self, point):
     def projectVector(self, point, onDirection):
         raise RejectionException("Projecting vector onto empty Region")
 
+    def closestPointTo(self, target):
+        raise RejectionException("Finding closest point in empty Region")
+
     @property
     def AABB(self):
         raise TypeError("EmptyRegion does not have a well defined AABB")
@@ -474,6 +485,9 @@ def projectVector(self, point, onDirection):
             f'{type(self).__name__} does not yet support projection using "on"'
         )
 
+    def closestPointTo(self, target):
+        raise NotImplementedError
+
     @property
     def AABB(self):
         raise NotImplementedError
@@ -587,6 +601,12 @@ def projectVector(self, point, onDirection):
             f'{type(self).__name__} does not yet support projection using "on"'
         )
 
+    def closestPointTo(self, target):
+        target = toVector(target)
+        candidate_points = [region.closestPointTo(target) for region in self.regions]
+        candidate_points.sort(key=lambda pt: pt.distanceTo(target))
+        return candidate_points[0]
+
     @property
     def AABB(self):
         raise NotImplementedError
@@ -688,6 +708,9 @@ def projectVector(self, point, onDirection):
             f'{type(self).__name__} does not yet support projection using "on"'
         )
 
+    def closestPointTo(self, target):
+        raise NotImplementedError
+
     @property
     def AABB(self):
         raise NotImplementedError
@@ -1010,6 +1033,10 @@ def projectVector(self, point, onDirection):
 
         return Vector(*closest_point)
 
+    @distributionFunction
+    def closestPointTo(self, target):
+        return toVector(trimesh.proximity.closest_point(self.mesh, target))
+
     @cached_property
     @distributionFunction
     def circumcircle(self):
@@ -1769,6 +1796,15 @@ def distanceTo(self, point):
 
         return abs(dist)
 
+    @distributionFunction
+    def closestPointTo(self, target):
+        target = toVector(target)
+
+        if self.containsPoint(target):
+            return target
+
+        return super().closestPointTo(target)
+
     @distributionFunction
     def minimumDistanceTo(self, other):
         """Get the minimum distance between this region and another.
@@ -2315,6 +2351,9 @@ def distanceTo(self, point):
     def projectVector(self, point, onDirection):
         raise NotImplementedError
 
+    def closestPointTo(self, target):
+        raise NotImplementedError
+
     def uniformPointInner(self):
         # First generate a point uniformly in a box with dimensions
         # equal to scale, centered at the origin.
@@ -2605,6 +2644,12 @@ def projectVector(self, point, onDirection):
             f'{type(self).__name__} does not yet support projection using "on"'
         )
 
+    @distributionFunction
+    def closestPointTo(self, target):
+        target = toVector(target)
+        pt_2d = toVector(shapely.ops.nearest_points(self.polygons, target)[0])
+        return Vector(pt_2d.x, pt_2d.y, target.z)
+
     @property
     def AABB(self):
         raise NotImplementedError
@@ -2844,6 +2889,9 @@ def defaultOrientation(self, point):
     def projectVector(self, point, onDirection):
         raise NotImplementedError
 
+    def closestPointTo(self, target):
+        raise NotImplementedError
+
     @cached_property
     def AABB(self):
         return (
@@ -3140,6 +3188,12 @@ def distanceTo(self, point):
         dist2D = shapely.distance(self.polygons, makeShapelyPoint(point))
         return math.hypot(dist2D, point[2] - self.z)
 
+    @distributionFunction
+    def closestPointTo(self, target):
+        target = toVector(target)
+        pt_2d = toVector(shapely.ops.nearest_points(self.polygons, target)[0])
+        return Vector(pt_2d.x, pt_2d.y, self.z)
+
     @cached_property
     @distributionFunction
     def inradius(self):
@@ -3731,6 +3785,10 @@ def distanceTo(self, point) -> float:
         dist2D = self.lineString.distance(makeShapelyPoint(point))
         return math.hypot(dist2D, point.z)
 
+    @distributionMethod
+    def closestPointTo(self, target):
+        return toVector(shapely.ops.nearest_points(self.lineString, target)[0])
+
     def projectVector(self, point, onDirection):
         raise TypeError('PolylineRegion does not support projection using "on"')
 
@@ -3958,6 +4016,12 @@ def distanceTo(self, point):
         distance, _ = self.kdTree.query(point)
         return distance
 
+    @distributionMethod
+    def closestPointTo(self, target):
+        point = toVector(point).coordinates
+        _, neighbor_i = self.kdTree.query(point)
+        return toVector(self.points[neighbor_i])
+
     def projectVector(self, point, onDirection):
         raise TypeError('PointSetRegion does not support projection using "on"')
 
@@ -4082,6 +4146,9 @@ def containsRegionInner(self, reg, tolerance):
     def projectVector(self, point, onDirection):
         raise TypeError('GridRegion does not support projection using "on"')
 
+    def closestPointTo(self, target):
+        raise NotImplementedError
+
 
 ###################################################################################################
 # View Regions

src/scenic/core/vectors.py

@@ -16,7 +16,7 @@
 
 import numpy
 from scipy.spatial.transform import Rotation
-import shapely.geometry
+import shapely
 
 from scenic.core.distributions import (
     Distribution,
@@ -450,7 +450,9 @@ def toVector(self) -> Vector:
 
     @staticmethod
     def _canCoerceType(ty):
-        return issubclass(ty, (tuple, list, numpy.ndarray)) or hasattr(ty, "toVector")
+        return issubclass(ty, (tuple, list, numpy.ndarray, shapely.Point)) or hasattr(
+            ty, "toVector"
+        )
 
     @staticmethod
     def _coerce(thing) -> Vector:
@@ -461,6 +463,8 @@ def _coerce(thing) -> Vector:
                     "expected 2D/3D vector, got " f"{type(thing).__name__} of length {l}"
                 )
             return Vector(*thing)
+        elif isinstance(thing, shapely.Point):
+            return Vector(*thing.coords[0])
         else:
             return thing.toVector()
 

src/scenic/core/workspaces.py

@@ -117,7 +117,10 @@ def distanceTo(self, point):
         return self.region.distanceTo(point)
 
     def projectVector(self, point, onDirection):
-        raise self.region.projectVector(point, onDirection)
+        return self.region.projectVector(point, onDirection)
+
+    def closestPointTo(self, target):
+        return self.region.closestPointTo(target)
 
     @property
     def AABB(self):