def update(self, dt):
# calculate the translation desired, apply rotation to it so that the translation
# is applied from the camera points of view then apply it to the camera position
translation = np.array([0, 0, 0, 1], dtype=np.float)
if self.moving_right:
translation[0] += dt
if self.moving_left:
translation[0] -= dt
if self.moving_up:
translation[1] += dt
if self.moving_down:
translation[1] -= dt
if self.moving_forward:
translation[2] += dt / 10
if self.moving_backward:
translation[2] -= dt / 10
translation *= 500 # FIXME: define speed somewhere
rotation = np.eye(4, dtype=np.float)
glm.yrotate(rotation, self._yaw)
glm.xrotate(rotation, self._pitch)
glm.zrotate(rotation, self._roll)
translation = np.matmul(rotation, translation)
self._position += translation[:3]
if self.turning_left:
self._yaw -= 60 * dt
if self.turning_right:
self._yaw += 60 * dt
def get_view(self):
view = np.eye(4, dtype=np.float)
glm.translate(
view, *(-self._position
)) # shift the world so that the camera is at the origin
glm.yrotate(view,
self._yaw) # rotate the world so that it faces the camera
glm.xrotate(view, self._pitch)
glm.zrotate(view, self._roll)
glm.scale(view, 1, -1, -1)
return view
def on_draw(dt):
nonlocal phi, theta, omega, dx, dy, dz, scale, d
model = np.eye(4, 4, dtype=np.float32)
glm.xrotate(model, omega)
glm.yrotate(model, phi)
glm.zrotate(model, theta)
glm.scale(model, scale, scale, scale)
glm.translate(model, dx, dy, dz)
cube['model'] = model
cube['color'] = color
window.clear()
cube.draw(gl.GL_TRIANGLES, I)
def on_draw(dt):
nonlocal phi, theta, omega, dx, dy, dz, scale, d
matrix = np.eye(4, 4, dtype=np.float32)
glm.xrotate(matrix, omega)
glm.yrotate(matrix, phi)
glm.zrotate(matrix, theta)
glm.scale(matrix, scale, scale, scale)
glm.translate(matrix, dx, dy, dz)
result = [np.matmul([*p, 1], matrix)[0:3] for p in V]
cube['position'] = [project(*p, d) for p in result]
window.clear()
cube.draw(gl.GL_LINE_LOOP, I)
def render(self, dt):
self.window.activate()
self.window.clear()
if self.iMat is not None:
self.iMat = np.eye(4, dtype=np.float32)
if not self.gimbal:
glm.xrotate(self.iMat, self.horizontal_angle)
glm.yrotate(self.iMat, self.vertical_angle)
else:
self.position = [0., 0., self.params["distance"]]
glm.translate(self.iMat, *self.position)
if self.gimbal:
glm.xrotate(self.iMat, self.params["horizontal_angle"])
glm.yrotate(self.iMat, self.params["vertical_angle"])
self.params["iMat"] = self.iMat
for p in self.program_params:
self.program[p] = self.params[p]
dt = dt / self.fps
if self.iTime:
self.program["iTime"] = dt
try:
self.program.draw(gl.GL_TRIANGLE_STRIP)
if self.old_program:
self.old_program.delete()
del self.old_program
self.old_program = None
print("Loaded new program!")
except RuntimeError:
if not self.old_program:
raise
self.old_program.draw(gl.GL_TRIANGLE_STRIP)
self.program.delete()
del self.program
self.program = self.old_program
self.old_program = None
self.paused = True
if self.title == "Map":
gl.glEnable(gl.GL_BLEND)
gl.glBlendFunc(gl.GL_ONE_MINUS_DST_COLOR, gl.GL_ZERO)
self.point_program.draw(gl.GL_POINTS)
self.prev_params = copy.deepcopy(self.params)
def run(args):
# Command line arguments
address = args.address
show_window = args.show_window
poll_timeout = args.poll_timeout
width = args.width
height = args.height
mesh_filename = args.mesh_filename
use_msgpack_for_mesh = args.use_msgpack_for_mesh
window_width = args.window_width
if window_width is None:
window_width = width
window_height = args.window_height
if window_height is None:
window_height = height
window_pos_x = args.window_pos_x
window_pos_y = args.window_pos_y
model_scale = args.model_scale
model_rpy = [args.model_roll, args.model_pitch, args.model_yaw]
depth_scale = args.depth_scale
yaw_speed = args.yaw_speed
pitch_speed = args.pitch_speed
# Compute vertical field of view (needed for glm.perspective)
horz_fov = args.horz_fov
focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(horz_fov), width)
vert_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(focal_length, height))
logger.info("Horizontal FOV: {:.4f}, vertical FOV: {:.4f}".format(
horz_fov, vert_fov))
window_focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(horz_fov), window_width)
window_vert_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(window_focal_length, window_height))
logger.info("Window vertical FOV: {:.4f}".format(window_vert_fov))
# Fixed parameters
initial_distance = 10 # initial camera distance
# Glumpy initialization
# glumpy.app.use("glfw")
# glumpy.app.use("glfw", 'ES', 3, 3)
glumpy_config = glumpy.app.configuration.get_default()
glumpy_config.double_buffer = True
glumpy_config.samples = 0
glumpy_config.api = "ES"
glumpy_config.depth_size = 32
glumpy_config.major_version = 3
glumpy_config.minor_version = 1
glumpy_config.profile = "core"
# if show_window:
window = Window(width=window_width,
height=window_height,
fov=window_vert_fov,
znear=0.5,
zfar=1000,
config=glumpy_config,
visible=show_window,
title=b"mesh_renderer_zmq")
trackball = Trackball(pitch=0, yaw=0)
window.glumpy_window.attach(trackball)
app = Application()
logger.info("Glumpy backend: {}".format(glumpy.app.__backend__))
# Process initial events
if window_pos_x is not None or window_pos_y is not None:
if window_pos_x is None:
window_pos_x, _ = window.glumpy_window.get_position()
if window_pos_y is None:
_, window_pos_y = window.glumpy_window.get_position()
window.glumpy_window.set_position(window_pos_x, window_pos_y)
# Offscreen surface initialization
framebuffer = Framebuffer(width,
height,
num_color_attachments=3,
color_dtypes=[np.ubyte, np.float32, np.float32])
fb_color_drawer = FramebufferDrawer(framebuffer,
color_index=0,
draw_mode=FramebufferDrawer.BLIT)
fb_depth_drawer = FramebufferDrawer(framebuffer,
color_index=1,
draw_mode=FramebufferDrawer.DRAW_QUAD)
fb_depth_drawer.set_color_scale(depth_scale)
fb_normal_drawer = FramebufferDrawer(framebuffer,
color_index=2,
draw_mode=FramebufferDrawer.DRAW_QUAD)
fb_normal_drawer.set_color_scale(0.5)
fb_normal_drawer.set_color_offset(0.5)
# Offscreen surface initialization
window_framebuffer = Framebuffer(
window_width,
window_height,
num_color_attachments=3,
color_dtypes=[np.ubyte, np.float32, np.float32])
window_fb_color_drawer = FramebufferDrawer(
window_framebuffer, color_index=0, draw_mode=FramebufferDrawer.BLIT)
window_fb_depth_drawer = FramebufferDrawer(
window_framebuffer,
color_index=1,
draw_mode=FramebufferDrawer.DRAW_QUAD)
window_fb_depth_drawer.set_color_scale(depth_scale)
window_fb_normal_drawer = FramebufferDrawer(
window_framebuffer,
color_index=2,
draw_mode=FramebufferDrawer.DRAW_QUAD)
window_fb_normal_drawer.set_color_scale(0.5)
window_fb_normal_drawer.set_color_offset(0.5)
# Add framebuffer draw for normals
window.add_drawer(window_fb_normal_drawer)
# Transformation from world coordinates (z up, x foward, y left) to opengl coordinates (z backward, x right, y up)
world_to_opengl_mat = np.eye(4, dtype=np.float32)
world_to_opengl_mat = glm.xrotate(world_to_opengl_mat, -90)
world_to_opengl_mat = glm.zrotate(world_to_opengl_mat, 90)
opengl_to_world_mat = np.linalg.inv(world_to_opengl_mat)
# Mesh loading
if mesh_filename is not None:
mesh = None
if use_msgpack_for_mesh:
msgpack_mesh_filename = mesh_filename + ".msgpack"
if os.path.isfile(msgpack_mesh_filename):
logger.info("Loading mesh from msgpack file {}".format(
msgpack_mesh_filename))
mesh = SimpleMesh.read_from_msgpack(msgpack_mesh_filename)
if mesh is None:
logger.info("Loading mesh from file {}".format(mesh_filename))
mesh = SimpleMesh.read_from_file(mesh_filename)
if use_msgpack_for_mesh:
logger.info("Saving mesh to msgpack file {}".format(
msgpack_mesh_filename))
mesh.write_to_msgpack(msgpack_mesh_filename)
else:
mesh = CubeMesh()
logger.info("Setting uniform color")
mesh.set_colors_uniform([0.5, 0.5, 0.5, 1])
# logger.info("Computing mesh colors with z-colormap")
# mesh.set_colors_with_coordinate_colormap(min_coord=0, max_coord=10)
# Drawer initialization
# cube_mesh = CubeMesh()
# cube_drawer = MeshDrawer(cube_mesh)
# cube_drawer.depth_scale = 0.1
# cube_drawer.normal_scale = 0.1
# mesh = SimpleMesh.read_from_ply(sys.argv[1])
# mesh.write_to_pickle("mesh.pickle")
# mesh = SimpleMesh.read_from_pickle("mesh2.pickle")
# mesh = Mesh.from_file(filename)
# mesh_drawer = MeshDrawer(mesh)
mesh_drawer = MeshDrawer(mesh)
mesh_drawer.transform.scale = model_scale
mesh_drawer.transform.orientation_rpy = model_rpy
# Assume mesh is in world coordinate system
# mesh_drawer.model = np.matmul(np.transpose(world_to_opengl_mat), mesh_drawer.model)
# mesh_drawer.color_scale = color_scale
# mesh_drawer.normal_scale = normal_scale
# mesh_drawer.depth_scale = depth_scale
# glm.scale(mesh_drawer.model, 0.01)
# cube_drawer = CubeDrawer()
# Server initialization
logger.info("Starting ZMQ server on address {}".format(address))
server_conn = zmq_utils.Connection(address, zmq_utils.zmq.REP)
server_conn.bind()
renderer_service = renderer_zmq_service.RendererZMQService(
framebuffer, mesh_drawer, vert_fov, initial_distance, trackball,
world_to_opengl_mat)
global input_enabled
global override_renderer_service_transform
global window_enabled
input_enabled = args.input_enabled
window_enabled = window.visible
override_renderer_service_transform = False
def window_visible_callback(visible):
if visible:
logger.info("Showing window")
window.show()
else:
logger.info("Hiding window")
window.hide()
def window_active_callback(active):
global window_enabled
if active:
logger.info("Activating window")
else:
logger.info("Deactivating window")
window_enabled = active
renderer_service.window_visible_callbacks.register(window_visible_callback)
renderer_service.window_active_callbacks.register(window_active_callback)
# Some keyboard input handling
@window.glumpy_window.event
def on_key_press(symbol, modifiers):
try:
character = chr(symbol).lower()
except ValueError as err:
character = ""
logger.debug("Key pressed: {}, modifiers: {}, character: {}".format(
symbol, modifiers, character))
if character == 'n':
logger.info("Showing normals")
window.clear_drawers()
window.add_drawer(window_fb_normal_drawer)
elif character == 'c':
logger.info("Showing colors")
window.clear_drawers()
window.add_drawer(window_fb_color_drawer)
elif character == 'd':
logger.info("Showing depth")
window.clear_drawers()
window.add_drawer(window_fb_depth_drawer)
elif character == 'i':
global input_enabled
if input_enabled:
logger.info("Disabling input")
input_enabled = False
else:
logger.info("Enabling input")
input_enabled = True
elif character == 'a':
global window_enabled
if window_enabled:
logger.info("Disabling window")
window_enabled = False
else:
logger.info("Enabling window")
window_enabled = True
elif character == 'o':
global override_renderer_service_transform
if override_renderer_service_transform:
logger.info("Not overriding renderer service transform input")
override_renderer_service_transform = False
else:
logger.info("Overriding renderer service transform input")
override_renderer_service_transform = True
elif character == 'p':
logger.info("Current location: {}".format(trackball.location))
logger.info("Current world location: {}".format(
np.dot(opengl_to_world_mat[:3, :3],
trackball.camera_location)))
logger.info("Current orientation_rpy: {}".format(
trackball.orientation_rpy))
timer = utils.RateTimer(reset_interval=500)
while True:
dt = app.clock.tick()
# Handle network requests
request_dump = server_conn.recv(timeout=poll_timeout * 1000)
if request_dump is not None:
response_dump = renderer_service.handle_request_msg(
request_dump, raise_exception=True)
if response_dump is not None:
server_conn.send(response_dump)
# Perform any drawer processing
mesh_drawer.tick(dt)
# Animate model if desired
if yaw_speed != 0:
mesh_drawer.transform.yaw += yaw_speed * dt
if pitch_speed != 0:
mesh_drawer.transform.pitch += pitch_speed * dt
if override_renderer_service_transform:
renderer_service.view_transform = trackball.transform.copy()
if window.visible and window_enabled:
if input_enabled and renderer_service.input_enabled:
view_transform = trackball.transform
else:
view_transform = renderer_service.view_transform
with window_framebuffer.activate(clear=True):
mesh_drawer.draw(window.projection, view_transform.matrix,
window.width, window.height)
# For debugging
# color_pixels = framebuffer.read_rgba_pixels()
# depth_pixels = framebuffer.read_rgba_pixels(color_index=1)
# import cv2
# cv2.imshow("color", color_pixels)
# cv2.imshow("depth", depth_pixels[:, :, 0])
# cv2.waitKey(50)
window.override_view(trackball.view)
app.process(dt)
timer.update_and_print_rate(print_interval=100)
def _apply_rotation(self):
glm.yrotate(self._matrix, self._yaw)
glm.xrotate(self._matrix, -self._pitch)
glm.zrotate(self._matrix, -self._roll)