transformation.py

"""
From PyBullet example files.

https://github.com/bulletphysics/bullet3/master/examples/pybullet/gym/pybullet_envs/deep_mimic/mocap/transformation.py

TODO: convert to use scipy rotations module instead

"""
from __future__ import division, print_function

import math

import numpy


def identity_matrix():
  """Return 4x4 identity/unit matrix.

    >>> I = identity_matrix()
    >>> numpy.allclose(I, numpy.dot(I, I))
    True
    >>> numpy.sum(I), numpy.trace(I)
    (4.0, 4.0)
    >>> numpy.allclose(I, numpy.identity(4))
    True

    """
  return numpy.identity(4)


def translation_matrix(direction):
  """Return matrix to translate by direction vector.

    >>> v = numpy.random.random(3) - 0.5
    >>> numpy.allclose(v, translation_matrix(v)[:3, 3])
    True

    """
  M = numpy.identity(4)
  M[:3, 3] = direction[:3]
  return M


def translation_from_matrix(matrix):
  """Return translation vector from translation matrix.

    >>> v0 = numpy.random.random(3) - 0.5
    >>> v1 = translation_from_matrix(translation_matrix(v0))
    >>> numpy.allclose(v0, v1)
    True

    """
  return numpy.array(matrix, copy=False)[:3, 3].copy()


def reflection_matrix(point, normal):
  """Return matrix to mirror at plane defined by point and normal vector.

    >>> v0 = numpy.random.random(4) - 0.5
    >>> v0[3] = 1.
    >>> v1 = numpy.random.random(3) - 0.5
    >>> R = reflection_matrix(v0, v1)
    >>> numpy.allclose(2, numpy.trace(R))
    True
    >>> numpy.allclose(v0, numpy.dot(R, v0))
    True
    >>> v2 = v0.copy()
    >>> v2[:3] += v1
    >>> v3 = v0.copy()
    >>> v2[:3] -= v1
    >>> numpy.allclose(v2, numpy.dot(R, v3))
    True

    """
  normal = unit_vector(normal[:3])
  M = numpy.identity(4)
  M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
  M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
  return M


def reflection_from_matrix(matrix):
  """Return mirror plane point and normal vector from reflection matrix.

    >>> v0 = numpy.random.random(3) - 0.5
    >>> v1 = numpy.random.random(3) - 0.5
    >>> M0 = reflection_matrix(v0, v1)
    >>> point, normal = reflection_from_matrix(M0)
    >>> M1 = reflection_matrix(point, normal)
    >>> is_same_transform(M0, M1)
    True

    """
  M = numpy.array(matrix, dtype=numpy.float64, copy=False)
  # normal: unit eigenvector corresponding to eigenvalue -1
  w, V = numpy.linalg.eig(M[:3, :3])
  i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0]
  if not len(i):
    raise ValueError('no unit eigenvector corresponding to eigenvalue -1')
  normal = numpy.real(V[:, i[0]]).squeeze()
  # point: any unit eigenvector corresponding to eigenvalue 1
  w, V = numpy.linalg.eig(M)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
  if not len(i):
    raise ValueError('no unit eigenvector corresponding to eigenvalue 1')
  point = numpy.real(V[:, i[-1]]).squeeze()
  point /= point[3]
  return point, normal


def rotation_matrix(angle, direction, point=None):
  """Return matrix to rotate about axis defined by point and direction.

    >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0])
    >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])
    True
    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
    >>> is_same_transform(R0, R1)
    True
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> R1 = rotation_matrix(-angle, -direc, point)
    >>> is_same_transform(R0, R1)
    True
    >>> I = numpy.identity(4, numpy.float64)
    >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
    True
    >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2,
    ...                                               direc, point)))
    True

    """
  sina = math.sin(angle)
  cosa = math.cos(angle)
  direction = unit_vector(direction[:3])
  # rotation matrix around unit vector
  R = numpy.diag([cosa, cosa, cosa])
  R += numpy.outer(direction, direction) * (1.0 - cosa)
  direction *= sina
  R += numpy.array([[0.0, -direction[2], direction[1]], [direction[2], 0.0, -direction[0]],
                    [-direction[1], direction[0], 0.0]])
  M = numpy.identity(4)
  M[:3, :3] = R
  if point is not None:
    # rotation not around origin
    point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
    M[:3, 3] = point - numpy.dot(R, point)
  return M


def rotation_from_matrix(matrix):
  """Return rotation angle and axis from rotation matrix.

    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> angle, direc, point = rotation_from_matrix(R0)
    >>> R1 = rotation_matrix(angle, direc, point)
    >>> is_same_transform(R0, R1)
    True

    """
  R = numpy.array(matrix, dtype=numpy.float64, copy=False)
  R33 = R[:3, :3]
  # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
  w, W = numpy.linalg.eig(R33.T)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
  if not len(i):
    raise ValueError('no unit eigenvector corresponding to eigenvalue 1')
  direction = numpy.real(W[:, i[-1]]).squeeze()
  # point: unit eigenvector of R33 corresponding to eigenvalue of 1
  w, Q = numpy.linalg.eig(R)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
  if not len(i):
    raise ValueError('no unit eigenvector corresponding to eigenvalue 1')
  point = numpy.real(Q[:, i[-1]]).squeeze()
  point /= point[3]
  # rotation angle depending on direction
  cosa = (numpy.trace(R33) - 1.0) / 2.0
  if abs(direction[2]) > 1e-8:
    sina = (R[1, 0] + (cosa - 1.0) * direction[0] * direction[1]) / direction[2]
  elif abs(direction[1]) > 1e-8:
    sina = (R[0, 2] + (cosa - 1.0) * direction[0] * direction[2]) / direction[1]
  else:
    sina = (R[2, 1] + (cosa - 1.0) * direction[1] * direction[2]) / direction[0]
  angle = math.atan2(sina, cosa)
  return angle, direction, point


