do circumradius check before computing bounding boxes; skip inradius …

…check unless precomputed

src/scenic/core/object_types.py

@@ -1334,15 +1334,16 @@ def occupiedSpace(self):
         if self._sampleParent and self._sampleParent._hasStaticBounds:
             return self._sampleParent.occupiedSpace
 
+        shape = self.shape
         scaledShape = self._scaledShape
         if scaledShape:
             mesh = scaledShape.mesh
             dimensions = None  # mesh does not need to be scaled
             convex = scaledShape.isConvex
         else:
-            mesh = self.shape.mesh
+            mesh = shape.mesh
             dimensions = (self.width, self.length, self.height)
-            convex = self.shape.isConvex
+            convex = shape.isConvex
         return MeshVolumeRegion(
             mesh=mesh,
             dimensions=dimensions,
@@ -1351,6 +1352,7 @@ def occupiedSpace(self):
             centerMesh=False,
             _internal=True,
             _isConvex=convex,
+            _shape=shape,
             _scaledShape=scaledShape,
         )
 

src/scenic/core/regions.py

@@ -57,7 +57,13 @@
 )
 from scenic.core.lazy_eval import isLazy, valueInContext
 from scenic.core.type_support import toOrientation, toScalar, toVector
-from scenic.core.utils import cached, cached_method, cached_property, unifyMesh
+from scenic.core.utils import (
+    cached,
+    cached_method,
+    cached_property,
+    findMeshInteriorPoint,
+    unifyMesh,
+)
 from scenic.core.vectors import (
     Orientation,
     OrientedVector,
@@ -807,7 +813,7 @@ def __init__(
         # Copy parameters
         self._mesh = mesh
         self.dimensions = None if dimensions is None else toVector(dimensions)
-        self.position = None if position is None else toVector(position)
+        self.position = Vector(0, 0, 0) if position is None else toVector(position)
         self.rotation = None if rotation is None else toOrientation(rotation)
         self.orientation = None if orientation is None else toDistribution(orientation)
         self.tolerance = tolerance
@@ -939,17 +945,6 @@ def mesh(self):
             mesh.vertices -= mesh.bounding_box.center_mass
 
         # Apply scaling, rotation, and translation, if any
-        if self.dimensions is not None:
-            scale = numpy.array(self.dimensions) / mesh.extents
-        else:
-            scale = None
-        if self.rotation is not None:
-            angles = self.rotation._trimeshEulerAngles()
-        else:
-            angles = None
-        self._transform = compose_matrix(
-            scale=scale, angles=angles, translate=self.position
-        )
         mesh.apply_transform(self._transform)
 
         self._mesh = mesh
@@ -1016,6 +1011,19 @@ def AABB(self):
             tuple(self.mesh.bounds[1]),
         )
 
+    @cached_property
+    def _transform(self):
+        if self.dimensions is not None:
+            scale = numpy.array(self.dimensions) / self._mesh.extents
+        else:
+            scale = None
+        if self.rotation is not None:
+            angles = self.rotation._trimeshEulerAngles()
+        else:
+            angles = None
+        transform = compose_matrix(scale=scale, angles=angles, translate=self.position)
+        return transform
+
     @cached_property
     def _boundingPolygonHull(self):
         assert not isLazy(self)
@@ -1093,11 +1101,13 @@ def __init__(
         *args,
         _internal=False,
         _isConvex=None,
+        _shape=None,
         _scaledShape=None,
         **kwargs,
     ):
         super().__init__(*args, **kwargs)
         self._isConvex = _isConvex
+        self._shape = _shape
         self._scaledShape = _scaledShape
         self._num_samples = None
 
@@ -1129,6 +1139,31 @@ def intersects(self, other, triedReversed=False):
         """
         if isinstance(other, MeshVolumeRegion):
             # PASS 1
+            # Check if the centers of the regions are far enough apart that the regions
+            # cannot overlap. This check only requires the circumradius of each region,
+            # which we can often precompute without explicitly constructing the mesh.
+            center_distance = numpy.linalg.norm(self.position - other.position)
+            if center_distance > self._circumradius + other._circumradius:
+                return False
+
+            # PASS 1.5
+            # If precomputed inradii are available, check if the volumes are close enough
+            # to ensure a collision. (While we're at it, check circumradii too.)
+            if self._scaledShape and other._scaledShape:
+                s_point = self._interiorPoint
+                s_inradius, s_circumradius = self._interiorPointRadii
+                o_point = other._interiorPoint
+                o_inradius, o_circumradius = other._interiorPointRadii
+
+                point_distance = numpy.linalg.norm(s_point - o_point)
+
+                if point_distance < s_inradius + o_inradius:
+                    return True
+
+                if point_distance > s_circumradius + o_circumradius:
+                    return False
+
+            # PASS 2
             # Check if bounding boxes intersect. If not, volumes cannot intersect.
             # For bounding boxes to intersect there must be overlap of the bounds
             # in all 3 dimensions.
@@ -1144,29 +1179,6 @@ def intersects(self, other, triedReversed=False):
             if not bb_overlap:
                 return False
 
-            # PASS 2
-            # Compute inradius and circumradius for a candidate point in each region,
-            # and compute the inradius and circumradius of each point. If the candidate
-            # points are closer than the sum of the inradius values, they must intersect.
-            # If the candidate points are farther apart than the sum of the circumradius
-            # values, they can't intersect.
-
-            # Get a candidate point from each mesh and the corresponding
-            # inradius/circumradius around that point.
-            s_point = self._candidatePoint
-            s_inradius, s_circumradius = self._candidateRadii
-            o_point = other._candidatePoint
-            o_inradius, o_circumradius = other._candidateRadii
-
-            # Get the distance between the two points and check for mandatory or impossible collision.
-            point_distance = numpy.linalg.norm(s_point - o_point)
-
-            if point_distance < s_inradius + o_inradius:
-                return True
-
-            if point_distance > s_circumradius + o_circumradius:
-                return False
-
             # PASS 3
             # Use FCL to check for intersection between the surfaces.
             # If the surfaces collide, that implies a collision of the volumes.
@@ -1185,13 +1197,14 @@ def intersects(self, other, triedReversed=False):
                 return surface_collision
 
             # PASS 4
-            # If we have 2 candidate points and both regions have only one body,
-            # we can just check if either region contains the candidate point of the
-            # other. (This is because we previously ruled out surface intersections)
+            # If both regions have only one body, we can just check if either region
+            # contains an arbitrary interior point of the other. (This is because we
+            # previously ruled out surface intersections)
             if self._bodyCount == 1 and other._bodyCount == 1:
-                return self._containsPointExact(o_point) or other._containsPointExact(
-                    s_point
-                )
+                overlap = self._containsPointExact(
+                    other._interiorPoint
+                ) or other._containsPointExact(self._interiorPoint)
+                return overlap
 
             # PASS 5
             # Compute intersection and check if it's empty. Expensive but guaranteed
@@ -1826,48 +1839,37 @@ def num_samples(self):
         return self._num_samples
 
     @cached_property
-    def _candidatePoint(self):
+    def _circumradius(self):
+        if self._scaledShape:
+            return self._scaledShape._circumradius
+        if self._shape:
+            scale = max(self.dimensions) if self.dimensions else 1
+            return scale * self._shape._circumradius
+
+        return numpy.max(numpy.linalg.norm(self.mesh.vertices, axis=1))
+
+    @cached_property
+    def _interiorPoint(self):
         # Use precomputed point if available (transformed appropriately)
         if self._scaledShape:
-            raw = self._scaledShape._candidatePoint
+            raw = self._scaledShape._interiorPoint
             homog = numpy.append(raw, [1])
             return numpy.dot(self._rigidTransform, homog)[:3]
+        if self._shape:
+            raw = self._shape._interiorPoint
+            homog = numpy.append(raw, [1])
+            return numpy.dot(self._transform, homog)[:3]
 
-        # Use center of mass if it's contained
-        mesh = self.mesh
-        com = mesh.bounding_box.center_mass
-        if mesh.contains([com])[0]:
-            return com
-
-        # Try sampling a point inside the volume
-        samples = trimesh.sample.volume_mesh(mesh, self.num_samples)
-        if samples.size > 0:
-            return samples[0]
-
-        # If all else fails, take a point from the surface and move inward
-        surfacePt, index = list(zip(*mesh.sample(1, return_index=True)))[0]
-        inward = -mesh.face_normals[index]
-        startPt = surfacePt + 1e-6 * inward
-        hits, _, _ = mesh.ray.intersects_location(
-            ray_origins=[startPt],
-            ray_directions=[inward],
-            multiple_hits=False,
-        )
-        if hits.size > 0:
-            endPt = hits[0]
-            midPt = (surfacePt + endPt) / 2.0
-            if mesh.contains([midPt])[0]:
-                return midPt
-        return surfacePt
+        return findMeshInteriorPoint(self.mesh, num_samples=self.num_samples)
 
     @cached_property
-    def _candidateRadii(self):
+    def _interiorPointRadii(self):
         # Use precomputed radii if available
         if self._scaledShape:
-            return self._scaledShape._candidateRadii
+            return self._scaledShape._interiorPointRadii
 
-        # Compute inradius and circumradius w.r.t. the candidate point
-        point = self._candidatePoint
+        # Compute inradius and circumradius w.r.t. the point
+        point = self._interiorPoint
         pq = trimesh.proximity.ProximityQuery(self.mesh)
         inradius = abs(pq.signed_distance([point])[0])
         circumradius = numpy.max(numpy.linalg.norm(self.mesh.vertices - point, axis=1))

src/scenic/core/shapes.py

@@ -11,7 +11,7 @@
 )
 
 from scenic.core.type_support import toOrientation
-from scenic.core.utils import cached_property, unifyMesh
+from scenic.core.utils import cached_property, findMeshInteriorPoint, unifyMesh
 from scenic.core.vectors import Orientation
 
 ###################################################################################################
@@ -64,6 +64,14 @@ def mesh(self):
     def isConvex(self):
         pass
 
+    @cached_property
+    def _circumradius(self):
+        return numpy.max(numpy.linalg.norm(self.mesh.vertices, axis=1))
+
+    @cached_property
+    def _interiorPoint(self):
+        return findMeshInteriorPoint(self.mesh)
+
 
 ###################################################################################################
 # 3D Shape Classes

src/scenic/core/utils.py

@@ -307,6 +307,40 @@ def repairMesh(mesh, pitch=(1 / 2) ** 6, verbose=True):
     raise ValueError("Mesh could not be repaired.")
 
 
+def findMeshInteriorPoint(mesh, num_samples=None):
+    # Use center of mass if it's contained
+    com = mesh.bounding_box.center_mass
+    if mesh.contains([com])[0]:
+        return com
+
+    # Try sampling a point inside the volume
+    if num_samples is None:
+        p_volume = mesh.volume / mesh.bounding_box.volume
+        if p_volume > 0.99:
+            num_samples = 1
+        else:
+            num_samples = math.ceil(min(1e6, max(1, math.log(0.01, 1 - p_volume))))
+    samples = trimesh.sample.volume_mesh(mesh, num_samples)
+    if samples.size > 0:
+        return samples[0]
+
+    # If all else fails, take a point from the surface and move inward
+    surfacePt, index = list(zip(*mesh.sample(1, return_index=True)))[0]
+    inward = -mesh.face_normals[index]
+    startPt = surfacePt + 1e-6 * inward
+    hits, _, _ = mesh.ray.intersects_location(
+        ray_origins=[startPt],
+        ray_directions=[inward],
+        multiple_hits=False,
+    )
+    if hits.size > 0:
+        endPt = hits[0]
+        midPt = (surfacePt + endPt) / 2.0
+        if mesh.contains([midPt])[0]:
+            return midPt
+    return surfacePt
+
+
 class DefaultIdentityDict:
     """Dictionary which is the identity map by default.
 

tests/core/test_regions.py

@@ -9,6 +9,7 @@
 from scenic.core.distributions import RandomControlFlowError, Range
 from scenic.core.object_types import Object, OrientedPoint
 from scenic.core.regions import *
+from scenic.core.shapes import MeshShape
 from scenic.core.vectors import Orientation, VectorField
 from tests.utils import deprecationTest, sampleSceneFrom
 
@@ -339,7 +340,21 @@ def test_mesh_intersects():
     assert not r1.getSurfaceRegion().intersects(r2.getSurfaceRegion())
 
 
-def test_mesh_candidatePoint():
+def test_mesh_circumradius(getAssetPath):
+    r1 = BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6))
+    bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4)
+    r2 = MeshVolumeRegion(r1.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r1)
+    planePath = getAssetPath("meshes/classic_plane.obj.bz2")
+    r3 = MeshVolumeRegion.fromFile(planePath, dimensions=(20, 20, 10))
+    shape = MeshShape.fromFile(planePath)
+    r4 = MeshVolumeRegion(shape.mesh, dimensions=(0.5, 2, 1.5), rotation=bo, _shape=shape)
+    for reg in (r1, r2, r3, r4):
+        pos = reg.position
+        d = 2.01 * reg._circumradius
+        assert SpheroidRegion(dimensions=(d, d, d), position=pos).containsRegion(reg)
+
+
+def test_mesh_interiorPoint():
     regions = [
         BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)),
         BoxRegion().difference(BoxRegion(dimensions=(0.1, 0.1, 0.1))),
@@ -355,11 +370,15 @@ def test_mesh_candidatePoint():
     r2 = MeshVolumeRegion(r.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r)
     regions.append(r2)
 
+    shape = MeshShape(BoxRegion(dimensions=(1, 2, 3)).mesh)
+    r3 = MeshVolumeRegion(shape.mesh, position=(-10, -5, 30), rotation=bo, _shape=shape)
+    regions.append(r3)
+
     for reg in regions:
-        cp = reg._candidatePoint
+        cp = reg._interiorPoint
         # N.B. _containsPointExact can fail with embreex installed!
         assert reg.containsPoint(cp)
-        inr, circumr = reg._candidateRadii
+        inr, circumr = reg._interiorPointRadii
         d = 1.99 * inr
         assert reg.containsRegion(SpheroidRegion(dimensions=(d, d, d), position=cp))
         d = 2.01 * circumr