def _rot_mat_to_yzy_euler(rot_mat):
# BS x 1
euler_angles_1 = _ivy.acos(rot_mat[..., 1, 1:2])
gimbal_validity = _ivy.abs(rot_mat[..., 1, 0:1]) > GIMBAL_TOL
r31 = rot_mat[..., 2, 0:1]
r33 = rot_mat[..., 2, 2:3]
gimbal_euler_angles_0 = _ivy.atan2(-r31, r33)
gimbal_euler_angles_2 = _ivy.zeros_like(gimbal_euler_angles_0)
# BS x 3
gimbal_euler_angles = _ivy.concatenate(
(gimbal_euler_angles_0, euler_angles_1, gimbal_euler_angles_2), -1)
# BS x 1
r23 = rot_mat[..., 1, 2:3]
r21 = rot_mat[..., 1, 0:1]
r32 = rot_mat[..., 2, 1:2]
r12 = rot_mat[..., 0, 1:2]
normal_euler_angles_0 = _ivy.atan2(r23, r21)
normal_euler_angles_2 = _ivy.atan2(r32, r12)
# BS x 3
normal_euler_angles = _ivy.concatenate(
(normal_euler_angles_0, euler_angles_1, normal_euler_angles_2), -1)
return _ivy.where(gimbal_validity, normal_euler_angles,
gimbal_euler_angles)
def _rot_mat_to_zxz_euler(rot_mat):
# BS x 1
euler_angles_1 = _ivy.acos(rot_mat[..., 2, 2:3])
gimbal_validity = _ivy.abs(rot_mat[..., 0, 2:3]) > GIMBAL_TOL
r12 = rot_mat[..., 0, 1:2]
r11 = rot_mat[..., 0, 0:1]
gimbal_euler_angles_0 = _ivy.atan2(-r12, r11)
gimbal_euler_angles_2 = _ivy.zeros_like(gimbal_euler_angles_0)
# BS x 3
gimbal_euler_angles = _ivy.concatenate(
(gimbal_euler_angles_0, euler_angles_1, gimbal_euler_angles_2), -1)
# BS x 1
r31 = rot_mat[..., 2, 0:1]
r32 = rot_mat[..., 2, 1:2]
r13 = rot_mat[..., 0, 2:3]
r23 = rot_mat[..., 1, 2:3]
normal_euler_angles_0 = _ivy.atan2(r31, r32)
normal_euler_angles_2 = _ivy.atan2(r13, -r23)
# BS x 3
normal_euler_angles = _ivy.concatenate(
(normal_euler_angles_0, euler_angles_1, normal_euler_angles_2), -1)
return _ivy.where(gimbal_validity, normal_euler_angles,
gimbal_euler_angles)
def _rot_mat_to_zyx_euler(rot_mat):
# BS x 1
euler_angles_1 = _ivy.asin(rot_mat[..., 0, 2:3])
gimbal_validity = _ivy.abs(rot_mat[..., 1, 1:2]) > GIMBAL_TOL
r21 = rot_mat[..., 1, 0:1]
r22 = rot_mat[..., 1, 1:2]
gimbal_euler_angles_0 = _ivy.atan2(r21, r22)
gimbal_euler_angles_2 = _ivy.zeros_like(gimbal_euler_angles_0)
# BS x 3
gimbal_euler_angles = _ivy.concatenate(
(gimbal_euler_angles_0, euler_angles_1, gimbal_euler_angles_2), -1)
# BS x 1
r12 = rot_mat[..., 0, 1:2]
r11 = rot_mat[..., 0, 0:1]
r23 = rot_mat[..., 1, 2:3]
r33 = rot_mat[..., 2, 2:3]
normal_euler_angles_0 = _ivy.atan2(-r12, r11)
normal_euler_angles_2 = _ivy.atan2(-r23, r33)
# BS x 3
normal_euler_angles = _ivy.concatenate(
(normal_euler_angles_0, euler_angles_1, normal_euler_angles_2), -1)
return _ivy.where(gimbal_validity, normal_euler_angles,
gimbal_euler_angles)
def test_atan2(x1_n_x2, dtype_str, tensor_fn, dev_str, call):
# smoke test
x1, x2 = x1_n_x2
x1 = tensor_fn(x1, dtype_str, dev_str)
x2 = tensor_fn(x2, dtype_str, dev_str)
ret = ivy.atan2(x1, x2)
# type test
assert ivy.is_array(ret)
# cardinality test
assert ret.shape == x1.shape
# value test
assert np.allclose(call(ivy.atan2, x1, x2),
ivy.numpy.atan2(ivy.to_numpy(x1), ivy.to_numpy(x2)))
# compilation test
helpers.assert_compilable(ivy.atan2)
def cartesian_to_polar_coords(cartesian_coords):
"""
Convert cartesian co-ordinates :math:`\mathbf{x}_c = [x, y, z]` to spherical polar co-ordinates
:math:`\mathbf{x}_p = [r, α, β]`.\n
`[reference] <https://en.wikipedia.org/wiki/Spherical_coordinate_system#Cartesian_coordinates>`_
:param cartesian_coords: Cartesian co-ordinates *[batch_shape,3]*
:type cartesian_coords: array
:return: Spherical polar co-ordinates *[batch_shape,3]*
"""
# BS x 1
x = cartesian_coords[..., 0:1]
y = cartesian_coords[..., 1:2]
z = cartesian_coords[..., 2:3]
r = (x**2 + y**2 + z**2)**0.5
phi = _ivy.atan2(y, x)
theta = _ivy.acos(z / (r + MIN_DENOMINATOR))
# BS x 3
return _ivy.concatenate((phi, theta, r), -1)
