# code derived from http://pyeuclid.googlecode.com/svn/trunk/euclid.py

import math,random

# ******************************************************************************************
class Vector3:
# ******************************************************************************************
    __slots__ = ['x', 'y', 'z']

    def __init__(self, array):
        assert hasattr(array, '__len__') and len(array) == 3
        self.x = array[0]
        self.y = array[1]
        self.z = array[2]

    def __copy__(self):
        return self.__class__(self.x, self.y, self.z)

    copy = __copy__

    def __repr__(self):
        return 'Vector3(%.4f, %.4f, %.4f)' % (self.x,
                                              self.y,
                                              self.z)

    def __eq__(self, other):
        if isinstance(other, Vector3):
            return self.x == other.x and \
                   self.y == other.y and \
                   self.z == other.z
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return self.x == other[0] and \
                   self.y == other[1] and \
                   self.z == other[2]

    def __neq__(self, other):
        return not self.__eq__(other)

    def __cmp__(self,other):
        return cmp((self.z,self.y,self.x),(other.z,other.y,other.x))

    def __nonzero__(self):
        return self.x != 0 or self.y != 0 or self.z != 0

    def __len__(self):
        return 3

    def __getitem__(self, key):
        return (self.x, self.y, self.z)[key]

    def __setitem__(self, key, value):
        l = [self.x, self.y, self.z]
        l[key] = value
        self.x, self.y, self.z = l

    def __iter__(self):
        return iter((self.x, self.y, self.z))

    def __getattr__(self, name):
        try:
            return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \
                          for c in name])
        except ValueError:
            raise AttributeError, name

    def __add__(self, other):
        if isinstance(other, Vector3):
            return Vector3([self.x + other.x,
                            self.y + other.y,
                            self.z + other.z])
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return Vector3([self.x + other[0],
                            self.y + other[1],
                            self.z + other[2]])
    __radd__ = __add__

    def __iadd__(self, other):
        if isinstance(other, Vector3):
            self.x += other.x
            self.y += other.y
            self.z += other.z
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            self.x += other[0]
            self.y += other[1]
            self.z += other[2]
        return self

    def __sub__(self, other):
        if isinstance(other, Vector3):
            return Vector3([self.x - other.x,
                            self.y - other.y,
                            self.z - other.z])
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return Vector3([self.x - other[0],
                            self.y - other[1],
                            self.z - other[2]])

   
    def __rsub__(self, other):
        if isinstance(other, Vector3):
            return Vector3([other.x - self.x,
                            other.y - self.y,
                            other.z - self.z])
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return Vector3([other.x - self[0],
                            other.y - self[1],
                            other.z - self[2]])

    def __mul__(self, other):
        if isinstance(other, Vector3):
            # TODO component-wise mul/div in-place and on Vector2; docs.
            return Vector3([self.x * other.x,
                            self.y * other.y,
                            self.z * other.z])
        else: 
            assert type(other) in (int, long, float)
            return Vector3([self.x * other,
                            self.y * other,
                            self.z * other])

    __rmul__ = __mul__

    def __imul__(self, other):
        assert type(other) in (int, long, float)
        self.x *= other
        self.y *= other
        self.z *= other
        return self

    def __div__(self, other):
        assert type(other) in (int, long, float)
        self.x /= other
        self.y /= other
        self.z /= other
        return self


    def __rdiv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3([operator.div(other, self.x),
                        operator.div(other, self.y),
                        operator.div(other, self.z)])

    def __floordiv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3([operator.floordiv(self.x, other),
                        operator.floordiv(self.y, other),
                        operator.floordiv(self.z, other)])


    def __rfloordiv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3([operator.floordiv(other, self.x),
                        operator.floordiv(other, self.y),
                        operator.floordiv(other, self.z)])

    def __truediv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3([operator.truediv(self.x, other),
                        operator.truediv(self.y, other),
                        operator.truediv(self.z, other)])


    def __rtruediv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3([operator.truediv(other, self.x),
                        operator.truediv(other, self.y),
                        operator.truediv(other, self.z)])
    
    def __neg__(self):
        return Vector3([-self.x,
                        -self.y,
                        -self.z])

    __pos__ = __copy__
    
    def __abs__(self):
        return math.sqrt(self.x ** 2 + \
                         self.y ** 2 + \
                         self.z ** 2)

    magnitude = __abs__

    def magnitude_squared(self):
        return self.x ** 2 + \
               self.y ** 2 + \
               self.z ** 2

    def normalize(self):
        d = self.magnitude()
        if d:
            self.x /= d
            self.y /= d
            self.z /= d
        return self

    def normalized(self):
        d = self.magnitude()
        if d:
            return Vector3([self.x / d, 
                            self.y / d, 
                            self.z / d])
        return self.copy()

    def dot(self, other):
      if isinstance(other, Vector3):
        return self.x * other.x + \
               self.y * other.y + \
               self.z * other.z
      else:
        assert hasattr(other, '__len__') and len(other) == 3
        return self.x * other[0] + \
               self.y * other[1] + \
               self.z * other[2]

    def cross(self, other):
      if isinstance(other, Vector3):
        return Vector3([  self.y * other.z - self.z * other.y,
                        - self.x * other.z + self.z * other.x,
                          self.x * other.y - self.y * other.x])
      else:
        assert hasattr(other, '__len__') and len(other) == 3
        return Vector3([  self.y * other[2] - self.z * other[1],
                        - self.x * other[2] + self.z * other[0],
                          self.x * other[1] - self.y * other[0]])

    def reflect(self, normal):
        # assume normal is normalized
      if isinstance(other, Vector3):
        d = 2 * (self.x * normal.x + self.y * normal.y + self.z * normal.z)
        return Vector3([self.x - d * normal.x,
                        self.y - d * normal.y,
                        self.z - d * normal.z])
      else:
        assert hasattr(other, '__len__') and len(other) == 3
        d = 2 * (self.x * normal[0] + self.y * normal[1] + self.z * normal[2])
        return Vector3([self.x - d * normal[0],
                        self.y - d * normal[1],
                        self.z - d * normal[2]])

    def inSST(self,symmetry):
        return {'cubic':       lambda R: R.inFZ('cubic')       and R.x >= R.y and R.y >= R.z and R.z >= 0.0,
                'hexagonal':   lambda R: R.inFZ('hexagonal')   and R.x >= math.sqrt(3)*R.y and R.y >= 0.0 and R.z >= 0.0,
                'tetragonal':  lambda R: R.inFZ('tetragonal')  and R.x >= R.y and R.y >= 0.0 and R.z >= 0.0,
                'orthorombic': lambda R: R.inFZ('orthorombic') and R.x >= 0.0 and R.y >= 0.0 and R.z >= 0.0,
               }.get(symmetry.lower(),False)(self)

    def inFZ(self,symmetry):
        return {'cubic':       lambda R:     math.sqrt(2.0)-1.0 >= abs(R.x) \
                                         and math.sqrt(2.0)-1.0 >= abs(R.y) \
                                         and math.sqrt(2.0)-1.0 >= abs(R.z) \
                                         and 1.0 >= abs(R.x) + abs(R.y) + abs(R.z),
                'hexagonal':   lambda R:     1.0 >= abs(R.x) and 1.0 >= abs(R.y) and 1.0 >= abs(R.z) \
                                         and 2.0 >= math.sqrt(3)*abs(R.x) + abs(R.y) \
                                         and 2.0 >= math.sqrt(3)*abs(R.y) + abs(R.x) \
                                         and 2.0 >= math.sqrt(3) + abs(R.z),
                'tetragonal':  lambda R:     1.0 >= abs(R.x) and 1.0 >= abs(R.y) \
                                         and math.sqrt(2.0) >= abs(R.x) + abs(R.y) \
                                         and math.sqrt(2.0) >= abs(R.z) + 1.0,
                'orthorombic': lambda R:     1.0 >= abs(R.x) and 1.0 >= abs(R.y) and 1.0 >= abs(R.z),
               }.get(symmetry.lower(),False)(self)

