From 7ee933b79d7749a393083455ee72ea841fbd3895 Mon Sep 17 00:00:00 2001
From: Martin Diehl <mail@martin-diehl.net>
Date: Mon, 11 Feb 2019 23:50:02 +0100
Subject: [PATCH] Rotation class

uses (and hides) Quaternion2. Should replace Quaternion class.
Orientation class should inherit from Rotation and adds symmetry.
---
 python/damask/__init__.py    |   2 +-
 python/damask/orientation.py | 345 +++++++++++++++++++++++++++++++++++
 2 files changed, 346 insertions(+), 1 deletion(-)

diff --git a/python/damask/__init__.py b/python/damask/__init__.py
index c8981069d..a9209a1c6 100644
--- a/python/damask/__init__.py
+++ b/python/damask/__init__.py
@@ -13,7 +13,7 @@ from .asciitable  import ASCIItable       # noqa
     
 from .config      import Material         # noqa
 from .colormaps   import Colormap, Color  # noqa
-from .orientation import Quaternion, Symmetry, Orientation # noqa
+from .orientation import Quaternion, Symmetry, Rotation, Orientation # noqa
 
 #from .block       import Block           # only one class
 from .result      import Result           # noqa
diff --git a/python/damask/orientation.py b/python/damask/orientation.py
index 73b7620ca..9a92c77d4 100644
--- a/python/damask/orientation.py
+++ b/python/damask/orientation.py
@@ -10,6 +10,350 @@ from . import Lambert
 
 P = -1
 
