def OnTopOf(thing: Union[Point3D, Vector3D, Object, Rectangle3D], dist: float = eps, strict: bool = False) -> Specifier: if isinstance(thing, Rectangle3D): if strict: new = DelayedArgument({'width', 'length', 'height'}, lambda s: PointInRegionDistribution( on_top_of_rect(s, thing, dist, strict))) else: new = DelayedArgument({'height'}, lambda s: PointInRegionDistribution( on_top_of_rect(s, thing, dist))) elif isinstance(thing, Object): if strict: new = DelayedArgument( {'width', 'length', 'height'}, lambda s: PointInRegionDistribution( top_surface_region(s, thing, dist, strict))) else: new = DelayedArgument({'height'}, lambda s: PointInRegionDistribution( top_surface_region(s, thing, dist))) elif isinstance(thing, Point3D): new = DelayedArgument( {'height'}, lambda s: thing.position + Vector3D(0, 0, dist + s.height / 2.0)) elif isinstance(thing, Vector3D): new = DelayedArgument( {'height'}, lambda s: thing + Vector3D(0, 0, dist + s.height / 2.0)) else: raise TypeError( f'Asking to be on top of {thing} which has unsupported type {type(thing)}' ) return Specifier('position', new)
def FacingToward3D(pos): pos = toType(pos, Vector3D) return Specifier( 'orientation', DelayedArgument({'position'}, lambda s: rotation_to_euler( Vector3D(0, 1, 0), Vector3D(pos[0], pos[1], s.position[2]) - s.position)))
def test_halfspace_convpolyh_noIntersection(self): hs = HalfSpace(Vector3D(0, 4, 0), Vector3D(0, 1, 0)) cp = ConvexPolyhedron( to_hsi(Cuboid(Vector3D(0.0, 0.0, 0.0), Vector3D(0, 0, 0), 1, 1, 1))) it = intersect(hs, cp) self.assertTrue(isinstance(it, Empty))
def test_all_others(self): s = Spherical(Vector3D(0, 0, 0), 3) c = Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1) it_1 = intersect(All(), s) it_2 = intersect(All(), c) self.assertEqual(it_1, s) self.assertEqual(it_2, c)
def test_others_all(self): s = Spherical(Vector3D(0, 0, 0), 3) c = Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1) it_1 = intersect(s, All()) it_2 = intersect(c, All()) self.assertEqual(it_1, s) self.assertEqual(it_2, c)
def test_halfspace_convpg_noIntersection(self): hs = HalfSpace(Vector3D(0, 4, 0), Vector3D(0, 1, 0)) hsi = HalfspaceIntersection( np.array([[1, 0, -3], [0, 1, -3], [-1, 0, 0], [0, -1, 0]]), np.array([1.0, 1.0])) cp = ConvexPolygon3D(hsi=hsi, origin=Vector3D(0, 0, 0), rot=Vector3D(0, 0, 0)) it = intersect(hs, cp) self.assertTrue(isinstance(it, Empty))
def uniform_point_inner(r: Plane): # We don't want arbitrarily chosen axis to be exactly aligned with normal if 1.0 - np.abs(r.normal, Vector3D(1.0, 0.0, 0.0)) >= 1e-5: u = np.cross(r.normal, Vector3D(1.0, 0.0, 0.0)) else: u = np.cross(r.normal, Vector3D(0.0, 1.0, 0.0)) v = np.cross(r.normal, u) a = np.random.uniform(-r.dist / 2.0, r.dist / 2.0) b = np.random.uniform(-r.dist / 2.0, r.dist / 2.0) offset = a * u + b * v return r.origin + offset
def AlignedWith(thing: Union[Point3D, Object], axis: str) -> Specifier: align_point = thing.position if axis == 'x': reg = Rectangle3D(100.0, 100.0, align_point, Vector3D(0.0, np.pi / 2.0, 0.0)) elif axis == 'y': reg = Rectangle3D(100.0, 100.0, align_point, Vector3D(0.0, 0.0, np.pi / 2.0)) elif axis == 'z': reg = Rectangle3D(100.0, 100.0, align_point, Vector3D(0.0, 0.0, 0.0)) else: raise ValueError("Specified axis must be one of 'x', 'y', or 'z'") new = PointInRegionDistribution(reg) return Specifier('position', new)
def test_halfspace_convpg_intersection(self): hs = HalfSpace(Vector3D(0, 2, 0), Vector3D(0, 1, 0)) hsi = HalfspaceIntersection( np.array([[1, 0, -3], [0, 1, -3], [-1, 0, 0], [0, -1, 0]]), np.array([1.0, 1.0])) cp = ConvexPolygon3D(hsi=hsi, origin=Vector3D(0, 0, 0), rot=Vector3D(0, 0, 0)) it = intersect(hs, cp) int_point_3d: Vector3D = rotate_euler_v3d( Vector3D(*it.hsi.interior_point, 0.0), it.rot) + it.origin self.assertTrue(isinstance(it, ConvexPolygon3D)) self.assertTrue(contains_point(hs, int_point_3d)) self.assertTrue(contains_point(cp, int_point_3d))
def uniform_point_inner(r: Cuboid): hw, hl, hh = r.hw, r.hl, r.hh rx = random.uniform(-hw, hw) ry = random.uniform(-hl, hl) rz = random.uniform(-hh, hh) pt = r.position + rotate_euler_v3d(Vector3D(rx, ry, rz), r.orientation) return pt
def draw_plane(ax, origin: np.array, normal: np.array, color: str = 'b', alpha: float = 0.5, size=1.0): norm_rot = np.array(rotation_to_euler(Vector3D(0.0, 0.0, 1.0), normal)) draw_rect_3d(ax, origin, size, size, norm_rot, color, alpha)
def top_surface_region(obj_to_place: Object, ref_obj: Object, dist: float, strict: bool = False) -> Rectangle3D: ref_top_surface = ref_obj.position + rotate_euler_v3d( Vector3D(0, 0, ref_obj.height / 2.0), ref_obj.orientation) rotated_offset = rotate_euler_v3d( Vector3D(0, 0, dist + obj_to_place.height / 2.0), ref_obj.orientation) region_pos = rotated_offset + ref_top_surface if strict: # assert ref_obj.width > obj_to_place.width and ref_obj.length > obj_to_place.length return Rectangle3D(ref_obj.width - obj_to_place.width, ref_obj.length - obj_to_place.length, region_pos, ref_obj.orientation) return Rectangle3D(ref_obj.width, ref_obj.length, region_pos, ref_obj.orientation)
def Beyond3D(pos, offset, from_pt): pos = toType(pos, Vector3D) d_type = underlyingType(offset) if d_type is float or d_type is int: offset = Vector3D(offset, 0, 0) elif d_type is not Vector3D: raise RuntimeParseError( 'specifier "beyond X by Y from Z" with Z not a vector') from_pt = toType(from_pt, Vector3D) new_pos = offset_beyond(pos, offset, from_pt) return Specifier('position', new_pos)
def uniform_point_inner(r: Spherical): x, y, z = r.center u = 2.0 * random.random() - 1.0 phi = 2.0 * math.pi * random.random() r = random.random()**(1 / 3.) x_offset = r * np.cos(phi) * (1 - u**2)**0.5 y_offset = r * np.sin(phi) * (1 - u**2)**0.5 z_offset = r * u pt = Vector3D(x + x_offset, y + y_offset, z + z_offset) return pt pass
def test_others_intersection(self): inter = Intersection([ Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1), Rectangle3D(1, 1, Vector3D(0, 0, 0), Vector3D(0, 0, 0)) ]) c = Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1) self.assertRaises(DispatchError, intersect, inter, c)
def on_top_of_rect(obj_to_place: Object, r: Rectangle3D, dist: float, strict: bool = False) -> Rectangle3D: offset = rotate_euler_v3d(Vector3D(0, 0, dist + obj_to_place.height / 2.0), r.rot) if strict: # assert r.width > obj_to_place.width and r.length > obj_to_place.length return Rectangle3D(r.width - obj_to_place.width, r.length - obj_to_place.length, r.origin + offset, r.rot) return Rectangle3D(r.width, r.length, r.origin + offset, r.rot)
def test_halfspace_halfspace_same(self): hs_1 = HalfSpace(Vector3D(0, 0, 0), Vector3D(0, 1, 0)) hs_2 = HalfSpace(Vector3D(0, 0, 0), Vector3D(0, 1, 0)) it = intersect(hs_1, hs_2) self.assertTrue(isinstance(it, ConvexPolyhedron)) self.assertTrue(contains_point(hs_1, Vector3D(*it.hsi.interior_point))) self.assertTrue(contains_point(hs_2, Vector3D(*it.hsi.interior_point)))
def test_halfspace_convpolyh_intersection(self): hs = HalfSpace(Vector3D(0, 1, 0), Vector3D(0, 1, 0)) cp = ConvexPolyhedron( to_hsi(Cuboid(Vector3D(0.0, 0.0, 0.0), Vector3D(0, 0, 0), 5, 5, 5))) it = intersect(hs, cp) self.assertTrue(isinstance(it, ConvexPolyhedron)) self.assertTrue(contains_point(hs, Vector3D(*it.hsi.interior_point))) self.assertTrue(contains_point(cp, Vector3D(*it.hsi.interior_point)))
class Point3D(Constructible): position: Vector3D = Vector3D(0, 0, 0) visibleDistance: float = 50 def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) def __post_init__(self): self.corners = (self.position, ) self.visibleRegion = Spherical(self.position, self.visibleDistance) def to_vector_3d(self): return self.position def sample_given_dependencies(self, dep_values): sample = super().sample_given_dependencies(dep_values) return sample def __getattr__(self, attr): if hasattr(self.position, attr): return getattr(self.to_vector_3d(), attr) else: raise AttributeError( f"'{type(self).__name__}' object has no attribute '{attr}'")
def uniform_point_inner(r: HalfSpace): untransformed_point = Vector3D(*np.random.uniform(0, r.dist, 3)) return rotate_euler_v3d(untransformed_point, r.rot) + r.point
def uniform_point_inner(r: PointSet): return Vector3D(*random.choice(r.points))
def test_others_empty(self): s = Spherical(Vector3D(0, 0, 0), 3) c = Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1) self.assertEqual(Empty(), intersect(s, Empty())) self.assertEqual(Empty(), intersect(c, Empty()))
def test_others_spherical(self): s = Spherical(Vector3D(0, 0, 0), 3) c = Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1) self.assertRaises(DispatchError, intersect, c, s)
def uniform_point_inner(r: ConvexPolyhedron): return Vector3D(*hit_and_run(r.hsi))
def test_empty_others(self): s = Spherical(Vector3D(0, 0, 0), 3) c = Cuboid(Vector3D(0, 0, 0), Vector3D(0, 0, 0), 1, 1, 1) self.assertEqual(Empty(), intersect(Empty(), s)) self.assertEqual(Empty(), intersect(Empty(), c))
def uniform_point_inner(r: ConvexPolygon3D): random_point_flat = Vector3D(*hit_and_run(r.hsi), 0) return rotate_euler_v3d(random_point_flat, r.rot) + r.origin
def uniform_point_inner(r: Rectangle3D): x = random.uniform(-r.width / 2.0, r.width / 2.0) y = random.uniform(-r.length / 2.0, r.length / 2.0) flat_point = Vector3D(x, y, 0) return rotate_euler_v3d(flat_point, r.rot) + r.origin
def contains_point(r: Spherical, point: Vector3D) -> bool: return point.distanceTo(r.center) <= r.radius
def rot(self): return rotation_to_euler(Vector3D(0, 0, 1), self.normal)
def test_halfspace_halfspace_noIntersectionByDistance(self): hs_1 = HalfSpace(Vector3D(0, 1, 0), Vector3D(0, 1, 0)) hs_2 = HalfSpace(Vector3D(0, 10000, 0), Vector3D(0, 1, 0)) it = intersect(hs_1, hs_2) self.assertTrue(isinstance(it, Empty))