# ******************************************************************************************
class Quaternion:
# ******************************************************************************************
    # All methods and naming conventions based off 
    # http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions

    # w is the real part, (x, y, z) are the imaginary parts
    __slots__ = ['w', 'x', 'y', 'z']

    def __init__(self, quatArray=[1.0,0.0,0.0,0.0]):
        self.w, \
        self.x, \
        self.y, \
        self.z = quatArray

    def __iter__(self):
        return iter([self.w,self.x,self.y,self.z])
        
    def __copy__(self):
        Q = Quaternion([self.w,self.x,self.y,self.z])
        return Q

    copy = __copy__

    def __repr__(self):
        return 'Quaternion(real=%.4f, imag=<%.4f, %.4f, %.4f>)' % \
            (self.w, self.x, self.y, self.z)

    def __mul__(self, other):
        if isinstance(other, Quaternion):
            Ax = self.x
            Ay = self.y
            Az = self.z
            Aw = self.w
            Bx = other.x
            By = other.y
            Bz = other.z
            Bw = other.w
            Q = Quaternion()
            Q.x = + Ax * Bw + Ay * Bz - Az * By + Aw * Bx    
            Q.y = - Ax * Bz + Ay * Bw + Az * Bx + Aw * By
            Q.z = + Ax * By - Ay * Bx + Az * Bw + Aw * Bz
            Q.w = - Ax * Bx - Ay * By - Az * Bz + Aw * Bw
            return Q
        elif isinstance(other, Vector3):    # active rotation of vector (i.e. within fixed coordinate system)
            w = self.w
            x = self.x
            y = self.y
            z = self.z
            Vx = other.x
            Vy = other.y
            Vz = other.z
                
            return other.__class__(\
               w * w * Vx + 2 * y * w * Vz - 2 * z * w * Vy + \
               x * x * Vx + 2 * y * x * Vy + 2 * z * x * Vz - \
               z * z * Vx - y * y * Vx,
               2 * x * y * Vx + y * y * Vy + 2 * z * y * Vz + \
               2 * w * z * Vx - z * z * Vy + w * w * Vy - \
               2 * x * w * Vz - x * x * Vy,
               2 * x * z * Vx + 2 * y * z * Vy + \
               z * z * Vz - 2 * w * y * Vx - y * y * Vz + \
               2 * w * x * Vy - x * x * Vz + w * w * Vz)
        elif isinstance(other, (int,float,long)):
            Q = self.copy()
            Q.w *= other
            Q.x *= other
            Q.y *= other
            Q.z *= other
            return Q
        else:
            return self.copy()

    def __imul__(self, other):
        if isinstance(other, Quaternion):
            Ax = self.x
            Ay = self.y
            Az = self.z
            Aw = self.w
            Bx = other.x
            By = other.y
            Bz = other.z
            Bw = other.w
            self.x =  Ax * Bw + Ay * Bz - Az * By + Aw * Bx    
            self.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By
            self.z =  Ax * By - Ay * Bx + Az * Bw + Aw * Bz
            self.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw
        return self

    def __div__(self, other):
        if isinstance(other, (int,float,long)):
            Q = self.copy()
            Q.w /= other
            Q.x /= other
            Q.y /= other
            Q.z /= other
            return Q
        else:
            return self.copy()

    def __idiv__(self, other):
        if isinstance(other, (int,float,long)):
            self.w /= other
            self.x /= other
            self.y /= other
            self.z /= other
        return self

    def __div__(self, other):
        if isinstance(other, (int,float,long)):
            Q = self.copy()
            Q.w /= other
            Q.x /= other
            Q.y /= other
            Q.z /= other
            return Q
        else:
            return self.copy()

    def __idiv__(self, other):
        if isinstance(other, (int,float,long)):
            self.w /= other
            self.x /= other
            self.y /= other
            self.z /= other
        return self

    def __add__(self, other):
        if isinstance(other, Quaternion):
            Q = self.copy()
            Q.w += other.w
            Q.x += other.x
            Q.y += other.y
            Q.z += other.z
            return Q
        else:
            return self.copy()

    def __iadd__(self, other):
        if isinstance(other, Quaternion):
            self.w += other.w
            self.x += other.x
            self.y += other.y
            self.z += other.z
        return self

    def __sub__(self, other):
        if isinstance(other, Quaternion):
            Q = self.copy()
            Q.w -= other.w
            Q.x -= other.x
            Q.y -= other.y
            Q.z -= other.z
            return Q
        else:
            return self.copy()

    def __isub__(self, other):
        if isinstance(other, Quaternion):
            self.w -= other.w
            self.x -= other.x
            self.y -= other.y
            self.z -= other.z
        return self

    def __neg__(self):
        self.w = -self.w
        self.x = -self.x
        self.y = -self.y
        self.z = -self.z
        return self

    def __abs__(self):
        return math.sqrt(self.w ** 2 + \
                         self.x ** 2 + \
                         self.y ** 2 + \
                         self.z ** 2)

    magnitude = __abs__

    def __eq__(self,other):
        return (abs(self.w-other.w) < 1e-8 and \
                abs(self.x-other.x) < 1e-8 and \
                abs(self.y-other.y) < 1e-8 and \
                abs(self.z-other.z) < 1e-8) \
                or \
               (abs(-self.w-other.w) < 1e-8 and \
                abs(-self.x-other.x) < 1e-8 and \
                abs(-self.y-other.y) < 1e-8 and \
                abs(-self.z-other.z) < 1e-8) 

    def __ne__(self,other):
        return not __eq__(self,other)

    def __cmp__(self,other):
        return cmp(self.Rodrigues(),other.Rodrigues())

    def magnitude_squared(self):
        return self.w ** 2 + \
               self.x ** 2 + \
               self.y ** 2 + \
               self.z ** 2 

    def identity(self):
        self.w = 1
        self.x = 0
        self.y = 0
        self.z = 0
        return self

    def rotateBy_angleaxis(self, angle, axis):
        self *= Quaternion.from_angleaxis(angle, axis)
        return self

    def rotateBy_Eulers(self, heading, attitude, bank):
        self *= Quaternion.from_Eulers(eulers, type)
        return self

    def rotateBy_matrix(self, m):
        self *= Quaternion.from_matrix(m)
        return self

    def normalize(self):
        d = self.magnitude()
        if d != 0:
            self /= d
        return self

    def conjugate(self):
        self.x = -self.x
        self.y = -self.y
        self.z = -self.z
        return self

    def inverse(self):
        d = self.magnitude()
        if d != 0:
          self.conjugate()
          self /= d
        return self

    def reduce(self, symmetry='cubic'):
        for q in self.symmetricEquivalents(symmetry):
          if q.inFZ(symmetry): break
        self.w = q.w
        self.x = q.x
        self.y = q.y
        self.z = q.z
        return self

    def normalized(self):
        return self.copy().normalize()

    def conjugated(self):
        return self.copy().conjugate()

    def inversed(self):
        return self.copy().inverse()

    def reduced(self):
        return self.copy().reduce()

    def angleaxis(self):
        if self.w > 1:
            self.normalize()
        angle = 2 * math.acos(self.w)
        s = math.sqrt(1 - self.w ** 2)
        if s < 0.001:
            return angle, Vector3([1, 0, 0])
        else:
            return angle, Vector3([self.x / s, self.y / s, self.z / s])

    def Rodrigues(self):
        if self.w != 0.0:
            return Vector3([self.x / self.w, self.y / self.w, self.z / self.w])
        else:
            return Vector3([float('inf')]*3)

    def Eulers(self,type='bunge'):
        angles = [0.0,0.0,0.0]
        if type.lower() == 'bunge' or type.lower() == 'zxz':
          angles[0] =  math.atan2( self.x*self.z+self.y*self.w,
                                  -self.y*self.z+self.x*self.w)
