"""Trackball class for 3D manipulation of viewpoints. """ import numpy as np import trimesh.transformations as transformations class Trackball(object): """A trackball class for creating camera transforms from mouse movements. """ STATE_ROTATE = 0 STATE_PAN = 1 STATE_ROLL = 2 STATE_ZOOM = 3 def __init__(self, pose, size, scale, target=np.array([0.0, 0.0, 0.0])): """Initialize a trackball with an initial camera-to-world pose and the given parameters. Parameters ---------- pose : [4,4] An initial camera-to-world pose for the trackball. size : (float, float) The width and height of the camera image in pixels. scale : float The diagonal of the scene's bounding box -- used for ensuring translation motions are sufficiently fast for differently-sized scenes. target : (3,) float The center of the scene in world coordinates. The trackball will revolve around this point. """ self._size = np.array(size) self._scale = float(scale) self._pose = pose self._n_pose = pose self._target = target self._n_target = target self._state = Trackball.STATE_ROTATE @property def pose(self): """autolab_core.RigidTransform : The current camera-to-world pose. """ return self._n_pose def set_state(self, state): """Set the state of the trackball in order to change the effect of dragging motions. Parameters ---------- state : int One of Trackball.STATE_ROTATE, Trackball.STATE_PAN, Trackball.STATE_ROLL, and Trackball.STATE_ZOOM. """ self._state = state def resize(self, size): """Resize the window. Parameters ---------- size : (float, float) The new width and height of the camera image in pixels. """ self._size = np.array(size) def down(self, point): """Record an initial mouse press at a given point. Parameters ---------- point : (2,) int The x and y pixel coordinates of the mouse press. """ self._pdown = np.array(point, dtype=np.float32) self._pose = self._n_pose self._target = self._n_target def drag(self, point): """Update the tracball during a drag. Parameters ---------- point : (2,) int The current x and y pixel coordinates of the mouse during a drag. This will compute a movement for the trackball with the relative motion between this point and the one marked by down(). """ point = np.array(point, dtype=np.float32) dx, dy = point - self._pdown mindim = 0.3 * np.min(self._size) target = self._target x_axis = self._pose[:3,0].flatten() y_axis = self._pose[:3,1].flatten() z_axis = self._pose[:3,2].flatten() eye = self._pose[:3,3].flatten() # Interpret drag as a rotation if self._state == Trackball.STATE_ROTATE: x_angle = -dx / mindim x_rot_mat = transformations.rotation_matrix( x_angle, y_axis, target ) y_angle = dy / mindim y_rot_mat = transformations.rotation_matrix( y_angle, x_axis, target ) self._n_pose = y_rot_mat.dot(x_rot_mat.dot(self._pose)) # Interpret drag as a roll about the camera axis elif self._state == Trackball.STATE_ROLL: center = self._size / 2.0 v_init = self._pdown - center v_curr = point - center v_init = v_init / np.linalg.norm(v_init) v_curr = v_curr / np.linalg.norm(v_curr) theta = (-np.arctan2(v_curr[1], v_curr[0]) + np.arctan2(v_init[1], v_init[0])) rot_mat = transformations.rotation_matrix(theta, z_axis, target) self._n_pose = rot_mat.dot(self._pose) # Interpret drag as a camera pan in view plane elif self._state == Trackball.STATE_PAN: dx = -dx / (5.0 * mindim) * self._scale dy = -dy / (5.0 * mindim) * self._scale translation = dx * x_axis + dy * y_axis self._n_target = self._target + translation t_tf = np.eye(4) t_tf[:3,3] = translation self._n_pose = t_tf.dot(self._pose) # Interpret drag as a zoom motion elif self._state == Trackball.STATE_ZOOM: radius = np.linalg.norm(eye - target) ratio = 0.0 if dy > 0: ratio = np.exp(abs(dy) / (0.5 * self._size[1])) - 1.0 elif dy < 0: ratio = 1.0 - np.exp(dy / (0.5 * (self._size[1]))) translation = -np.sign(dy) * ratio * radius * z_axis t_tf = np.eye(4) t_tf[:3,3] = translation self._n_pose = t_tf.dot(self._pose) def scroll(self, clicks): """Zoom using a mouse scroll wheel motion. Parameters ---------- clicks : int The number of clicks. Positive numbers indicate forward wheel movement. """ target = self._target ratio = 0.90 mult = 1.0 if clicks > 0: mult = ratio**clicks elif clicks < 0: mult = (1.0 / ratio)**abs(clicks) z_axis = self._n_pose[:3,2].flatten() eye = self._n_pose[:3,3].flatten() radius = np.linalg.norm(eye - target) translation = (mult * radius - radius) * z_axis t_tf = np.eye(4) t_tf[:3,3] = translation self._n_pose = t_tf.dot(self._n_pose) z_axis = self._pose[:3,2].flatten() eye = self._pose[:3,3].flatten() radius = np.linalg.norm(eye - target) translation = (mult * radius - radius) * z_axis t_tf = np.eye(4) t_tf[:3,3] = translation self._pose = t_tf.dot(self._pose) def rotate(self, azimuth, axis=None): """Rotate the trackball about the "Up" axis by azimuth radians. Parameters ---------- azimuth : float The number of radians to rotate. """ target = self._target y_axis = self._n_pose[:3,1].flatten() if axis is not None: y_axis = axis x_rot_mat = transformations.rotation_matrix(azimuth, y_axis, target) self._n_pose = x_rot_mat.dot(self._n_pose) y_axis = self._pose[:3,1].flatten() if axis is not None: y_axis = axis x_rot_mat = transformations.rotation_matrix(azimuth, y_axis, target) self._pose = x_rot_mat.dot(self._pose)