def scale_matrix(factor, origin=None, direction=None):
  """Return matrix to scale by factor around origin in direction.

    Use factor -1 for point symmetry.

    >>> v = (numpy.random.rand(4, 5) - 0.5) * 20
    >>> v[3] = 1
    >>> S = scale_matrix(-1.234)
    >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3])
    True
    >>> factor = random.random() * 10 - 5
    >>> origin = numpy.random.random(3) - 0.5
    >>> direct = numpy.random.random(3) - 0.5
    >>> S = scale_matrix(factor, origin)
    >>> S = scale_matrix(factor, origin, direct)

    """
  if direction is None:
    # uniform scaling
    M = numpy.diag([factor, factor, factor, 1.0])
    if origin is not None:
      M[:3, 3] = origin[:3]
      M[:3, 3] *= 1.0 - factor
  else:
    # nonuniform scaling
    direction = unit_vector(direction[:3])
    factor = 1.0 - factor
    M = numpy.identity(4)
    M[:3, :3] -= factor * numpy.outer(direction, direction)
    if origin is not None:
      M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction
  return M


def scale_from_matrix(matrix):
  """Return scaling factor, origin and direction from scaling matrix.

    >>> factor = random.random() * 10 - 5
    >>> origin = numpy.random.random(3) - 0.5
    >>> direct = numpy.random.random(3) - 0.5
    >>> S0 = scale_matrix(factor, origin)
    >>> factor, origin, direction = scale_from_matrix(S0)
    >>> S1 = scale_matrix(factor, origin, direction)
    >>> is_same_transform(S0, S1)
    True
    >>> S0 = scale_matrix(factor, origin, direct)
    >>> factor, origin, direction = scale_from_matrix(S0)
    >>> S1 = scale_matrix(factor, origin, direction)
    >>> is_same_transform(S0, S1)
    True

    """
  M = numpy.array(matrix, dtype=numpy.float64, copy=False)
  M33 = M[:3, :3]
  factor = numpy.trace(M33) - 2.0
  try:
    # direction: unit eigenvector corresponding to eigenvalue factor
    w, V = numpy.linalg.eig(M33)
    i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0]
    direction = numpy.real(V[:, i]).squeeze()
    direction /= vector_norm(direction)
  except IndexError:
    # uniform scaling
    factor = (factor + 2.0) / 3.0
    direction = None
  # origin: any eigenvector corresponding to eigenvalue 1
  w, V = numpy.linalg.eig(M)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
  if not len(i):
    raise ValueError('no eigenvector corresponding to eigenvalue 1')
  origin = numpy.real(V[:, i[-1]]).squeeze()
  origin /= origin[3]
  return factor, origin, direction


def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False):
  """Return matrix to project onto plane defined by point and normal.

    Using either perspective point, projection direction, or none of both.

    If pseudo is True, perspective projections will preserve relative depth
    such that Perspective = dot(Orthogonal, PseudoPerspective).

    >>> P = projection_matrix([0, 0, 0], [1, 0, 0])
    >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:])
    True
    >>> point = numpy.random.random(3) - 0.5
    >>> normal = numpy.random.random(3) - 0.5
    >>> direct = numpy.random.random(3) - 0.5
    >>> persp = numpy.random.random(3) - 0.5
    >>> P0 = projection_matrix(point, normal)
    >>> P1 = projection_matrix(point, normal, direction=direct)
    >>> P2 = projection_matrix(point, normal, perspective=persp)
    >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
    >>> is_same_transform(P2, numpy.dot(P0, P3))
    True
    >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0])
    >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20
    >>> v0[3] = 1
    >>> v1 = numpy.dot(P, v0)
    >>> numpy.allclose(v1[1], v0[1])
    True
    >>> numpy.allclose(v1[0], 3-v1[1])
    True

    """
  M = numpy.identity(4)
  point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
  normal = unit_vector(normal[:3])
  if perspective is not None:
    # perspective projection
    perspective = numpy.array(perspective[:3], dtype=numpy.float64, copy=False)
    M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective - point, normal)
    M[:3, :3] -= numpy.outer(perspective, normal)
    if pseudo:
      # preserve relative depth
      M[:3, :3] -= numpy.outer(normal, normal)
      M[:3, 3] = numpy.dot(point, normal) * (perspective + normal)
    else:
      M[:3, 3] = numpy.dot(point, normal) * perspective
    M[3, :3] = -normal
    M[3, 3] = numpy.dot(perspective, normal)
  elif direction is not None:
    # parallel projection
    direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False)
    scale = numpy.dot(direction, normal)
    M[:3, :3] -= numpy.outer(direction, normal) / scale
    M[:3, 3] = direction * (numpy.dot(point, normal) / scale)
  else:
    # orthogonal projection
    M[:3, :3] -= numpy.outer(normal, normal)
    M[:3, 3] = numpy.dot(point, normal) * normal
  return M


def projection_from_matrix(matrix, pseudo=False):
  """Return projection plane and perspective point from projection matrix.

    Return values are same as arguments for projection_matrix function:
    point, normal, direction, perspective, and pseudo.

    >>> point = numpy.random.random(3) - 0.5
    >>> normal = numpy.random.random(3) - 0.5
    >>> direct = numpy.random.random(3) - 0.5
    >>> persp = numpy.random.random(3) - 0.5
    >>> P0 = projection_matrix(point, normal)
    >>> result = projection_from_matrix(P0)
    >>> P1 = projection_matrix(*result)
    >>> is_same_transform(P0, P1)
    True
    >>> P0 = projection_matrix(point, normal, direct)
    >>> result = projection_from_matrix(P0)
    >>> P1 = projection_matrix(*result)
    >>> is_same_transform(P0, P1)
    True
    >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
    >>> result = projection_from_matrix(P0, pseudo=False)
    >>> P1 = projection_matrix(*result)
    >>> is_same_transform(P0, P1)
    True
    >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
    >>> result = projection_from_matrix(P0, pseudo=True)
    >>> P1 = projection_matrix(*result)
    >>> is_same_transform(P0, P1)
    True

    """
  M = numpy.array(matrix, dtype=numpy.float64, copy=False)
  M33 = M[:3, :3]
  w, V = numpy.linalg.eig(M)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
  if not pseudo and len(i):
    # point: any eigenvector corresponding to eigenvalue 1
    point = numpy.real(V[:, i[-1]]).squeeze()
    point /= point[3]
    # direction: unit eigenvector corresponding to eigenvalue 0
    w, V = numpy.linalg.eig(M33)
    i = numpy.where(abs(numpy.real(w)) < 1e-8)[0]
    if not len(i):
      raise ValueError('no eigenvector corresponding to eigenvalue 0')
    direction = numpy.real(V[:, i[0]]).squeeze()
    direction /= vector_norm(direction)
    # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
    w, V = numpy.linalg.eig(M33.T)
    i = numpy.where(abs(numpy.real(w)) < 1e-8)[0]
    if len(i):
      # parallel projection
      normal = numpy.real(V[:, i[0]]).squeeze()
      normal /= vector_norm(normal)
      return point, normal, direction, None, False
    else:
      # orthogonal projection, where normal equals direction vector
      return point, direction, None, None, False
  else:
    # perspective projection
    i = numpy.where(abs(numpy.real(w)) > 1e-8)[0]
    if not len(i):
      raise ValueError('no eigenvector not corresponding to eigenvalue 0')
    point = numpy.real(V[:, i[-1]]).squeeze()
    point /= point[3]
    normal = -M[3, :3]
    perspective = M[:3, 3] / numpy.dot(point[:3], normal)
    if pseudo:
      perspective -= normal
    return point, normal, None, perspective, pseudo


