def __init__(self, xs, ys, zs, course=0, pitch=0, roll=0):
center_x, center_y, center_z = (xs[0] + xs[1]) / 2, (
ys[0] + ys[1]) / 2, (zs[0] + zs[1]) / 2
xs, ys, zs = np.array(xs) - center_x, np.array(
ys) - center_y, np.array(zs) - center_z
verts = np.array([[xs[0], ys[0], zs[0]], [xs[1], ys[0], zs[0]],
[xs[0], ys[1], zs[0]], [xs[1], ys[1], zs[0]],
[xs[0], ys[0], zs[1]], [xs[1], ys[0], zs[1]],
[xs[0], ys[1], zs[1]], [xs[1], ys[1], zs[1]]])
rotate_course = math.rotate_matrix2d(np.radians([course])[0])
rotate_pitch = math.rotate_matrix2d(np.radians([pitch])[0])
rotate_roll = math.rotate_matrix2d(np.radians([roll])[0])
verts[:, [1, 2]] = math.homo_translate(rotate_roll, verts[:, [1, 2]])
verts[:, [0, 2]] = math.homo_translate(rotate_pitch, verts[:, [0, 2]])
verts[:, [0, 1]] = math.homo_translate(rotate_course, verts[:, [0, 1]])
verts += [center_x, center_y, center_z]
faces = [[0, 1, 2], [1, 2, 3], [4, 5, 6], [5, 6, 7], [0, 1, 4],
[1, 4, 5], [2, 3, 6], [3, 6, 7], [0, 2, 4], [2, 4, 6],
[1, 3, 5], [3, 5, 7]]
super(Box, self).__init__(verts, faces=faces)
def __init__(self, xs, ys, zs, course=0, pitch=0, roll=0):
center_x, center_y, center_z = (xs[0] + xs[1]) / 2, (ys[0] + ys[1]) / 2, (zs[0] + zs[1]) / 2
xs, ys, zs = np.array(xs) - center_x, np.array(ys) - center_y, np.array(zs) - center_z
verts = np.array([
[xs[0], ys[0], zs[0]],
[xs[1], ys[0], zs[0]],
[xs[0], ys[1], zs[0]],
[xs[1], ys[1], zs[0]],
[xs[0], ys[0], zs[1]],
[xs[1], ys[0], zs[1]],
[xs[0], ys[1], zs[1]],
[xs[1], ys[1], zs[1]]])
rotate_course = math.rotate_matrix2d(np.radians([course])[0])
rotate_pitch = math.rotate_matrix2d(np.radians([pitch])[0])
rotate_roll = math.rotate_matrix2d(np.radians([roll])[0])
verts[:, [1, 2]] = math.homo_translate(rotate_roll, verts[:, [1, 2]])
verts[:, [0, 2]] = math.homo_translate(rotate_pitch, verts[:, [0, 2]])
verts[:, [0, 1]] = math.homo_translate(rotate_course, verts[:, [0, 1]])
verts += [center_x, center_y, center_z]
faces = [[0, 1, 2], [1, 2, 3],
[4, 5, 6], [5, 6, 7],
[0, 1, 4], [1, 4, 5],
[2, 3, 6], [3, 6, 7],
[0, 2, 4], [2, 4, 6],
[1, 3, 5], [3, 5, 7]]
super(Box, self).__init__(verts, faces=faces)
def calculate_point(self, inner_angle):
xy = self.a * np.cos(np.deg2rad(inner_angle)), self.b * np.sin(
np.deg2rad(inner_angle))
xy = math.homo_translate(math.rotate_matrix2d(np.deg2rad(self.angle)),
xy)
xy = np.array([self.x, self.y]) + xy
return xy
def _get_camera_direction(self):
course = np.radians([self.course])[0]
pitch = np.radians([self.pitch])[0]
if pitch == 270:
return np.array([0, 0, -1])
v = np.array([0, np.cos(pitch), np.sin(pitch)])
v = np.dot(math.rotate_matrix2d(-course), v)
return v
def _get_camera_direction(self):
course = np.radians([self.course])[0]
pitch = np.radians([self.pitch])[0]
if pitch == 270:
return np.array([0, 0, -1])
v = np.array([0, np.cos(pitch), np.sin(pitch)])
v = np.dot(math.rotate_matrix2d(-course), v)
return v
def _transformation_ellipse_to_circle(self, ellipse):
center, axes, angle = (ellipse.x, ellipse.y), (ellipse.a, ellipse.b), ellipse.angle
rotate = rotate_matrix2d(np.deg2rad(-angle))
move_to_zeros = [[1, 0, -center[0]],
[0, 1, -center[1]],
[0, 0, 1]]
scale = [[1 / axes[0], 0, 0],
[0, 1 / axes[1], 0],
[0, 0, 1]]
move_to_offset = [[1, 0, center[0]],
[0, 1, center[1]],
[0, 0, 1]]
return np.dot(move_to_offset, np.dot(scale, np.dot(rotate, move_to_zeros)))
def calculate_point(self, inner_angle):
xy = self.a * np.cos(np.deg2rad(inner_angle)), self.b * np.sin(np.deg2rad(inner_angle))
xy = math.homo_translate(math.rotate_matrix2d(np.deg2rad(self.angle)), xy)
xy = np.array([self.x, self.y]) + xy
return xy
