def draw(self, quaternion=(0, 0, 0, 1), edge_colour=(1, 1, 1)):
"""
Draw the drone.
Parameters
----------
quaternion : Optional[Sequence[float]]
The x, y, z, and w quaternion of the pose. Default is no rotation.
edge_colour : Optional[Sequence[float]]
The colour to draw the edges in. Default is white.
"""
super(Drone, self).draw(quaternion, edge_colour)
# Draw arrow
with new_matrix():
gl.glRotate(*Quat.to_axis(quaternion))
for colour, surface in zip(self.surf_colours, self.surf_surfaces):
self._draw_components(self.vertices, colour, (), surface,
edge_colour)
for colours, edges, surfaces in zip(self.arrow_colours,
self.arrow_edges,
self.arrow_surfaces):
self._draw_components(self.arrow_vertices, colours, edges,
surfaces, edge_colour)
def render(self, pose_cam, pose_drone):
"""
Render the scene.
Parameters
----------
pose_cam : Pose
The pose of the drone when the background image was taken.
pose_drone : Pose
The current pose of the drone.
"""
rel_pos, rot_cam, rot_drone = self._find_rel_pos(pose_cam, pose_drone)
rot_cam[0] *= -1
rot_cam[1], rot_cam[2] = rot_cam[2], rot_cam[1]
rot_drone[0] *= -1
rot_drone[1], rot_drone[2] = rot_drone[2], rot_drone[1]
# Temporarily turn off zooming.
if self.distance:
scale = np.linalg.norm(rel_pos) / self.distance
rel_pos = unit_vector(rel_pos) * self.distance
else:
scale = 1
centre = self._find_drone_on_image(rel_pos)
with new_matrix():
# Set camera orientation.
rot_cam[:3] *= -1 # z-axis is with respect to origin, not camera.
gl.glRotate(*Quat.to_axis(rot_cam))
# Set camera position.
# Convert position to OpenGL coordinate frame first.
rel_pos[1], rel_pos[2] = rel_pos[2], -rel_pos[1]
gl.glTranslate(*rel_pos)
self.draw_background(scale=scale, centre=centre)
self.model.draw(rot_drone)