def clip_matrix(left, right, bottom, top, near, far, perspective=False):
  """Return matrix to obtain normalized device coordinates from frustum.

    The frustum bounds are axis-aligned along x (left, right),
    y (bottom, top) and z (near, far).

    Normalized device coordinates are in range [-1, 1] if coordinates are
    inside the frustum.

    If perspective is True the frustum is a truncated pyramid with the
    perspective point at origin and direction along z axis, otherwise an
    orthographic canonical view volume (a box).

    Homogeneous coordinates transformed by the perspective clip matrix
    need to be dehomogenized (divided by w coordinate).

    >>> frustum = numpy.random.rand(6)
    >>> frustum[1] += frustum[0]
    >>> frustum[3] += frustum[2]
    >>> frustum[5] += frustum[4]
    >>> M = clip_matrix(perspective=False, *frustum)
    >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1])
    array([-1., -1., -1.,  1.])
    >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1])
    array([ 1.,  1.,  1.,  1.])
    >>> M = clip_matrix(perspective=True, *frustum)
    >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1])
    >>> v / v[3]
    array([-1., -1., -1.,  1.])
    >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1])
    >>> v / v[3]
    array([ 1.,  1., -1.,  1.])

    """
  if left >= right or bottom >= top or near >= far:
    raise ValueError('invalid frustum')
  if perspective:
    if near <= _EPS:
      raise ValueError('invalid frustum: near <= 0')
    t = 2.0 * near
    M = [[t / (left - right), 0.0, (right + left) / (right - left), 0.0],
         [0.0, t / (bottom - top), (top + bottom) / (top - bottom), 0.0],
         [0.0, 0.0, (far + near) / (near - far), t * far / (far - near)], [0.0, 0.0, -1.0, 0.0]]
  else:
    M = [[2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)],
         [0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)],
         [0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)], [0.0, 0.0, 0.0, 1.0]]
  return numpy.array(M)


def shear_matrix(angle, direction, point, normal):
  """Return matrix to shear by angle along direction vector on shear plane.

    The shear plane is defined by a point and normal vector. The direction
    vector must be orthogonal to the plane's normal vector.

    A point P is transformed by the shear matrix into P" such that
    the vector P-P" is parallel to the direction vector and its extent is
    given by the angle of P-P'-P", where P' is the orthogonal projection
    of P onto the shear plane.

    >>> angle = (random.random() - 0.5) * 4*math.pi
    >>> direct = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> normal = numpy.cross(direct, numpy.random.random(3))
    >>> S = shear_matrix(angle, direct, point, normal)
    >>> numpy.allclose(1, numpy.linalg.det(S))
    True

    """
  normal = unit_vector(normal[:3])
  direction = unit_vector(direction[:3])
  if abs(numpy.dot(normal, direction)) > 1e-6:
    raise ValueError('direction and normal vectors are not orthogonal')
  angle = math.tan(angle)
  M = numpy.identity(4)
  M[:3, :3] += angle * numpy.outer(direction, normal)
  M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
  return M


def shear_from_matrix(matrix):
  """Return shear angle, direction and plane from shear matrix.

    >>> angle = (random.random() - 0.5) * 4*math.pi
    >>> direct = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> normal = numpy.cross(direct, numpy.random.random(3))
    >>> S0 = shear_matrix(angle, direct, point, normal)
    >>> angle, direct, point, normal = shear_from_matrix(S0)
    >>> S1 = shear_matrix(angle, direct, point, normal)
    >>> is_same_transform(S0, S1)
    True

    """
  M = numpy.array(matrix, dtype=numpy.float64, copy=False)
  M33 = M[:3, :3]
  # normal: cross independent eigenvectors corresponding to the eigenvalue 1
  w, V = numpy.linalg.eig(M33)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0]
  if len(i) < 2:
    raise ValueError('no two linear independent eigenvectors found %s' % w)
  V = numpy.real(V[:, i]).squeeze().T
  lenorm = -1.0
  for i0, i1 in ((0, 1), (0, 2), (1, 2)):
    n = numpy.cross(V[i0], V[i1])
    w = vector_norm(n)
    if w > lenorm:
      lenorm = w
      normal = n
  normal /= lenorm
  # direction and angle
  direction = numpy.dot(M33 - numpy.identity(3), normal)
  angle = vector_norm(direction)
  direction /= angle
  angle = math.atan(angle)
  # point: eigenvector corresponding to eigenvalue 1
  w, V = numpy.linalg.eig(M)
  i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
  if not len(i):
    raise ValueError('no eigenvector corresponding to eigenvalue 1')
  point = numpy.real(V[:, i[-1]]).squeeze()
  point /= point[3]
  return angle, direction, point, normal


