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
