(rmat)
return np.array([z, y, x])
Converts a 3x3 rotation matrix to ZYX euler angles.
Converts a 3x3 rotation matrix to ZYX euler angles.
Converts a 3x3 rotation matrix to ZYX euler angles .
def _rmat_to_euler_zyx(rmat):
  """Converts a 3x3 rotation matrix to ZYX euler angles."""
  if rmat[2, 0] > _POLE_LIMIT:
    logging.warning('Angle at North Pole')
    x = np.arctan2(rmat[0, 1], rmat[0, 2])
    y = -np.pi/2
    z = 0.0
    return np.array([z, y, x])

  if rmat[2, 0] < -_POLE_LIMIT:
    logging.warning('Angle at South Pole')
    x = np.arctan2(rmat[0, 1], rmat[0, 2])
    y = np.pi/2
    z = 0.0
    return np.array([z, y, x])

  x = np.arctan2(rmat[2, 1], rmat[2, 2])
  y = -np.arcsin(rmat[2, 0])
  z = np.arctan2(rmat[1, 0], rmat[0, 0])

  # order of return is the order of input
  return np.array([z, y, x])
def _rmat_to_euler_zyx ( rmat ) : if rmat [ 2 , 0 ] > _POLE_LIMIT : logging . warning ( 'Angle at North Pole' ) x = np . arctan2 ( rmat [ 0 , 1 ] , rmat [ 0 , 2 ] ) y = - np . pi / 2 z = 0.0 return np . array ( [ z , y , x ] ) if rmat [ 2 , 0 ] < - _POLE_LIMIT : logging . warning ( 'Angle at South Pole' ) x = np . arctan2 ( rmat [ 0 , 1 ] , rmat [ 0 , 2 ] ) y = np . pi / 2 z = 0.0 return np . array ( [ z , y , x ] ) x = np . arctan2 ( rmat [ 2 , 1 ] , rmat [ 2 , 2 ] ) y = - np . arcsin ( rmat [ 2 , 0 ] ) z = np . arctan2 ( rmat [ 1 , 0 ] , rmat [ 0 , 0 ] ) # order of return is the order of input return np . array ( [ z , y , x ] )