def decompose_matrix(matrix):
  """Return sequence of transformations from transformation matrix.

    matrix : array_like
        Non-degenerative homogeneous transformation matrix

    Return tuple of:
        scale : vector of 3 scaling factors
        shear : list of shear factors for x-y, x-z, y-z axes
        angles : list of Euler angles about static x, y, z axes
        translate : translation vector along x, y, z axes
        perspective : perspective partition of matrix

    Raise ValueError if matrix is of wrong type or degenerative.

    >>> T0 = translation_matrix([1, 2, 3])
    >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
    >>> T1 = translation_matrix(trans)
    >>> numpy.allclose(T0, T1)
    True
    >>> S = scale_matrix(0.123)
    >>> scale, shear, angles, trans, persp = decompose_matrix(S)
    >>> scale[0]
    0.123
    >>> R0 = euler_matrix(1, 2, 3)
    >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
    >>> R1 = euler_matrix(*angles)
    >>> numpy.allclose(R0, R1)
    True

    """
  M = numpy.array(matrix, dtype=numpy.float64, copy=True).T
  if abs(M[3, 3]) < _EPS:
    raise ValueError('M[3, 3] is zero')
  M /= M[3, 3]
  P = M.copy()
  P[:, 3] = 0.0, 0.0, 0.0, 1.0
  if not numpy.linalg.det(P):
    raise ValueError('matrix is singular')

  scale = numpy.zeros((3,))
  shear = [0.0, 0.0, 0.0]
  angles = [0.0, 0.0, 0.0]

  if any(abs(M[:3, 3]) > _EPS):
    perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T))
    M[:, 3] = 0.0, 0.0, 0.0, 1.0
  else:
    perspective = numpy.array([0.0, 0.0, 0.0, 1.0])

  translate = M[3, :3].copy()
  M[3, :3] = 0.0

  row = M[:3, :3].copy()
  scale[0] = vector_norm(row[0])
  row[0] /= scale[0]
  shear[0] = numpy.dot(row[0], row[1])
  row[1] -= row[0] * shear[0]
  scale[1] = vector_norm(row[1])
  row[1] /= scale[1]
  shear[0] /= scale[1]
  shear[1] = numpy.dot(row[0], row[2])
  row[2] -= row[0] * shear[1]
  shear[2] = numpy.dot(row[1], row[2])
  row[2] -= row[1] * shear[2]
  scale[2] = vector_norm(row[2])
  row[2] /= scale[2]
  shear[1:] /= scale[2]

  if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0:
    numpy.negative(scale, scale)
    numpy.negative(row, row)

  angles[1] = math.asin(-row[0, 2])
  if math.cos(angles[1]):
    angles[0] = math.atan2(row[1, 2], row[2, 2])
    angles[2] = math.atan2(row[0, 1], row[0, 0])
  else:
    # angles[0] = math.atan2(row[1, 0], row[1, 1])
    angles[0] = math.atan2(-row[2, 1], row[1, 1])
    angles[2] = 0.0

  return scale, shear, angles, translate, perspective


def compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None):
  """Return transformation matrix from sequence of transformations.

    This is the inverse of the decompose_matrix function.

    Sequence of transformations:
        scale : vector of 3 scaling factors
        shear : list of shear factors for x-y, x-z, y-z axes
        angles : list of Euler angles about static x, y, z axes
        translate : translation vector along x, y, z axes
        perspective : perspective partition of matrix

    >>> scale = numpy.random.random(3) - 0.5
    >>> shear = numpy.random.random(3) - 0.5
    >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi)
    >>> trans = numpy.random.random(3) - 0.5
    >>> persp = numpy.random.random(4) - 0.5
    >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
    >>> result = decompose_matrix(M0)
    >>> M1 = compose_matrix(*result)
    >>> is_same_transform(M0, M1)
    True

    """
  M = numpy.identity(4)
  if perspective is not None:
    P = numpy.identity(4)
    P[3, :] = perspective[:4]
    M = numpy.dot(M, P)
  if translate is not None:
    T = numpy.identity(4)
    T[:3, 3] = translate[:3]
    M = numpy.dot(M, T)
  if angles is not None:
    R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz')
    M = numpy.dot(M, R)
  if shear is not None:
    Z = numpy.identity(4)
    Z[1, 2] = shear[2]
    Z[0, 2] = shear[1]
    Z[0, 1] = shear[0]
    M = numpy.dot(M, Z)
  if scale is not None:
    S = numpy.identity(4)
    S[0, 0] = scale[0]
    S[1, 1] = scale[1]
    S[2, 2] = scale[2]
    M = numpy.dot(M, S)
  M /= M[3, 3]
  return M


def orthogonalization_matrix(lengths, angles):
  """Return orthogonalization matrix for crystallographic cell coordinates.

    Angles are expected in degrees.

    The de-orthogonalization matrix is the inverse.

    >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
    >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
    True
    >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
    >>> numpy.allclose(numpy.sum(O), 43.063229)
    True

    """
  a, b, c = lengths
  angles = numpy.radians(angles)
  sina, sinb, _ = numpy.sin(angles)
  cosa, cosb, cosg = numpy.cos(angles)
  co = (cosa * cosb - cosg) / (sina * sinb)
  return numpy.array([[a * sinb * math.sqrt(1.0 - co * co), 0.0, 0.0, 0.0],
                      [-a * sinb * co, b * sina, 0.0, 0.0], [a * cosb, b * cosa, c, 0.0],
                      [0.0, 0.0, 0.0, 1.0]])


