예제 #1
0
    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)
예제 #2
0
    def draw(self, quaternion=(0, 0, 0, 1), edge_colour=(1, 1, 1)):
        """
        Draw the shape.

        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, as RGB values between 0 and 1.
            Default is white.

        """
        with new_matrix():
            gl.glRotate(*Quat.to_axis(quaternion))
            self._draw_components(self.vertices, self.colours, self.edges,
                                  self.surfaces, edge_colour)
예제 #3
0
    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)