def intersect(c1: Union[ConvexPolygon3D, Rectangle3D], c2: Convex) -> Region:
projected_hsis = proj_hsi_to_plane(to_hsi(c2), c1.normal, c1.origin)
if projected_hsis is None:
return Empty()
hsi = intersect(projected_hsis, to_hsi(c1))
if hsi is None:
return Empty()
return ConvexPolygon3D(hsi, c1.origin, c1.rot)
def intersect(r1: Convex, r2: Convex) -> Region:
hs_intersection = intersect(to_hsi(r1), to_hsi(r2))
if hs_intersection is None:
return Empty()
return ConvexPolyhedron(hs_intersection)
def intersect(r1: LineSeg, r2: ConvexPolyhedron) -> Region:
# Q: How to check for no overlap?
halfspaces = to_hsi(r2).halfspaces
l_origin = r1.start
start_end_dist = np.linalg.norm(r1.end - r1.start)
l_dir = (r1.end - r1.start) / start_end_dist
# The original line segment acts as initial parameters
t_max = start_end_dist
t_min = 0.0
for halfspace in halfspaces:
A, b = halfspace[:3], halfspace[3]
if np.dot(A, r1.start) > -b and np.dot(
A, r1.end) > -b: # Line and convex poly do not intersect
return Empty()
dir_align = np.dot(A, l_dir)
point_align = np.dot(A, l_origin)
if np.abs(dir_align) < 1e-8: # Orthogonal
continue
if dir_align > 0: # Pointing in same direction
t_max = np.minimum(t_max, (-b - point_align) / dir_align)
else:
t_min = np.maximum(t_min, (-b - point_align) / dir_align)
return LineSeg(l_origin + t_min * l_dir, l_origin + t_max * l_dir)
def intersect(r1: Convex, r2: Cuboid) -> Region:
if contains(r1, r2):
return r2
if contains(r2, r1):
return r1
hs_intersection = intersect(to_hsi(r1), to_hsi(r2))
if hs_intersection is None:
return Empty()
return ConvexPolyhedron(hs_intersection)
def intersect(p1: Plane, p2: Plane) -> Union[Empty, Line]:
if p1.normal == p2.normal and p1.origin == p2.origin:
return p1
if p1.normal == p2.normal:
return Empty()
line_dir = Vector3D(*np.cross(p1.normal, p2.normal))
s1 = np.dot(p1.normal, p1.origin)
s2 = np.dot(p2.normal, p2.origin)
n1_sq = np.dot(p1.normal, p1.normal)
n2_sq = np.dot(p2.normal, p2.normal)
n1n2 = np.dot(p1.normal, p2.normal)
a = (s2 * n1n2 - s1 * n1_sq) / (n1n2 * n1n2 - n1_sq * n2_sq)
b = (s1 * n1n2 - s2 * n1_sq) / (n1n2 * n1n2 - n1_sq * n2_sq)
line_origin = a * p1.normal + b * p2.normal
line_origin = Vector3D(*line_origin)
return Line(line_origin, line_dir)
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_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 test_empty_empty(self):
self.assertEqual(Empty(), intersect(Empty(), Empty()))
def test_empty_all(self):
self.assertEqual(Empty(), intersect(Empty(), All()))
def test_all_empty(self):
it = intersect(All(), Empty())
self.assertEqual(Empty(), it)