def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
  """Return affine transform matrix to register two point sets.

    v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous
    coordinates, where ndims is the dimensionality of the coordinate space.

    If shear is False, a similarity transformation matrix is returned.
    If also scale is False, a rigid/Euclidean transformation matrix
    is returned.

    By default the algorithm by Hartley and Zissermann [15] is used.
    If usesvd is True, similarity and Euclidean transformation matrices
    are calculated by minimizing the weighted sum of squared deviations
    (RMSD) according to the algorithm by Kabsch [8].
    Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]
    is used, which is slower when using this Python implementation.

    The returned matrix performs rotation, translation and uniform scaling
    (if specified).

    >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
    >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
    >>> affine_matrix_from_points(v0, v1)
    array([[   0.14549,    0.00062,  675.50008],
           [   0.00048,    0.14094,   53.24971],
           [   0.     ,    0.     ,    1.     ]])
    >>> T = translation_matrix(numpy.random.random(3)-0.5)
    >>> R = random_rotation_matrix(numpy.random.random(3))
    >>> S = scale_matrix(random.random())
    >>> M = concatenate_matrices(T, R, S)
    >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
    >>> v0[3] = 1
    >>> v1 = numpy.dot(M, v0)
    >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1)
    >>> M = affine_matrix_from_points(v0[:3], v1[:3])
    >>> numpy.allclose(v1, numpy.dot(M, v0))
    True

    More examples in superimposition_matrix()

    """
  v0 = numpy.array(v0, dtype=numpy.float64, copy=True)
  v1 = numpy.array(v1, dtype=numpy.float64, copy=True)

  ndims = v0.shape[0]
  if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
    raise ValueError('input arrays are of wrong shape or type')

  # move centroids to origin
  t0 = -numpy.mean(v0, axis=1)
  M0 = numpy.identity(ndims + 1)
  M0[:ndims, ndims] = t0
  v0 += t0.reshape(ndims, 1)
  t1 = -numpy.mean(v1, axis=1)
  M1 = numpy.identity(ndims + 1)
  M1[:ndims, ndims] = t1
  v1 += t1.reshape(ndims, 1)

  if shear:
    # Affine transformation
    A = numpy.concatenate((v0, v1), axis=0)
    u, s, vh = numpy.linalg.svd(A.T)
    vh = vh[:ndims].T
    B = vh[:ndims]
    C = vh[ndims:2 * ndims]
    t = numpy.dot(C, numpy.linalg.pinv(B))
    t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1)
    M = numpy.vstack((t, ((0.0,) * ndims) + (1.0,)))
  elif usesvd or ndims != 3:
    # Rigid transformation via SVD of covariance matrix
    u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T))
    # rotation matrix from SVD orthonormal bases
    R = numpy.dot(u, vh)
    if numpy.linalg.det(R) < 0.0:
      # R does not constitute right handed system
      R -= numpy.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)
      s[-1] *= -1.0
    # homogeneous transformation matrix
    M = numpy.identity(ndims + 1)
    M[:ndims, :ndims] = R
  else:
    # Rigid transformation matrix via quaternion
    # compute symmetric matrix N
    xx, yy, zz = numpy.sum(v0 * v1, axis=1)
    xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1)
    xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1)
    N = [[xx + yy + zz, 0.0, 0.0, 0.0], [yz - zy, xx - yy - zz, 0.0, 0.0],
         [zx - xz, xy + yx, yy - xx - zz, 0.0], [xy - yx, zx + xz, yz + zy, zz - xx - yy]]
    # quaternion: eigenvector corresponding to most positive eigenvalue
    w, V = numpy.linalg.eigh(N)
    q = V[:, numpy.argmax(w)]
    q /= vector_norm(q)  # unit quaternion
    # homogeneous transformation matrix
    M = quaternion_matrix(q)

  if scale and not shear:
    # Affine transformation; scale is ratio of RMS deviations from centroid
    v0 *= v0
    v1 *= v1
    M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0))

  # move centroids back
  M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0))
  M /= M[ndims, ndims]
  return M


def superimposition_matrix(v0, v1, scale=False, usesvd=True):
  """Return matrix to transform given 3D point set into second point set.

    v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points.

    The parameters scale and usesvd are explained in the more general
    affine_matrix_from_points function.

    The returned matrix is a similarity or Euclidean transformation matrix.
    This function has a fast C implementation in transformations.c.

    >>> v0 = numpy.random.rand(3, 10)
    >>> M = superimposition_matrix(v0, v0)
    >>> numpy.allclose(M, numpy.identity(4))
    True
    >>> R = random_rotation_matrix(numpy.random.random(3))
    >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]]
    >>> v1 = numpy.dot(R, v0)
    >>> M = superimposition_matrix(v0, v1)
    >>> numpy.allclose(v1, numpy.dot(M, v0))
    True
    >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
    >>> v0[3] = 1
    >>> v1 = numpy.dot(R, v0)
    >>> M = superimposition_matrix(v0, v1)
    >>> numpy.allclose(v1, numpy.dot(M, v0))
    True
    >>> S = scale_matrix(random.random())
    >>> T = translation_matrix(numpy.random.random(3)-0.5)
    >>> M = concatenate_matrices(T, R, S)
    >>> v1 = numpy.dot(M, v0)
    >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1)
    >>> M = superimposition_matrix(v0, v1, scale=True)
    >>> numpy.allclose(v1, numpy.dot(M, v0))
    True
    >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
    >>> numpy.allclose(v1, numpy.dot(M, v0))
    True
    >>> v = numpy.empty((4, 100, 3))
    >>> v[:, :, 0] = v0
    >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
    >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0]))
    True

    """
  v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3]
  v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3]
  return affine_matrix_from_points(v0, v1, shear=False, scale=scale, usesvd=usesvd)


def euler_matrix(ai, aj, ak, axes='sxyz'):
  """Return homogeneous rotation matrix from Euler angles and axis sequence.

    ai, aj, ak : Euler's roll, pitch and yaw angles
    axes : One of 24 axis sequences as string or encoded tuple

    >>> R = euler_matrix(1, 2, 3, 'syxz')
    >>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
    True
    >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
    >>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
    True
    >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5)
    >>> for axes in _AXES2TUPLE.keys():
    ...    R = euler_matrix(ai, aj, ak, axes)
    >>> for axes in _TUPLE2AXES.keys():
    ...    R = euler_matrix(ai, aj, ak, axes)

    """
  try:
    firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
  except (AttributeError, KeyError):
    _TUPLE2AXES[axes]  # noqa: validation
    firstaxis, parity, repetition, frame = axes

  i = firstaxis
  j = _NEXT_AXIS[i + parity]
  k = _NEXT_AXIS[i - parity + 1]

  if frame:
    ai, ak = ak, ai
  if parity:
    ai, aj, ak = -ai, -aj, -ak

  si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
  ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
  cc, cs = ci * ck, ci * sk
  sc, ss = si * ck, si * sk

  M = numpy.identity(4)
  if repetition:
    M[i, i] = cj
    M[i, j] = sj * si
    M[i, k] = sj * ci
    M[j, i] = sj * sk
    M[j, j] = -cj * ss + cc
    M[j, k] = -cj * cs - sc
    M[k, i] = -sj * ck
    M[k, j] = cj * sc + cs
    M[k, k] = cj * cc - ss
  else:
    M[i, i] = cj * ck
    M[i, j] = sj * sc - cs
    M[i, k] = sj * cc + ss
    M[j, i] = cj * sk
    M[j, j] = sj * ss + cc
    M[j, k] = sj * cs - sc
    M[k, i] = -sj
    M[k, j] = cj * si
    M[k, k] = cj * ci
  return M


