"""
A stationary rectangular solid that something may collide with
"""
import numpy as np
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from static import StaticObject
[docs]class Obstacle(StaticObject):
def __init__(self, pt1, pt2, color='r'):
"""Create a 3D Rectangle from 2 points
:param pt1: The first point (x, y, z) defining the rect
:type pt1: numpy.ndarray
:param pt2: The second point (x, y, z) defining the rect
:type pt2: numpy.ndarray
:param color: color of the obstacle
:type color: str
:rtype: None
"""
self.pt1 = pt1
self.pt2 = pt2
self.color = color
# TODO: Make this use numpy arrays instead of lists
[docs] def get_paths(self):
"""Returns the paths for each of the surfaces of the
rectangle for plotting.
:returns (bottom, top, front, back, left, right)
:rtype: list of 6 4x3 numpy.ndarrays
"""
[x1, y1, z1] = self.pt1
[x2, y2, z2] = self.pt2
pt1 = [x1, y1, z1]
pt2 = [x1, y1, z2]
pt3 = [x1, y2, z1]
pt4 = [x1, y2, z2]
pt5 = [x2, y1, z1]
pt6 = [x2, y1, z2]
pt7 = [x2, y2, z1]
pt8 = [x2, y2, z2]
bottom = [pt1, pt3, pt7, pt5]
top = [pt2, pt4, pt8, pt6]
front = [pt1, pt2, pt6, pt5]
back = [pt3, pt4, pt8, pt7]
left = [pt1, pt2, pt4, pt3]
right = [pt5, pt6, pt8, pt7]
paths = [bottom, top, front, back, left, right]
return paths
[docs] def is_hit(self, position):
"""Checks if the rectangle is hit by a point or path
:param position: An objects position (x, y, z) or positions if
it is a path([x1, x2, ..], [y1, y2, ..], [z1, z2, ..]
:type position: numpy.ndarray or numpy.matrix
:returns: Whether the obstacle was hit by a point or path
:rtype: bool
"""
is_point = len(position.shape) == 1
if is_point:
x, y, z = position
else:
assert position.shape[1] == 3
x = position[:, 0]
y = position[:, 1]
z = position[:, 2]
[x1, y1, z1] = self.pt1
[x2, y2, z2] = self.pt2
x_hit = (x >= x1) & (x <= x2)
y_hit = (y >= y1) & (y <= y2)
z_hit = (z >= z1) & (z <= z2)
all_hit = x_hit & y_hit & z_hit
if is_point:
return (x_hit and y_hit and z_hit)
else:
return np.any(all_hit)
[docs] def is_hit_by_sphere(self, center, radius):
"""Checks if the rectangle is hit by a sphere
:param center: Sphere's center (x, y, z)
:type center: numpy.ndarray
:param radius: The sphere's radius
:type radius: int
:returns: Whether obstacle was hit by a sphere
:rtype: bool
"""
[x1, y1, z1] = self.pt1
[x2, y2, z2] = self.pt2
x, y, z = center
x_hit = (x + radius >= x1) & (x - radius <= x2)
y_hit = (y + radius >= y1) & (y - radius <= y2)
z_hit = (z + radius >= z1) & (z - radius <= z2)
return x_hit and y_hit and z_hit
[docs] def display(self):
"""Display obstacle properties
:rtype: None
"""
print "Center: {}".format(self.center)
print "Width: {}".format(self.width)
print "Height: {}".format(self.height)
print "Depth: {}".format(self.depth)
[docs] def plot(self, ax):
"""Plots the obstacle at its location
:param ax: Figure to plot on
:type ax: matplotlib.axes
:rtpye: None
"""
paths = self.get_paths()
rectangle = Poly3DCollection(paths, facecolors=self.color)
ax.add_collection3d(rectangle)