text: # -*- coding: utf-8 -*-
  # transformations.py
  
  # Adapted from:
  # https://github.com/akanazawa/cmr/blob/c24cab6aececa1cb8416ccb2d3ee470915726937/utils/transformations.py
  
  # Copyright (c) 2006-2017, Christoph Gohlke
  # Copyright (c) 2006-2017, The Regents of the University of California
  # Produced at the Laboratory for Fluorescence Dynamics
  # All rights reserved.
  #
  # Redistribution and use in source and binary forms, with or without
  # modification, are permitted provided that the following conditions are met:
  #
  # * Redistributions of source code must retain the above copyright
  #   notice, this list of conditions and the following disclaimer.
  # * Redistributions in binary form must reproduce the above copyright
  #   notice, this list of conditions and the following disclaimer in the
  #   documentation and/or other materials provided with the distribution.
  # * Neither the name of the copyright holders nor the names of any
  #   contributors may be used to endorse or promote products derived
  #   from this software without specific prior written permission.
  #
  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  # ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  # POSSIBILITY OF SUCH DAMAGE.
  
  
  import math
  import numpy
  
  _EPS = numpy.finfo(float).eps * 4.0
  
  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