def euler_from_matrix(matrix, axes='sxyz'):
  """Return Euler angles from rotation matrix for specified axis sequence.

    axes : One of 24 axis sequences as string or encoded tuple

    Note that many Euler angle triplets can describe one matrix.

    >>> R0 = euler_matrix(1, 2, 3, 'syxz')
    >>> al, be, ga = euler_from_matrix(R0, 'syxz')
    >>> R1 = euler_matrix(al, be, ga, 'syxz')
    >>> numpy.allclose(R0, R1)
    True
    >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5)
    >>> for axes in _AXES2TUPLE.keys():
    ...    R0 = euler_matrix(axes=axes, *angles)
    ...    R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
    ...    if not numpy.allclose(R0, R1): print(axes, "failed")

    """
  try:
    firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
  except (AttributeError, KeyError):
    _TUPLE2AXES[axes]  # noqa: validation
    firstaxis, parity, repetition, frame = axes

  i = firstaxis
  j = _NEXT_AXIS[i + parity]
  k = _NEXT_AXIS[i - parity + 1]

  M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]
  if repetition:
    sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
    if sy > _EPS:
      ax = math.atan2(M[i, j], M[i, k])
      ay = math.atan2(sy, M[i, i])
      az = math.atan2(M[j, i], -M[k, i])
    else:
      ax = math.atan2(-M[j, k], M[j, j])
      ay = math.atan2(sy, M[i, i])
      az = 0.0
  else:
    cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
    if cy > _EPS:
      ax = math.atan2(M[k, j], M[k, k])
      ay = math.atan2(-M[k, i], cy)
      az = math.atan2(M[j, i], M[i, i])
    else:
      ax = math.atan2(-M[j, k], M[j, j])
      ay = math.atan2(-M[k, i], cy)
      az = 0.0

  if parity:
    ax, ay, az = -ax, -ay, -az
  if frame:
    ax, az = az, ax
  return ax, ay, az


def euler_from_quaternion(quaternion, axes='sxyz'):
  """Return Euler angles from quaternion for specified axis sequence.

    >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
    >>> numpy.allclose(angles, [0.123, 0, 0])
    True

    """
  return euler_from_matrix(quaternion_matrix(quaternion), axes)


def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
  """Return quaternion from Euler angles and axis sequence.

    ai, aj, ak : Euler's roll, pitch and yaw angles
    axes : One of 24 axis sequences as string or encoded tuple

    >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
    >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
    True

    """
  try:
    firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
  except (AttributeError, KeyError):
    _TUPLE2AXES[axes]  # noqa: validation
    firstaxis, parity, repetition, frame = axes

  i = firstaxis + 1
  j = _NEXT_AXIS[i + parity - 1] + 1
  k = _NEXT_AXIS[i - parity] + 1

  if frame:
    ai, ak = ak, ai
  if parity:
    aj = -aj

  ai /= 2.0
  aj /= 2.0
  ak /= 2.0
  ci = math.cos(ai)
  si = math.sin(ai)
  cj = math.cos(aj)
  sj = math.sin(aj)
  ck = math.cos(ak)
  sk = math.sin(ak)
  cc = ci * ck
  cs = ci * sk
  sc = si * ck
  ss = si * sk

  q = numpy.empty((4,))
  if repetition:
    q[0] = cj * (cc - ss)
    q[i] = cj * (cs + sc)
    q[j] = sj * (cc + ss)
    q[k] = sj * (cs - sc)
  else:
    q[0] = cj * cc + sj * ss
    q[i] = cj * sc - sj * cs
    q[j] = cj * ss + sj * cc
    q[k] = cj * cs - sj * sc
  if parity:
    q[j] *= -1.0

  return q


def quaternion_about_axis(angle, axis):
  """Return quaternion for rotation about axis.

    >>> q = quaternion_about_axis(0.123, [1, 0, 0])
    >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0])
    True

    """
  q = numpy.array([0.0, axis[0], axis[1], axis[2]])
  qlen = vector_norm(q)
  if qlen > _EPS:
    q *= math.sin(angle / 2.0) / qlen
  q[0] = math.cos(angle / 2.0)
  return q


def quaternion_matrix(quaternion):
  """Return homogeneous rotation matrix from quaternion.

    >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
    >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
    True
    >>> M = quaternion_matrix([1, 0, 0, 0])
    >>> numpy.allclose(M, numpy.identity(4))
    True
    >>> M = quaternion_matrix([0, 1, 0, 0])
    >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
    True

    """
  q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
  n = numpy.dot(q, q)
  if n < _EPS:
    return numpy.identity(4)
  q *= math.sqrt(2.0 / n)
  q = numpy.outer(q, q)
  return numpy.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]])