+####################################################################################################
+class Quaternion2:
+    u"""
+    Quaternion with basic operations
+    
+    q is the real part, p = (x, y, z) are the imaginary parts.
+    Defintion of multiplication depends on variable P, P ∉ {-1,1}.
+    """
+
+    def __init__(self,
+                 q    = 0.0,
+                 p    = np.zeros(3,dtype=float)):
+      """Initializes to identity unless specified"""
+      self.q = q
+      self.p = np.array(p)
+
+
+    def __copy__(self):
+      """Copy"""
+      return self.__class__(q=self.q,
+                            p=self.p.copy())
+
+    copy = __copy__
+
+
+    def __iter__(self):
+      """Components"""
+      return iter(self.asList())
+
+    def asArray(self):
+      """As numpy array"""
+      return np.array((self.q,self.p[0],self.p[1],self.p[2]))
+ 
+    def asList(self):
+      return [self.q]+list(self.p)
+
+
+    def __repr__(self):
+      """Readable string"""
+      return 'Quaternion(real={q:+.6f}, imag=<{p[0]:+.6f}, {p[1]:+.6f}, {p[2]:+.6f}>)'.format(q=self.q,p=self.p)
+      
+
+    def __add__(self, other):
+      """Addition"""
+      if isinstance(other, Quaternion2):
+        return self.__class__(q=self.q + other.q,
+                              p=self.p + other.p)
+      else:
+        return NotImplemented
+
+    def __iadd__(self, other):
+      """In-place addition"""
+      if isinstance(other, Quaternion2):
+        self.q += other.q
+        self.p += other.p
+        return self
+      else:
+        return NotImplemented
+      
+    def __pos__(self):                                                                      
+      """Unary positive operator"""                                                                       
+      return self
+      
+      
+    def __sub__(self, other):
+      """Subtraction"""
+      if isinstance(other, Quaternion2):
+        return self.__class__(q=self.q - other.q,
+                              p=self.p - other.p)
+      else:
+        return NotImplemented
+
+    def __isub__(self, other):
+      """In-place subtraction"""
+      if isinstance(other, Quaternion2):
+        self.q -= other.q
+        self.p -= other.p
+        return self
+      else:
+        return NotImplemented
+        
+    def __neg__(self):                                                                      
+      """Unary positive operator"""  
+      self.q *= -1.0
+      self.p *= -1.0                                                                     
+      return self
+      
+      
+    def __mul__(self, other):
+      """Multiplication with quaternion or scalar"""
+      if isinstance(other, Quaternion2):
+        return self.__class__(q=self.q*other.q - np.dot(self.p,other.p),
+                              p=self.q*other.p + other.q*self.p + P * np.cross(self.p,other.p))
+      elif isinstance(other, (int, float)):
+        return self.__class__(q=self.q*other,
+                              p=self.p*other)
+      else:
+        return NotImplemented
+
+    def __imul__(self, other):
+      """In-place multiplication with quaternion or scalar"""
+      if isinstance(other, Quaternion2):
+        self.q = self.q*other.q - np.dot(self.p,other.p)
+        self.p = self.q*other.p + other.q*self.p + P * np.cross(self.p,other.p)
+        return self
+      elif isinstance(other, (int, float)):
+        self *= other
+        return self
+      else:
+        return NotImplemented
+      
+
+    def __truediv__(self, other):
+      """Divsion with quaternion or scalar"""
+      if isinstance(other, Quaternion2):
+        s = other.conjugate()/abs(other)**2.
+        return self.__class__(q=self.q * s,
+                              p=self.p * s)
+      elif isinstance(other, (int, float)):
+        self.q /= other
+        self.p /= other
+        return self
+      else:
+        return NotImplemented
+
+    def __itruediv__(self, other):
+      """In-place divsion with quaternion or scalar"""
+      if isinstance(other, Quaternion2):
+        s = other.conjugate()/abs(other)**2.
+        self *= s
+        return self
+      elif isinstance(other, (int, float)):
+        self.q /= other
+        return self
+      else:
+        return NotImplemented
+
+
+    def __pow__(self, exponent):
+      """Power"""
+      if isinstance(exponent, (int, float)):
+        omega = np.acos(self.q)
+        return self.__class__(q=         np.cos(exponent*omega),
+                              p=self.p * np.sin(exponent*omega)/np.sin(omega))
+      else:
+        return NotImplemented
+
+    def __ipow__(self, exponent):
+      """In-place power"""
+      if isinstance(exponent, (int, float)):
+        omega = np.acos(self.q)
+        self.q  = np.cos(exponent*omega)
+        self.p *= np.sin(exponent*omega)/np.sin(omega)
+      else:
+        return NotImplemented
+      
+
+    def __abs__(self):
+      """Norm"""
+      return math.sqrt(self.q ** 2 + np.dot(self.p,self.p))
+
+    magnitude = __abs__
+
+
+    def __eq__(self,other):
+      """Equal (sufficiently close) to each other"""
+      return np.isclose(( self-other).magnitude(),0.0) \
+          or np.isclose((-self-other).magnitude(),0.0)
+
+    def __ne__(self,other):
+      """Not equal (sufficiently close) to each other"""
+      return not self.__eq__(other)
+
+
+    def normalize(self):
+      d = self.magnitude()
+      if d > 0.0:
+          self.q /= d
+          self.p /= d
+      return self
+      
+    def normalized(self):
+      return self.copy().normalize()
+
+
+    def conjugate(self):
+      self.p = -self.p
+      return self
+      
+    def conjugated(self):
+      return self.copy().conjugate()
+
+
+    def homomorph(self):
+      if self.q < 0.0:
+        self.q = -self.q
+        self.p = -self.p
+      return self
+      
+    def homomorphed(self):
+      return self.copy().homomorph()
+      
+
+
+####################################################################################################
+class Rotation:
+    u"""
+    Orientation stored as unit quaternion.
+    
+    All methods and naming conventions based on Rowenhorst_etal2015
+    Convention 1: coordinate frames are right-handed
+    Convention 2: a rotation angle ω is taken to be positive for a counterclockwise rotation
+                  when viewing from the end point of the rotation axis towards the origin
+    Convention 3: rotations will be interpreted in the passive sense
+    Convention 4: Euler angle triplets are implemented using the Bunge convention,
+                  with the angular ranges as [0, 2π],[0, π],[0, 2π]
+    Convention 5: the rotation angle ω is limited to the interval [0, π]
+    Convention 6: P = -1 (as default)
+    
+    q is the real part, p = (x, y, z) are the imaginary parts.
+
+    Vector "a" (defined in coordinate system "A") is passively rotated
+               resulting in new coordinates "b" when expressed in system "B".
+    b = Q * a
+    b = np.dot(Q.asMatrix(),a)
+    ToDo: Denfine how to 3x3 and 3x3x3x3 matrices
+    """
+
+    __slots__ = ['quaternion']
+    
+    def __init__(self,quaternion = np.array([1.0,0.0,0.0,0.0])):
+      """Initializes to identity unless specified"""
+      self.quaternion = Quaternion2(q=quaternion[0],p=quaternion[1:4])
+      self.quaternion.homomorph() # ToDo: Needed?
+
+    def __repr__(self):
+      """Value in selected representation"""
+      return '\n'.join([
+            'Quaternion: {}'.format(self.quaternion),
+            'Matrix:\n{}'.format( '\n'.join(['\t'.join(list(map(str,self.asMatrix()[i,:]))) for i in range(3)]) ),
+            'Bunge Eulers / deg: {}'.format('\t'.join(list(map(str,self.asEulers(degrees=True)))) ),
+              ])
+
+
+    def asQuaternion(self):
+      return self.quaternion.asArray()
+      
+    def asEulers(self,
+                 degrees = False):
+      return np.degrees(qu2eu(self.quaternion.asArray())) if degrees else qu2eu(self.quaternion.asArray())
+    
+    def asAngleAxis(self,
+                    degrees = False):
+                      
+      ax = qu2ax(self.quaternion.asArray())
+      if degrees: ax[0] = np.degrees(ax[0])
+      
+      return ax
+      
+    def asMatrix(self):
+      return qu2om(self.quaternion.asArray())
+
+    def asRodrigues(self):
+      return qu2ro(self.quaternion.asArray())
+  
+    def asHomochoric(self):
+     return qu2ho(self.quaternion.asArray())
+         
+    def asCubochoric(self):
+      return qu2cu(self.quaternion.asArray())
+      
+      
+  
+    @classmethod
+    def fromQuaternion(cls,
+                      quaternion,
+                      P = -1):
+
+      qu = quaternion
+      if P > 0: qu[1:3] *= -1                                                                       # convert from P=1 to P=-1
+      if qu[0] < 0.0:
+        raise ValueError('Quaternion has negative first component.\n{}'.format(qu[0]))
+      if not np.isclose(np.linalg.norm(qu), 1.0):
+        raise ValueError('Quaternion is not of unit length.\n{} {} {} {}'.format(*qu))
+
+      return cls(qu)
+      
+    @classmethod
+    def fromEulers(cls,
+                   eulers,
+                   degrees = False):
+                     
+      eu = np.radians(eulers) if degrees else eulers
+      if np.any(eu < 0.0) or np.any(eu > 2.0*np.pi) or eu[1] > np.pi:
+        raise ValueError('Euler angles outside of [0..2π],[0..π],[0..2π].\n{} {} {}.'.format(*eu))
+        
+      return cls(eu2qu(eu))
+      
+    @classmethod
+    def fromAngleAxis(cls,
+                      angleAxis,
+                      degrees = False,
+                      P = -1):
+                        
+      ax = angleAxis
+      if P > 0: ax[1:3] *= -1                                                                       # convert from P=1 to P=-1
+      if degrees: ax[0] = np.degrees(ax[0])
+      if ax[0] < 0.0 or ax[0] > np.pi:
+        raise ValueError('Axis angle rotation angle outside of [0..π].\n'.format(ax[0]))
+      if not np.isclose(np.linalg.norm(ax[1:3]), 1.0):
+        raise ValueError('Axis angle rotation axis is not of unit length.\n{} {} {}'.format(*ax[1:3]))
+      
+      return cls(ax2qu(ax))
+      
+    @classmethod
+    def fromMatrix(cls,
+                   matrix):
+      
+      om = matrix 
+      if not np.isclose(np.linalg.det(om),1.0):
+        raise ValueError('matrix is not a proper rotation.\n{}'.format(om))
+      if    not np.isclose(np.dot(om[0],om[1]), 0.0) \
+         or not np.isclose(np.dot(om[1],om[2]), 0.0) \
+         or not np.isclose(np.dot(om[2],om[0]), 0.0):
+        raise ValueError('matrix is not orthogonal.\n{}'.format(om))
+
+      return cls(om2qu(om))
+      
+    @classmethod
+    def fromRodrigues(cls,
+                      rodrigues,
+                      P = -1):
+        
+      ro = rodrigues
+      if P > 0: ro[1:3] *= -1                                                                       # convert from P=1 to P=-1
+      if not np.isclose(np.linalg.norm(ro[1:3]), 1.0):
+        raise ValueError('Rodrigues rotation axis is not of unit length.\n{} {} {}'.format(*ro[1:3]))
+      if ro[0] < 0.0:
+        raise ValueError('Rodriques rotation angle not positive.\n'.format(ro[0]))
+        
+      return cls(ro2qu(ro))
+      
+
+
 # ******************************************************************************************
 class Quaternion:
     u"""
@@ -1383,6 +1727,7 @@ def om2qu(om):
   if any(om2ax(om)[0:3]*qu[1:4] < 0.0): print(om2ax(om),qu) # something is wrong here
   return qu
 
+
 def qu2ax(qu):
   """Quaternion to axis angle"""
   omega = 2.0 * np.arccos(qu[0])