def tait_bryan_angles_from_quaternion_intrinsic_zyx(self, quaternion): assert self.n == 3, ('The quaternion representation' ' and the Tait-Bryan angles representation' ' do not exist' ' for rotations in %d dimensions.' % self.n) quaternion = gs.to_ndarray(quaternion, to_ndim=2) w, x, y, z = gs.hsplit(quaternion, 4) angle_1 = gs.arctan2(y * z + w * x, 1. / 2. - (x**2 + y**2)) angle_2 = gs.arcsin(-2. * (x * z - w * y)) angle_3 = gs.arctan2(x * y + w * z, 1. / 2. - (y**2 + z**2)) tait_bryan_angles = gs.concatenate([angle_3, angle_2, angle_1], axis=1) return tait_bryan_angles
def tait_bryan_angles_from_quaternion_intrinsic_xyz(self, quaternion): assert self.n == 3, ('The quaternion representation' ' and the Tait-Bryan angles representation' ' do not exist' ' for rotations in %d dimensions.' % self.n) quaternion = gs.to_ndarray(quaternion, to_ndim=2) w, x, y, z = gs.hsplit(quaternion, 4) angle_3 = gs.arctan2(2. * (x * y + w * z), w * w + x * x - y * y - z * z) angle_2 = gs.arcsin(-2. * (x * z - w * y)) angle_1 = gs.arctan2(2. * (y * z + w * x), w * w - x * x - y * y + z * z) tait_bryan_angles = gs.concatenate([angle_1, angle_2, angle_3], axis=1) return tait_bryan_angles
def tait_bryan_angles_from_quaternion_intrinsic_zyx(quaternion): """Convert quaternion to tait bryan representation of order zyx. Parameters ---------- quaternion : array-like, shape=[..., 4] Returns ------- tait_bryan_angles : array-like, shape=[..., 3] """ w, x, y, z = gs.hsplit(quaternion, 4) angle_1 = gs.arctan2(y * z + w * x, 1. / 2. - (x**2 + y**2)) angle_2 = gs.arcsin(-2. * (x * z - w * y)) angle_3 = gs.arctan2(x * y + w * z, 1. / 2. - (y**2 + z**2)) tait_bryan_angles = gs.concatenate([angle_1, angle_2, angle_3], axis=1) return tait_bryan_angles
def matrix_from_quaternion(self, quaternion): """Convert a unit quaternion into a rotation vector. Parameters ---------- quaternion : array-like, shape=[..., 4] Returns ------- rot_mat : array-like, shape=[..., 3] """ n_quaternions, _ = quaternion.shape w, x, y, z = gs.hsplit(quaternion, 4) rot_mat = gs.zeros((n_quaternions, ) + (self.n, ) * 2) for i in range(n_quaternions): # TODO(nina): Vectorize by applying the composition of # quaternions to the identity matrix column_1 = [ w[i]**2 + x[i]**2 - y[i]**2 - z[i]**2, 2 * x[i] * y[i] - 2 * w[i] * z[i], 2 * x[i] * z[i] + 2 * w[i] * y[i] ] column_2 = [ 2 * x[i] * y[i] + 2 * w[i] * z[i], w[i]**2 - x[i]**2 + y[i]**2 - z[i]**2, 2 * y[i] * z[i] - 2 * w[i] * x[i] ] column_3 = [ 2 * x[i] * z[i] - 2 * w[i] * y[i], 2 * y[i] * z[i] + 2 * w[i] * x[i], w[i]**2 - x[i]**2 - y[i]**2 + z[i]**2 ] mask_i = gs.get_mask_i_float(i, n_quaternions) rot_mat_i = gs.transpose(gs.hstack([column_1, column_2, column_3])) rot_mat_i = gs.to_ndarray(rot_mat_i, to_ndim=3) rot_mat += gs.einsum('...,...ij->...ij', mask_i, rot_mat_i) return rot_mat
def tait_bryan_angles_from_quaternion_intrinsic_xyz(quaternion): """Convert quaternion to tait bryan representation of order xyz. Parameters ---------- quaternion : array-like, shape=[..., 4] Returns ------- tait_bryan_angles : array-like, shape=[..., 3] """ w, x, y, z = gs.hsplit(quaternion, 4) angle_1 = gs.arctan2(2. * (-x * y + w * z), w * w + x * x - y * y - z * z) angle_2 = gs.arcsin(2 * (x * z + w * y)) angle_3 = gs.arctan2(2. * (-y * z + w * x), w * w + z * z - x * x - y * y) tait_bryan_angles = gs.concatenate([angle_1, angle_2, angle_3], axis=1) return tait_bryan_angles
def matrix_from_quaternion(self, quaternion): """ Convert a unit quaternion into a rotation vector. """ assert self.n == 3, ('The quaternion representation does not exist' ' for rotations in %d dimensions.' % self.n) quaternion = gs.to_ndarray(quaternion, to_ndim=2) n_quaternions, _ = quaternion.shape a, b, c, d = gs.hsplit(quaternion, 4) rot_mat = gs.zeros((n_quaternions, ) + (self.n, ) * 2) for i in range(n_quaternions): # TODO(nina): vectorize by applying the composition of # quaternions to the identity matrix column_1 = [ a[i]**2 + b[i]**2 - c[i]**2 - d[i]**2, 2 * b[i] * c[i] - 2 * a[i] * d[i], 2 * b[i] * d[i] + 2 * a[i] * c[i] ] column_2 = [ 2 * b[i] * c[i] + 2 * a[i] * d[i], a[i]**2 - b[i]**2 + c[i]**2 - d[i]**2, 2 * c[i] * d[i] - 2 * a[i] * b[i] ] column_3 = [ 2 * b[i] * d[i] - 2 * a[i] * c[i], 2 * c[i] * d[i] + 2 * a[i] * b[i], a[i]**2 - b[i]**2 - c[i]**2 + d[i]**2 ] rot_mat[i] = gs.hstack([column_1, column_2, column_3]).transpose() assert rot_mat.ndim == 3 return rot_mat
def matrix_from_quaternion(self, quaternion): """ Convert a unit quaternion into a rotation vector. """ assert self.n == 3, ('The quaternion representation does not exist' ' for rotations in %d dimensions.' % self.n) quaternion = gs.to_ndarray(quaternion, to_ndim=2) n_quaternions, _ = quaternion.shape w, x, y, z = gs.hsplit(quaternion, 4) rot_mat = gs.zeros((n_quaternions, ) + (self.n, ) * 2) for i in range(n_quaternions): # TODO(nina): vectorize by applying the composition of # quaternions to the identity matrix column_1 = [ w[i]**2 + x[i]**2 - y[i]**2 - z[i]**2, 2 * x[i] * y[i] - 2 * w[i] * z[i], 2 * x[i] * z[i] + 2 * w[i] * y[i] ] column_2 = [ 2 * x[i] * y[i] + 2 * w[i] * z[i], w[i]**2 - x[i]**2 + y[i]**2 - z[i]**2, 2 * y[i] * z[i] - 2 * w[i] * x[i] ] column_3 = [ 2 * x[i] * z[i] - 2 * w[i] * y[i], 2 * y[i] * z[i] + 2 * w[i] * x[i], w[i]**2 - x[i]**2 - y[i]**2 + z[i]**2 ] rot_mat[i] = gs.hstack([column_1, column_2, column_3]).transpose() assert gs.ndim(rot_mat) == 3 return rot_mat