(quat)
return np.array(
      ((1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0),
       (q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0),
       (q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0),
       (0.0, 0.0, 0.0, 1.0)),
      dtype=np.float64)
Return homogeneous rotation matrix from quaternion.

  Args:
    quat: A quaternion [w, i, j, k].

  Returns:
    A 4x4 homogeneous matrix with the rotation corresponding to `quat`.
Return homogeneous rotation matrix from quaternion.
Return homogeneous rotation matrix from quaternion .
def quat_to_mat(quat):
  """Return homogeneous rotation matrix from quaternion.

  Args:
    quat: A quaternion [w, i, j, k].

  Returns:
    A 4x4 homogeneous matrix with the rotation corresponding to `quat`.
  """
  q = np.array(quat, dtype=np.float64, copy=True)
  nq = np.dot(q, q)
  if nq < _TOL:
    return np.identity(4)
  q *= np.sqrt(2.0 / nq)
  q = np.outer(q, q)
  return np.array(
      ((1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0),
       (q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0),
       (q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0),
       (0.0, 0.0, 0.0, 1.0)),
      dtype=np.float64)
def quat_to_mat ( quat ) : q = np . array ( quat , dtype = np . float64 , copy = True ) nq = np . dot ( q , q ) if nq < _TOL : return np . identity ( 4 ) q *= np . sqrt ( 2.0 / nq ) q = np . outer ( q , q ) return np . array ( ( ( 1.0 - q [ 2 , 2 ] - q [ 3 , 3 ] , q [ 1 , 2 ] - q [ 3 , 0 ] , q [ 1 , 3 ] + q [ 2 , 0 ] , 0.0 ) , ( q [ 1 , 2 ] + q [ 3 , 0 ] , 1.0 - q [ 1 , 1 ] - q [ 3 , 3 ] , q [ 2 , 3 ] - q [ 1 , 0 ] , 0.0 ) , ( q [ 1 , 3 ] - q [ 2 , 0 ] , q [ 2 , 3 ] + q [ 1 , 0 ] , 1.0 - q [ 1 , 1 ] - q [ 2 , 2 ] , 0.0 ) , ( 0.0 , 0.0 , 0.0 , 1.0 ) ) , dtype = np . float64 )