def quaternion_from_matrix(matrix, isprecise=False):
  """Return quaternion from rotation matrix.

    If isprecise is True, the input matrix is assumed to be a precise rotation
    matrix and a faster algorithm is used.

    >>> q = quaternion_from_matrix(numpy.identity(4), True)
    >>> numpy.allclose(q, [1, 0, 0, 0])
    True
    >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1]))
    >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0])
    True
    >>> R = rotation_matrix(0.123, (1, 2, 3))
    >>> q = quaternion_from_matrix(R, True)
    >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
    True
    >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
    ...      [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
    >>> q = quaternion_from_matrix(R)
    >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
    True
    >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
    ...      [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
    >>> q = quaternion_from_matrix(R)
    >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
    True
    >>> R = random_rotation_matrix()
    >>> q = quaternion_from_matrix(R)
    >>> is_same_transform(R, quaternion_matrix(q))
    True
    >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
    ...                    quaternion_from_matrix(R, isprecise=True))
    True
    >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0)
    >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
    ...                    quaternion_from_matrix(R, isprecise=True))
    True

    """
  M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
  if isprecise:
    q = numpy.empty((4,))
    t = numpy.trace(M)
    if t > M[3, 3]:
      q[0] = t
      q[3] = M[1, 0] - M[0, 1]
      q[2] = M[0, 2] - M[2, 0]
      q[1] = M[2, 1] - M[1, 2]
    else:
      i, j, k = 0, 1, 2
      if M[1, 1] > M[0, 0]:
        i, j, k = 1, 2, 0
      if M[2, 2] > M[i, i]:
        i, j, k = 2, 0, 1
      t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
      q[i] = t
      q[j] = M[i, j] + M[j, i]
      q[k] = M[k, i] + M[i, k]
      q[3] = M[k, j] - M[j, k]
      q = q[[3, 0, 1, 2]]
    q *= 0.5 / math.sqrt(t * M[3, 3])
  else:
    m00 = M[0, 0]
    m01 = M[0, 1]
    m02 = M[0, 2]
    m10 = M[1, 0]
    m11 = M[1, 1]
    m12 = M[1, 2]
    m20 = M[2, 0]
    m21 = M[2, 1]
    m22 = M[2, 2]
    # symmetric matrix K
    K = numpy.array([[m00 - m11 - m22, 0.0, 0.0, 0.0], [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
                     [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
                     [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
    K /= 3.0
    # quaternion is eigenvector of K that corresponds to largest eigenvalue
    w, V = numpy.linalg.eigh(K)
    q = V[[3, 0, 1, 2], numpy.argmax(w)]
  if q[0] < 0.0:
    numpy.negative(q, q)
  return q


def quaternion_multiply(quaternion1, quaternion0):
  """Return multiplication of two quaternions.

    >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
    >>> numpy.allclose(q, [28, -44, -14, 48])
    True

    """
  w0, x0, y0, z0 = quaternion0
  w1, x1, y1, z1 = quaternion1
  return numpy.array([
      -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
      -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0
  ],
                     dtype=numpy.float64)


def quaternion_conjugate(quaternion):
  """Return conjugate of quaternion.

    >>> q0 = random_quaternion()
    >>> q1 = quaternion_conjugate(q0)
    >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:])
    True

    """
  q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
  numpy.negative(q[1:], q[1:])
  return q


def quaternion_inverse(quaternion):
  """Return inverse of quaternion.

    >>> q0 = random_quaternion()
    >>> q1 = quaternion_inverse(q0)
    >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])
    True

    """
  q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
  numpy.negative(q[1:], q[1:])
  return q / numpy.dot(q, q)


def quaternion_real(quaternion):
  """Return real part of quaternion.

    >>> quaternion_real([3, 0, 1, 2])
    3.0

    """
  return float(quaternion[0])


def quaternion_imag(quaternion):
  """Return imaginary part of quaternion.

    >>> quaternion_imag([3, 0, 1, 2])
    array([ 0.,  1.,  2.])

    """
  return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True)


def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
  """Return spherical linear interpolation between two quaternions.

    >>> q0 = random_quaternion()
    >>> q1 = random_quaternion()
    >>> q = quaternion_slerp(q0, q1, 0)
    >>> numpy.allclose(q, q0)
    True
    >>> q = quaternion_slerp(q0, q1, 1, 1)
    >>> numpy.allclose(q, q1)
    True
    >>> q = quaternion_slerp(q0, q1, 0.5)
    >>> angle = math.acos(numpy.dot(q0, q))
    >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \
        numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle)
    True

    """
  q0 = unit_vector(quat0[:4])
  q1 = unit_vector(quat1[:4])
  if fraction == 0.0:
    return q0
  elif fraction == 1.0:
    return q1
  d = numpy.dot(q0, q1)
  if abs(abs(d) - 1.0) < _EPS:
    return q0
  if shortestpath and d < 0.0:
    # invert rotation
    d = -d
    numpy.negative(q1, q1)
  angle = math.acos(d) + spin * math.pi
  if abs(angle) < _EPS:
    return q0
  isin = 1.0 / math.sin(angle)
  q0 *= math.sin((1.0 - fraction) * angle) * isin
  q1 *= math.sin(fraction * angle) * isin
  q0 += q1
  return q0


def random_quaternion(rand=None):
  """Return uniform random unit quaternion.

    rand: array like or None
        Three independent random variables that are uniformly distributed
        between 0 and 1.

    >>> q = random_quaternion()
    >>> numpy.allclose(1, vector_norm(q))
    True
    >>> q = random_quaternion(numpy.random.random(3))
    >>> len(q.shape), q.shape[0]==4
    (1, True)

    """
  if rand is None:
    rand = numpy.random.rand(3)
  else:
    assert len(rand) == 3
  r1 = numpy.sqrt(1.0 - rand[0])
  r2 = numpy.sqrt(rand[0])
  pi2 = math.pi * 2.0
  t1 = pi2 * rand[1]
  t2 = pi2 * rand[2]
  return numpy.array(
      [numpy.cos(t2) * r2,
       numpy.sin(t1) * r1,
       numpy.cos(t1) * r1,
       numpy.sin(t2) * r2])


def random_rotation_matrix(rand=None):
  """Return uniform random rotation matrix.

    rand: array like
        Three independent random variables that are uniformly distributed
        between 0 and 1 for each returned quaternion.

    >>> R = random_rotation_matrix()
    >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
    True

    """
  return quaternion_matrix(random_quaternion(rand))


# epsilon for testing whether a number is close to zero
_EPS = numpy.finfo(float).eps * 4.0

# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]

# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
    'sxyz': (0, 0, 0, 0),
    'sxyx': (0, 0, 1, 0),
    'sxzy': (0, 1, 0, 0),
    'sxzx': (0, 1, 1, 0),
    'syzx': (1, 0, 0, 0),
    'syzy': (1, 0, 1, 0),
    'syxz': (1, 1, 0, 0),
    'syxy': (1, 1, 1, 0),
    'szxy': (2, 0, 0, 0),
    'szxz': (2, 0, 1, 0),
    'szyx': (2, 1, 0, 0),
    'szyz': (2, 1, 1, 0),
    'rzyx': (0, 0, 0, 1),
    'rxyx': (0, 0, 1, 1),
    'ryzx': (0, 1, 0, 1),
    'rxzx': (0, 1, 1, 1),
    'rxzy': (1, 0, 0, 1),
    'ryzy': (1, 0, 1, 1),
    'rzxy': (1, 1, 0, 1),
    'ryxy': (1, 1, 1, 1),
    'ryxz': (2, 0, 0, 1),
    'rzxz': (2, 0, 1, 1),
    'rxyz': (2, 1, 0, 1),
    'rzyz': (2, 1, 1, 1)
}

_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())


def vector_norm(data, axis=None, out=None):
  """Return length, i.e. Euclidean norm, of ndarray along axis.

    >>> v = numpy.random.random(3)
    >>> n = vector_norm(v)
    >>> numpy.allclose(n, numpy.linalg.norm(v))
    True
    >>> v = numpy.random.rand(6, 5, 3)
    >>> n = vector_norm(v, axis=-1)
    >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
    True
    >>> n = vector_norm(v, axis=1)
    >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
    True
    >>> v = numpy.random.rand(5, 4, 3)
    >>> n = numpy.empty((5, 3))
    >>> vector_norm(v, axis=1, out=n)
    >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
    True
    >>> vector_norm([])
    0.0
    >>> vector_norm([1])
    1.0

    """
  data = numpy.array(data, dtype=numpy.float64, copy=True)
  if out is None:
    if data.ndim == 1:
      return math.sqrt(numpy.dot(data, data))
    data *= data
    out = numpy.atleast_1d(numpy.sum(data, axis=axis))
    numpy.sqrt(out, out)
    return out
  else:
    data *= data
    numpy.sum(data, axis=axis, out=out)
    numpy.sqrt(out, out)


def unit_vector(data, axis=None, out=None):
  """Return ndarray normalized by length, i.e. Euclidean norm, along axis.

    >>> v0 = numpy.random.random(3)
    >>> v1 = unit_vector(v0)
    >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
    True
    >>> v0 = numpy.random.rand(5, 4, 3)
    >>> v1 = unit_vector(v0, axis=-1)
    >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
    >>> numpy.allclose(v1, v2)
    True
    >>> v1 = unit_vector(v0, axis=1)
    >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
    >>> numpy.allclose(v1, v2)
    True
    >>> v1 = numpy.empty((5, 4, 3))
    >>> unit_vector(v0, axis=1, out=v1)
    >>> numpy.allclose(v1, v2)
    True
    >>> list(unit_vector([]))
    []
    >>> list(unit_vector([1]))
    [1.0]

    """
  if out is None:
    data = numpy.array(data, dtype=numpy.float64, copy=True)
    if data.ndim == 1:
      data /= math.sqrt(numpy.dot(data, data))
      return data
  else:
    if out is not data:
      out[:] = numpy.array(data, copy=False)
    data = out
  length = numpy.atleast_1d(numpy.sum(data * data, axis))
  numpy.sqrt(length, length)
  if axis is not None:
    length = numpy.expand_dims(length, axis)
  data /= length
  if out is None:
    return data


def random_vector(size):
  """Return array of random doubles in the half-open interval [0.0, 1.0).

    >>> v = random_vector(10000)
    >>> numpy.all(v >= 0) and numpy.all(v < 1)
    True
    >>> v0 = random_vector(10)
    >>> v1 = random_vector(10)
    >>> numpy.any(v0 == v1)
    False

    """
  return numpy.random.random(size)


def vector_product(v0, v1, axis=0):
  """Return vector perpendicular to vectors.

    >>> v = vector_product([2, 0, 0], [0, 3, 0])
    >>> numpy.allclose(v, [0, 0, 6])
    True
    >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
    >>> v1 = [[3], [0], [0]]
    >>> v = vector_product(v0, v1)
    >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])
    True
    >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
    >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
    >>> v = vector_product(v0, v1, axis=1)
    >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])
    True

    """
  return numpy.cross(v0, v1, axis=axis)


def angle_between_vectors(v0, v1, directed=True, axis=0):
  """Return angle between vectors.

    If directed is False, the input vectors are interpreted as undirected axes,
    i.e. the maximum angle is pi/2.

    >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])
    >>> numpy.allclose(a, math.pi)
    True
    >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)
    >>> numpy.allclose(a, 0)
    True
    >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
    >>> v1 = [[3], [0], [0]]
    >>> a = angle_between_vectors(v0, v1)
    >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532])
    True
    >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
    >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
    >>> a = angle_between_vectors(v0, v1, axis=1)
    >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])
    True

    """
  v0 = numpy.array(v0, dtype=numpy.float64, copy=False)
  v1 = numpy.array(v1, dtype=numpy.float64, copy=False)
  dot = numpy.sum(v0 * v1, axis=axis)
  dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)
  dot = numpy.clip(dot, -1.0, 1.0)
  return numpy.arccos(dot if directed else numpy.fabs(dot))


def inverse_matrix(matrix):
  """Return inverse of square transformation matrix.

    >>> M0 = random_rotation_matrix()
    >>> M1 = inverse_matrix(M0.T)
    >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))
    True
    >>> for size in range(1, 7):
    ...     M0 = numpy.random.rand(size, size)
    ...     M1 = inverse_matrix(M0)
    ...     if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size)

    """
  return numpy.linalg.inv(matrix)


def concatenate_matrices(*matrices):
  """Return concatenation of series of transformation matrices.

    >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5
    >>> numpy.allclose(M, concatenate_matrices(M))
    True
    >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))
    True

    """
  M = numpy.identity(4)
  for i in matrices:
    M = numpy.dot(M, i)
  return M


def is_same_transform(matrix0, matrix1):
  """Return True if two matrices perform same transformation.

    >>> is_same_transform(numpy.identity(4), numpy.identity(4))
    True
    >>> is_same_transform(numpy.identity(4), random_rotation_matrix())
    False

    """
  matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True)
  matrix0 /= matrix0[3, 3]
  matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True)
  matrix1 /= matrix1[3, 3]
  return numpy.allclose(matrix0, matrix1)


