(mat)
return q
Return quaternion from homogeneous or rotation matrix.

  Args:
    mat: A homogeneous transform or rotation matrix

  Returns:
    A quaternion [w, i, j, k].
Return quaternion from homogeneous or rotation matrix.
Return quaternion from homogeneous or rotation matrix .
def mat_to_quat(mat):
  """Return quaternion from homogeneous or rotation matrix.

  Args:
    mat: A homogeneous transform or rotation matrix

  Returns:
    A quaternion [w, i, j, k].
  """
  if mat.shape == (3, 3):
    tmp = np.eye(4)
    tmp[0:3, 0:3] = mat
    mat = tmp

  q = np.empty((4,), dtype=np.float64)
  t = np.trace(mat)
  if t > mat[3, 3]:
    q[0] = t
    q[3] = mat[1, 0] - mat[0, 1]
    q[2] = mat[0, 2] - mat[2, 0]
    q[1] = mat[2, 1] - mat[1, 2]
  else:
    i, j, k = 0, 1, 2
    if mat[1, 1] > mat[0, 0]:
      i, j, k = 1, 2, 0
    if mat[2, 2] > mat[i, i]:
      i, j, k = 2, 0, 1
    t = mat[i, i] - (mat[j, j] + mat[k, k]) + mat[3, 3]
    q[i + 1] = t
    q[j + 1] = mat[i, j] + mat[j, i]
    q[k + 1] = mat[k, i] + mat[i, k]
    q[0] = mat[k, j] - mat[j, k]
  q *= 0.5 / np.sqrt(t * mat[3, 3])
  return q
def mat_to_quat ( mat ) : if mat . shape == ( 3 , 3 ) : tmp = np . eye ( 4 ) tmp [ 0 : 3 , 0 : 3 ] = mat mat = tmp q = np . empty ( ( 4 , ) , dtype = np . float64 ) t = np . trace ( mat ) if t > mat [ 3 , 3 ] : q [ 0 ] = t q [ 3 ] = mat [ 1 , 0 ] - mat [ 0 , 1 ] q [ 2 ] = mat [ 0 , 2 ] - mat [ 2 , 0 ] q [ 1 ] = mat [ 2 , 1 ] - mat [ 1 , 2 ] else : i , j , k = 0 , 1 , 2 if mat [ 1 , 1 ] > mat [ 0 , 0 ] : i , j , k = 1 , 2 , 0 if mat [ 2 , 2 ] > mat [ i , i ] : i , j , k = 2 , 0 , 1 t = mat [ i , i ] - ( mat [ j , j ] + mat [ k , k ] ) + mat [ 3 , 3 ] q [ i + 1 ] = t q [ j + 1 ] = mat [ i , j ] + mat [ j , i ] q [ k + 1 ] = mat [ k , i ] + mat [ i , k ] q [ 0 ] = mat [ k , j ] - mat [ j , k ] q *= 0.5 / np . sqrt ( t * mat [ 3 , 3 ] ) return q