def _distance_to_ellipse_as_circle(self, points, ellipse):
to_circle = self._transformation_ellipse_to_circle(ellipse)
points = homo_translate(to_circle, points)
center, axes, angle = (ellipse.x, ellipse.y), (ellipse.a, ellipse.b), ellipse.angle
center = homo_translate(to_circle, center)
return np.abs(np.linalg.norm(points - center, axis=-1) - 1.0)
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 create_frustum_points(self, frustums_depth=1.0):
rt_inv = np.linalg.inv(self.get_mv_matrix())
p = self.get_p_matrix()[:-1, :-1]
camera_corners = math.homo_translate(np.linalg.inv(p), aabb.rect_to_quad([[-1.0, -1.0 * self.aspect],
[1.0, 1.0 * self.aspect]]))
corners = np.hstack([camera_corners, [[-1]] * 4]) * frustums_depth
frustum_points = math.homo_translate(rt_inv, np.vstack([[[0, 0, 0]], corners]))
return frustum_points
def _calculate_sectors(self, points, ellipse, sectors_number):
points = points.reshape(-1, 2)
to_circle = self._transformation_ellipse_to_circle(ellipse)
points = homo_translate(to_circle, points)
center, axes, angle = (ellipse.x, ellipse.y), (ellipse.a, ellipse.b), ellipse.angle
center = homo_translate(to_circle, center)
points = points - center
return np.unique((sectors_number * (np.arctan2(points[:, 1], points[:, 0]))) // (2 * np.pi))
def create_frustum_points(self, frustums_depth=1.0):
rt_inv = np.linalg.inv(self.get_mv_matrix())
p = self.get_p_matrix()[:-1, :-1]
camera_corners = math.homo_translate(
np.linalg.inv(p),
aabb.rect_to_quad([[-1.0, -1.0 * self.aspect],
[1.0, 1.0 * self.aspect]]))
corners = np.hstack([camera_corners, [[-1]] * 4]) * frustums_depth
frustum_points = math.homo_translate(rt_inv,
np.vstack([[[0, 0, 0]], corners]))
return frustum_points
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 _determine_cursor_position(self):
x, y = self._last_mouse_xy
z = np.zeros((1, 1), np.float32)
yield from self._gl_executor.map(gl.glReadPixels, int(x), int(y), 1, 1, gl.GL_DEPTH_COMPONENT, gl.GL_FLOAT, z)
z = z[0, 0]
if z == 1.0:
return None
xy = (x - 0.5) / self._width, (y - 0.5) / self._height
xyz = np.hstack([xy, z]) * 2.0 - 1.0
mvp_mtx = self._camera.get_mvp_matrix()
xyz = math.homo_translate(np.linalg.inv(mvp_mtx), xyz)
return xyz
def _determine_cursor_position(self):
x, y = self._last_mouse_xy
z = np.zeros((1, 1), np.float32)
yield from self._gl_executor.map(gl.glReadPixels, int(x), int(y), 1, 1,
gl.GL_DEPTH_COMPONENT, gl.GL_FLOAT, z)
z = z[0, 0]
if z == 1.0:
return None
xy = (x - 0.5) / self._width, (y - 0.5) / self._height
xyz = np.hstack([xy, z]) * 2.0 - 1.0
mvp_mtx = self._camera.get_mvp_matrix()
xyz = math.homo_translate(np.linalg.inv(mvp_mtx), xyz)
return xyz
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