#          angles[1] =  math.acos(-self.x**2-self.y**2+self.z**2+self.w**2)
          angles[1] =  math.acos(1.0 - 2*(self.x**2+self.y**2))
          angles[2] =  math.atan2( self.x*self.z-self.y*self.w,
                                  +self.y*self.z+self.x*self.w)

          if angles[0] < 0.0:
            angles[0] += 2*math.pi
          if angles[1] < 0.0:
            angles[1] += math.pi
            angles[2] *= -1
          if angles[2] < 0.0:
            angles[2] += 2*math.pi
        return angles

    def inSST(self,symmetry):
      return self.Rodrigues().inSST(symmetry)

    def inFZ(self,symmetry):
      return self.Rodrigues().inFZ(symmetry)

    def symmetricEquivalents(self,symmetry):
      symQuats = {'cubic':
                      [
                        [ 1.0,0.0,0.0,0.0 ],
                        [ 0.0,-1.0,0.0,0.0 ],
                        [ 0.0,0.0,-1.0,0.0 ],
                        [ 0.0,0.0,0.0,1.0 ],
                        [ 0.0,0.0,0.5*math.sqrt(2), 0.5*math.sqrt(2) ],
                        [ 0.0,0.0,0.5*math.sqrt(2),-0.5*math.sqrt(2) ],
                        [ 0.0, 0.5*math.sqrt(2),-0.5*math.sqrt(2),0.0 ],
                        [ 0.0,-0.5*math.sqrt(2),-0.5*math.sqrt(2),0.0 ],
                        [ 0.0, 0.5*math.sqrt(2), 0.0, 0.5*math.sqrt(2) ],
                        [ 0.0, 0.5*math.sqrt(2), 0.0,-0.5*math.sqrt(2) ],
                        [  0.5, 0.5, 0.5, 0.5 ],
                        [ -0.5, 0.5, 0.5, 0.5 ],
                        [ -0.5, 0.5, 0.5,-0.5 ],
                        [ -0.5, 0.5,-0.5, 0.5 ],
                        [ -0.5,-0.5, 0.5, 0.5 ],
                        [ -0.5,-0.5, 0.5,-0.5 ],
                        [ -0.5,-0.5,-0.5, 0.5 ],
                        [ -0.5, 0.5,-0.5,-0.5 ],
                        [ -0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ],
                        [  0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ],
                        [ -0.5*math.sqrt(2),0.0,-0.5*math.sqrt(2),0.0 ],
                        [ -0.5*math.sqrt(2),0.0, 0.5*math.sqrt(2),0.0 ],
                        [ -0.5*math.sqrt(2), 0.5*math.sqrt(2),0.0,0.0 ],
                        [ -0.5*math.sqrt(2),-0.5*math.sqrt(2),0.0,0.0 ],
                      ],
                  'hexagonal':
                      [
                        [ 1.0,0.0,0.0,0.0],
                        [ 0.0,1.0,0.0,0.0],
                        [ 0.0,0.0,1.0,0.0],
                        [ 0.0,0.0,0.0,1.0],
                        [ 0.0, 0.5,-0.5*math.sqrt(3),0.0],
                        [ 0.0,-0.5,-0.5*math.sqrt(3),0.0],
                        [ 0.5,0.0,0.0,0.5*math.sqrt(3)],
                        [-0.5,0.0,0.0,0.5*math.sqrt(3)],
                        [-0.5*math.sqrt(3),0.0,0.0, 0.5],
                        [-0.5*math.sqrt(3),0.0,0.0,-0.5],
                        [0.0, 0.5*math.sqrt(3), 0.5,0.0],
                        [0.0,-0.5*math.sqrt(3), 0.5,0.0],
                      ],
                  'tetragonal':
                      [
                        [ 0.0,0.0,1.0,0.0 ],
                        [ 0.0, 0.5*math.sqrt(2),0.5*math.sqrt(2),0.0 ],
                        [ 0.0,-0.5*math.sqrt(2),0.5*math.sqrt(2),0.0 ],
                        [ 0.0,0.0,0.0,1.0 ],
                        [ 0.0,1.0,0.0,0.0 ],
                        [  0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ],
                        [ -0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ],
                        [ 1.0,0.0,0.0,0.0 ],
                      ],
                  'orthorombic':
                      [
                        [ 0.0,0.0,1.0,0.0 ],
                        [ 0.0,0.0,0.0,1.0 ],
                        [ 0.0,1.0,0.0,0.0 ],
                        [ 1.0,0.0,0.0,0.0 ],
                      ],
               }
      equivalents = [self*Quaternion(q) for q in symQuats[symmetry.lower()]]
      return equivalents


    def disorientation(self,other,symmetry):

      QinSST = Quaternion()
      breaker = False
      for me in self.symmetricEquivalents(symmetry):
        me.inverse()
        for they in other.symmetricEquivalents(symmetry):
          theQ = me * they
          if theQ.w < 0.0: theQ.w = -theQ.w
          if theQ.inSST(symmetry):
            QinSST = theQ.copy()
            breaker = True
            break
        if breaker: break
        
      return QinSST

