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))
