# # -----------------------Rotation-----------------------------------------
# import warnings
# import numpy as np
# import re
# _AXIS_TO_IND = {'x': 0, 'y': 1, 'z': 2}
# def _elementary_basis_vector(axis):
# b = np.zeros(3)
# b[_AXIS_TO_IND[axis]] = 1
# return b
# def _compute_euler_from_dcm(dcm, seq, extrinsic=False):
#
# if extrinsic:
# seq = seq[::-1]
#
# if dcm.ndim == 2:
# dcm = dcm[None, :, :]
# num_rotations = dcm.shape[0]
#
# # Step 0
# # Algorithm assumes axes as column vectors, here we use 1D vectors
# n1 = _elementary_basis_vector(seq[0])
# n2 = _elementary_basis_vector(seq[1])
# n3 = _elementary_basis_vector(seq[2])
#
# # Step 2
# sl = np.dot(np.cross(n1, n2), n3)
# cl = np.dot(n1, n3)
#
# # angle offset is lambda from the paper referenced in [2] from docstring of
# # `as_euler` function
# offset = np.arctan2(sl, cl)
# c = np.vstack((n2, np.cross(n1, n2), n1))
#
# # Step 3
# rot = np.array([
# [1, 0, 0],
# [0, cl, sl],
# [0, -sl, cl],
# ])
# res = np.einsum('...ij,...jk->...ik', c, dcm)
# dcm_transformed = np.einsum('...ij,...jk->...ik', res, c.T.dot(rot))
#
# # Step 4
# angles = np.empty((num_rotations, 3))
# # Ensure less than unit norm
# positive_unity = dcm_transformed[:, 2, 2] > 1
# negative_unity = dcm_transformed[:, 2, 2] < -1
# dcm_transformed[positive_unity, 2, 2] = 1
# dcm_transformed[negative_unity, 2, 2] = -1
# angles[:, 1] = np.arccos(dcm_transformed[:, 2, 2])
#
# # Steps 5, 6
# eps = 1e-7
# safe1 = (np.abs(angles[:, 1]) >= eps)
# safe2 = (np.abs(angles[:, 1] - np.pi) >= eps)
#
# # Step 4 (Completion)
# angles[:, 1] += offset
#
# # 5b
# safe_mask = np.logical_and(safe1, safe2)
# angles[safe_mask, 0] = np.arctan2(dcm_transformed[safe_mask, 0, 2],
# -dcm_transformed[safe_mask, 1, 2])
# angles[safe_mask, 2] = np.arctan2(dcm_transformed[safe_mask, 2, 0],
# dcm_transformed[safe_mask, 2, 1])
#
# if extrinsic:
# # For extrinsic, set first angle to zero so that after reversal we
# # ensure that third angle is zero
# # 6a
# angles[~safe_mask, 0] = 0
# # 6b
# angles[~safe1, 2] = np.arctan2(
# dcm_transformed[~safe1, 1, 0] - dcm_transformed[~safe1, 0, 1],
# dcm_transformed[~safe1, 0, 0] + dcm_transformed[~safe1, 1, 1]
# )
# # 6c
# angles[~safe2, 2] = -np.arctan2(
# dcm_transformed[~safe2, 1, 0] + dcm_transformed[~safe2, 0, 1],
# dcm_transformed[~safe2, 0, 0] - dcm_transformed[~safe2, 1, 1]
# )
# else:
# # For instrinsic, set third angle to zero
# # 6a
# angles[~safe_mask, 2] = 0
# # 6b
# angles[~safe1, 0] = np.arctan2(
# dcm_transformed[~safe1, 1, 0] - dcm_transformed[~safe1, 0, 1],
# dcm_transformed[~safe1, 0, 0] + dcm_transformed[~safe1, 1, 1]
# )
# # 6c
# angles[~safe2, 0] = np.arctan2(
# dcm_transformed[~safe2, 1, 0] + dcm_transformed[~safe2, 0, 1],
# dcm_transformed[~safe2, 0, 0] - dcm_transformed[~safe2, 1, 1]
# )
#
# # Step 7
# if seq[0] == seq[2]:
# # lambda = 0, so we can only ensure angle2 -> [0, pi]
# adjust_mask = np.logical_or(angles[:, 1] < 0, angles[:, 1] > np.pi)
# else:
# # lambda = + or - pi/2, so we can ensure angle2 -> [-pi/2, pi/2]
# adjust_mask = np.logical_or(angles[:, 1] < -np.pi / 2,
# angles[:, 1] > np.pi / 2)
#
# # Dont adjust gimbal locked angle sequences
# adjust_mask = np.logical_and(adjust_mask, safe_mask)
#
# angles[adjust_mask, 0] += np.pi
# angles[adjust_mask, 1] = 2 * offset - angles[adjust_mask, 1]
# angles[adjust_mask, 2] -= np.pi
#
# angles[angles < -np.pi] += 2 * np.pi
# angles[angles > np.pi] -= 2 * np.pi
#
# # Step 8
# if not np.all(safe_mask):
# warnings.warn("Gimbal lock detected. Setting third angle to zero since"
# " it is not possible to uniquely determine all angles.")
#
# # Reverse role of extrinsic and intrinsic rotations, but let third angle be
# # zero for gimbal locked cases
# if extrinsic:
# angles = angles[:, ::-1]
# return angles
# def _elementary_basis_vector(axis):
# b = np.zeros(3)
# b[_AXIS_TO_IND[axis]] = 1
# return b
# class Rotation(object):
# def __init__(self, quat, normalized=False, copy=True):
# self._single = False
# quat = np.asarray(quat, dtype=float)
#
# if quat.ndim not in [1, 2] or quat.shape[-1] != 4:
# raise ValueError("Expected `quat` to have shape (4,) or (N x 4), "
# "got {}.".format(quat.shape))
#
# if quat.shape == (4,):
# quat = quat[None, :]
# self._single = True
#
# if normalized:
# self._quat = quat.copy() if copy else quat
# else:
# self._quat = quat.copy()
# norms = scipy.linalg.norm(quat, axis=1)
#
# zero_norms = norms == 0
# if zero_norms.any():
# raise ValueError("Found zero norm quaternions in `quat`.")
#
# self._quat[~zero_norms] /= norms[~zero_norms][:, None]
#
# def __len__(self):
# return self._quat.shape[0]
#
# @classmethod
# def from_dcm(cls,dcm):
# is_single = False
# dcm = np.asarray(dcm, dtype=float)
#
# if dcm.ndim not in [2, 3] or dcm.shape[-2:] != (3, 3):
# raise ValueError("Expected `dcm` to have shape (3, 3) or "
# "(N, 3, 3), got {}".format(dcm.shape))
#
# # If a single dcm is given, convert it to 3D 1 x 3 x 3 matrix but set
# # self._single to True so that we can return appropriate objects in
# # the `to_...` methods
# if dcm.shape == (3, 3):
# dcm = dcm.reshape((1, 3, 3))
# is_single = True
#
# num_rotations = dcm.shape[0]
#
# decision_matrix = np.empty((num_rotations, 4))
# decision_matrix[:, :3] = dcm.diagonal(axis1=1, axis2=2)
# decision_matrix[:, -1] = decision_matrix[:, :3].sum(axis=1)
# choices = decision_matrix.argmax(axis=1)
#
# quat = np.empty((num_rotations, 4))
#
# ind = np.nonzero(choices != 3)[0]
# i = choices[ind]
# j = (i + 1) % 3
# k = (j + 1) % 3
#
# quat[ind, i] = 1 - decision_matrix[ind, -1] + 2 * dcm[ind, i, i]
# quat[ind, j] = dcm[ind, j, i] + dcm[ind, i, j]
# quat[ind, k] = dcm[ind, k, i] + dcm[ind, i, k]
# quat[ind, 3] = dcm[ind, k, j] - dcm[ind, j, k]
#
# ind = np.nonzero(choices == 3)[0]
# quat[ind, 0] = dcm[ind, 2, 1] - dcm[ind, 1, 2]
# quat[ind, 1] = dcm[ind, 0, 2] - dcm[ind, 2, 0]
# quat[ind, 2] = dcm[ind, 1, 0] - dcm[ind, 0, 1]
# quat[ind, 3] = 1 + decision_matrix[ind, -1]
#
# quat /= np.linalg.norm(quat, axis=1)[:, None]
#
# if is_single:
# return cls(quat[0], normalized=True, copy=False)
# else:
# return cls(quat, normalized=True, copy=False)
#
# def as_dcm(self):
# x = self._quat[:, 0]
# y = self._quat[:, 1]
# z = self._quat[:, 2]
# w = self._quat[:, 3]
#
# x2 = x * x
# y2 = y * y
# z2 = z * z
# w2 = w * w
#
# xy = x * y
# zw = z * w
# xz = x * z
# yw = y * w
# yz = y * z
# xw = x * w
#
# num_rotations = len(self)
# dcm = np.empty((num_rotations, 3, 3))
#
# dcm[:, 0, 0] = x2 - y2 - z2 + w2
# dcm[:, 1, 0] = 2 * (xy + zw)
# dcm[:, 2, 0] = 2 * (xz - yw)
#
# dcm[:, 0, 1] = 2 * (xy - zw)
# dcm[:, 1, 1] = - x2 + y2 - z2 + w2
# dcm[:, 2, 1] = 2 * (yz + xw)
#
# dcm[:, 0, 2] = 2 * (xz + yw)
# dcm[:, 1, 2] = 2 * (yz - xw)
# dcm[:, 2, 2] = - x2 - y2 + z2 + w2
#
# if self._single:
# return dcm[0]
# else:
# return dcm
#
# def as_euler(self, seq, degrees=False):
# if len(seq) != 3:
# raise ValueError("Expected 3 axes, got {}.".format(seq))
#
# intrinsic = (re.match(r'^[XYZ]{1,3}$', seq) is not None)
# extrinsic = (re.match(r'^[xyz]{1,3}$', seq) is not None)
# if not (intrinsic or extrinsic):
# raise ValueError("Expected axes from `seq` to be from "
# "['x', 'y', 'z'] or ['X', 'Y', 'Z'], "
# "got {}".format(seq))
#
# if any(seq[i] == seq[i+1] for i in range(2)):
# raise ValueError("Expected consecutive axes to be different, "
# "got {}".format(seq))
#
# seq = seq.lower()
#
# angles = _compute_euler_from_dcm(self.as_dcm(), seq, extrinsic)
# if degrees:
# angles = np.rad2deg(angles)
#
# return angles[0] if self._single else angles
# # -----------------------Rotation-----------------------------------------