#    # Static constructors
    def new_identity(cls):
        return cls()
    new_identity = classmethod(new_identity)

    def from_random(cls):
        r1 = random.random()
        r2 = random.random()
        r3 = random.random()
        Q = cls()
        Q.w = math.cos(2.0*math.pi*r1)*math.sqrt(r3)
        Q.x = math.sin(2.0*math.pi*r2)*math.sqrt(1.0-r3)
        Q.y = math.cos(2.0*math.pi*r2)*math.sqrt(1.0-r3)
        Q.z = math.sin(2.0*math.pi*r1)*math.sqrt(r3)
        return Q
    from_random = classmethod(from_random)

    def from_Rodrigues(cls, rodrigues):
        if not isinstance(rodrigues, Vector3): rodrigues = Vector3(rodrigues)
        halfangle = math.atan(rodrigues.magnitude())
        c = math.cos(halfangle)
        Q = cls()
        Q.w = c
        Q.x = rodrigues.x * c
        Q.y = rodrigues.y * c
        Q.z = rodrigues.z * c
        return Q
    from_Rodrigues = classmethod(from_Rodrigues)

    def from_angleaxis(cls, angle, axis):
        assert(isinstance(axis, Vector3))
        axis = axis.normalized()
        s = math.sin(angle / 2)
        Q = cls()
        Q.w = math.cos(angle / 2)
        Q.x = axis.x * s
        Q.y = axis.y * s
        Q.z = axis.z * s
        return Q
    from_angleaxis = classmethod(from_angleaxis)

    def from_Eulers(cls, eulers, type = 'Bunge'):
        c1 = math.cos(eulers[0] / 2.0)
        s1 = math.sin(eulers[0] / 2.0)
        c2 = math.cos(eulers[1] / 2.0)
        s2 = math.sin(eulers[1] / 2.0)
        c3 = math.cos(eulers[2] / 2.0)
        s3 = math.sin(eulers[2] / 2.0)
        Q = cls()

        if type.lower() == 'bunge' or type.lower() == 'zxz':
          Q.w =   c1 * c2 * c3 - s1 * c2 * s3
          Q.x =   c1 * s2 * c3 + s1 * s2 * s3
          Q.y = - c1 * s2 * s3 + s1 * s2 * c3
          Q.z =   c1 * c2 * s3 + s1 * c2 * c3
        else:
          print 'unknown Euler convention'
          Q.w = c1 * c2 * c3 - s1 * s2 * s3
          Q.x = s1 * s2 * c3 + c1 * c2 * s3
          Q.y = s1 * c2 * c3 + c1 * s2 * s3
          Q.z = c1 * s2 * c3 - s1 * c2 * s3
        return Q
    from_Eulers = classmethod(from_Eulers)
    
    def from_matrix(cls, m):
      if m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] > 0.00000001:
        t = m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] + 1.0
        s = 0.5/math.sqrt(t)
        
        return cls(
          s*t,
          (m[1*4 + 2] - m[2*4 + 1])*s,
          (m[2*4 + 0] - m[0*4 + 2])*s,
          (m[0*4 + 1] - m[1*4 + 0])*s
          )
        
      elif m[0*4 + 0] > m[1*4 + 1] and m[0*4 + 0] > m[2*4 + 2]:
        t = m[0*4 + 0] - m[1*4 + 1] - m[2*4 + 2] + 1.0
        s = 0.5/math.sqrt(t)
        
        return cls(
          (m[1*4 + 2] - m[2*4 + 1])*s,
          s*t,
          (m[0*4 + 1] + m[1*4 + 0])*s,
          (m[2*4 + 0] + m[0*4 + 2])*s
          )
        
      elif m[1*4 + 1] > m[2*4 + 2]:
        t = -m[0*4 + 0] + m[1*4 + 1] - m[2*4 + 2] + 1.0
        s = 0.5/math.sqrt(t)
        
        return cls(
          (m[2*4 + 0] - m[0*4 + 2])*s,
          (m[0*4 + 1] + m[1*4 + 0])*s,
          s*t,
          (m[1*4 + 2] + m[2*4 + 1])*s
          )
        
      else:
        t = -m[0*4 + 0] - m[1*4 + 1] + m[2*4 + 2] + 1.0
        s = 0.5/math.sqrt(t)
        
        return cls(
          (m[0*4 + 1] - m[1*4 + 0])*s,
          (m[2*4 + 0] + m[0*4 + 2])*s,
          (m[1*4 + 2] + m[2*4 + 1])*s,
          s*t
          )
    from_matrix = classmethod(from_matrix)
    
    def new_interpolate(cls, q1, q2, t):
        assert isinstance(q1, Quaternion) and isinstance(q2, Quaternion)
        Q = cls()

        costheta = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z
        if costheta < 0.:
            costheta = -costheta
            q1 = q1.conjugated()
        elif costheta > 1:
            costheta = 1

        theta = math.acos(costheta)
        if abs(theta) < 0.01:
            Q.w = q2.w
            Q.x = q2.x
            Q.y = q2.y
            Q.z = q2.z
            return Q

        sintheta = math.sqrt(1.0 - costheta * costheta)
        if abs(sintheta) < 0.01:
            Q.w = (q1.w + q2.w) * 0.5
            Q.x = (q1.x + q2.x) * 0.5
            Q.y = (q1.y + q2.y) * 0.5
            Q.z = (q1.z + q2.z) * 0.5
            return Q

        ratio1 = math.sin((1 - t) * theta) / sintheta
        ratio2 = math.sin(t * theta) / sintheta

        Q.w = q1.w * ratio1 + q2.w * ratio2
        Q.x = q1.x * ratio1 + q2.x * ratio2
        Q.y = q1.y * ratio1 + q2.y * ratio2
        Q.z = q1.z * ratio1 + q2.z * ratio2
        return Q
    new_interpolate = classmethod(new_interpolate)