def is_same_quaternion(q0, q1):
  """Return True if two quaternions are equal."""
  q0 = numpy.array(q0)
  q1 = numpy.array(q1)
  return numpy.allclose(q0, q1) or numpy.allclose(q0, -q1)


def _import_module(name, package=None, warn=True, postfix='_py', ignore='_'):
  """Try import all public attributes from module into global namespace.

    Existing attributes with name clashes are renamed with prefix.
    Attributes starting with underscore are ignored by default.

    Return True on successful import.

    """
  import warnings
  from importlib import import_module
  try:
    if not package:
      module = import_module(name)
    else:
      module = import_module('.' + name, package=package)
  except ImportError as err:
    if warn:
      warnings.warn(str(err))
  else:
    for attr in dir(module):
      if ignore and attr.startswith(ignore):
        continue
      if postfix:
        if attr in globals():
          globals()[attr + postfix] = globals()[attr]
        elif warn:
          warnings.warn('no Python implementation of ' + attr)
      globals()[attr] = getattr(module, attr)
    return True


# _import_module('_transformations', __package__)

# if __name__ == '__main__':
#     import doctest
#     import random  # noqa: used in doctests
#     try:
#         numpy.set_printoptions(suppress=True, precision=5, legacy='1.13')
#     except TypeError:
#         numpy.set_printoptions(suppress=True, precision=5)
#     doctest.testmod()