changed internal quaternion representation to q,p and simplified math

This commit is contained in:
Philip Eisenlohr 2018-12-04 19:20:24 -05:00
parent 1d7172c971
commit a7554891a4
1 changed files with 106 additions and 183 deletions

View File

@ -48,48 +48,42 @@ class Quaternion:
""" """
def __init__(self, def __init__(self,
quatArray = [1.0,0.0,0.0,0.0]): quat = None,
q = 1.0,
p = np.zeros(3,dtype=float)):
"""Initializes to identity unless specified""" """Initializes to identity unless specified"""
(self.w, self.q = quat[0] if quat is not None else q
self.x, self.p = np.array(quat[1:4]) if quat is not None else p
self.y,
self.z ) = quatArray
self.homomorph() self.homomorph()
def __iter__(self): def __iter__(self):
"""Components""" """Components"""
return iter([self.w,self.x,self.y,self.z]) return iter(self.asList())
def __copy__(self): def __copy__(self):
"""Copy""" """Copy"""
Q = Quaternion([self.w,self.x,self.y,self.z]) Q = Quaternion(q=self.q,p=self.p)
return Q return Q
copy = __copy__ copy = __copy__
def __repr__(self): def __repr__(self):
"""Readable string""" """Readable string"""
return 'Quaternion(real=%+.6f, imag=<%+.6f, %+.6f, %+.6f>)' % (self.w, self.x, self.y, self.z) return 'Quaternion(real={q:+.6f}, imag=<{p[0]:+.6f}, {p[1]:+.6f}, {p[2]:+.6f}>)'.format(q=self.q,p=self.p)
def __pow__(self, exponent): def __pow__(self, exponent):
"""Power""" """Power"""
omega = math.acos(self.w)
vRescale = math.sin(exponent*omega)/math.sin(omega)
Q = Quaternion() Q = Quaternion()
Q.w = math.cos(exponent*omega) omega = math.acos(self.q)
Q.x = self.x * vRescale Q.q = math.cos(exponent*omega)
Q.y = self.y * vRescale Q.p = self.p * math.sin(exponent*omega)/math.sin(omega)
Q.z = self.z * vRescale
return Q return Q
def __ipow__(self, exponent): def __ipow__(self, exponent):
"""In-place power""" """In-place power"""
omega = math.acos(self.w) omega = math.acos(self.q[0])
vRescale = math.sin(exponent*omega)/math.sin(omega) self.q = math.cos(exponent*omega)
self.w = np.cos(exponent*omega) self.p *= math.sin(exponent*omega)/math.sin(omega)
self.x *= vRescale
self.y *= vRescale
self.z *= vRescale
return self return self
def __mul__(self, other): def __mul__(self, other):
@ -97,45 +91,20 @@ class Quaternion:
# Rowenhorst_etal2015 MSMSE: value of P is selected as -1 # Rowenhorst_etal2015 MSMSE: value of P is selected as -1
P = -1.0 P = -1.0
try: # quaternion try: # quaternion
Aw = self.w
Ax = self.x
Ay = self.y
Az = self.z
Bw = other.w
Bx = other.x
By = other.y
Bz = other.z
Q = Quaternion() Q = Quaternion()
Q.w = - Ax * Bx - Ay * By - Az * Bz + Aw * Bw Q.q = self.q*other.q - np.dot(self.p,other.p)
Q.x = + Ax * Bw + Aw * Bx + P * (Ay * Bz - Az * By) Q.p = self.q*other.p + other.q*self.p + P * np.cross(self.p,other.p)
Q.y = + Ay * Bw + Aw * By + P * (Az * Bx - Ax * Bz)
Q.z = + Az * Bw + Aw * Bz + P * (Ax * By - Ay * Bx)
return Q return Q
except: pass except: pass
try: # vector (perform active rotation, i.e. q*v*q.conjugated) try: # vector (perform passive rotation)
w = self.w return (self.q*self.q - np.dot(self.p,self.p)) * np.array(other[:3]) \
x = self.x + 2.0*np.dot(self.p,other[:3]) * self.p \
y = self.y + 2.0*P*self.q * np.cross(self.p,other[:3])
z = self.z
Vx = other[0]
Vy = other[1]
Vz = other[2]
A = w**2 - x**2 - y**2 - z**2
B = 2.0*(x*Vx + y*Vy + z*Vz)
return np.array([
A*Vx + B*x + 2*P*w * (y*Vz - z*Vy),
A*Vy + B*y + 2*P*w * (z*Vx - x*Vz),
A*Vz + B*z + 2*P*w * (x*Vy - y*Vx),
])
except: pass except: pass
try: # scalar try: # scalar
Q = self.copy() Q = self.copy()
Q.w *= other Q.q *= other
Q.x *= other Q.p *= other
Q.y *= other
Q.z *= other
return Q return Q
except: except:
return self.copy() return self.copy()
@ -145,69 +114,49 @@ class Quaternion:
# Rowenhorst_etal2015 MSMSE: value of P is selected as -1 # Rowenhorst_etal2015 MSMSE: value of P is selected as -1
P = -1.0 P = -1.0
try: # Quaternion try: # Quaternion
Aw = self.w self.q = self.q*other.q - np.dot(self.p,other.p)
Ax = self.x self.p = self.q*other.p + other.q*self.p + P * np.cross(self.p,other.p)
Ay = self.y
Az = self.z
Bw = other.w
Bx = other.x
By = other.y
Bz = other.z
self.w = - Ax * Bx - Ay * By - Az * Bz + Aw * Bw
self.x = + Ax * Bw + Aw * Bx + P * (Ay * Bz - Az * By)
self.y = + Ay * Bw + Aw * By + P * (Az * Bx - Ax * Bz)
self.z = + Az * Bw + Aw * Bz + P * (Ax * By - Ay * Bx)
except: pass except: pass
return self return self
def __div__(self, other): def __div__(self, other):
"""Division""" """Division"""
if isinstance(other, (int,float)): if isinstance(other, (int,float)):
w = self.w / other q = self.q / other
x = self.x / other p = self.p / other
y = self.y / other return self.__class__(q=q,p=p)
z = self.z / other
return self.__class__([w,x,y,z])
else: else:
return NotImplemented return NotImplemented
def __idiv__(self, other): def __idiv__(self, other):
"""In-place division""" """In-place division"""
if isinstance(other, (int,float)): if isinstance(other, (int,float)):
self.w /= other self.q /= other
self.x /= other self.p /= other
self.y /= other
self.z /= other
return self return self
def __add__(self, other): def __add__(self, other):
"""Addition""" """Addition"""
if isinstance(other, Quaternion): if isinstance(other, Quaternion):
w = self.w + other.w q = self.q + other.q
x = self.x + other.x p = self.p + other.p
y = self.y + other.y return self.__class__(q=q,p=p)
z = self.z + other.z
return self.__class__([w,x,y,z])
else: else:
return NotImplemented return NotImplemented
def __iadd__(self, other): def __iadd__(self, other):
"""In-place addition""" """In-place addition"""
if isinstance(other, Quaternion): if isinstance(other, Quaternion):
self.w += other.w self.q += other.q
self.x += other.x self.p += other.p
self.y += other.y
self.z += other.z
return self return self
def __sub__(self, other): def __sub__(self, other):
"""Subtraction""" """Subtraction"""
if isinstance(other, Quaternion): if isinstance(other, Quaternion):
Q = self.copy() Q = self.copy()
Q.w -= other.w Q.q -= other.q
Q.x -= other.x Q.p -= other.p
Q.y -= other.y
Q.z -= other.z
return Q return Q
else: else:
return self.copy() return self.copy()
@ -215,40 +164,25 @@ class Quaternion:
def __isub__(self, other): def __isub__(self, other):
"""In-place subtraction""" """In-place subtraction"""
if isinstance(other, Quaternion): if isinstance(other, Quaternion):
self.w -= other.w self.q -= other.q
self.x -= other.x self.p -= other.p
self.y -= other.y
self.z -= other.z
return self return self
def __neg__(self): def __neg__(self):
"""Additive inverse""" """Additive inverse"""
self.w = -self.w self.q = -self.q
self.x = -self.x self.p = -self.p
self.y = -self.y
self.z = -self.z
return self return self
def __abs__(self): def __abs__(self):
"""Norm""" """Norm"""
return math.sqrt(self.w ** 2 + \ return math.sqrt(self.q ** 2 + np.dot(self.p,self.p))
self.x ** 2 + \
self.y ** 2 + \
self.z ** 2)
magnitude = __abs__ magnitude = __abs__
def __eq__(self,other): def __eq__(self,other):
"""Equal at e-8 precision""" """Equal at e-8 precision"""
return (abs(self.w-other.w) < 1e-8 and \ return (self-other).magnitude() < 1e-8 or (-self-other).magnitude() < 1e-8
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): def __ne__(self,other):
"""Not equal at e-8 precision""" """Not equal at e-8 precision"""
@ -259,16 +193,11 @@ class Quaternion:
return (self.Rodrigues()>other.Rodrigues()) - (self.Rodrigues()<other.Rodrigues()) return (self.Rodrigues()>other.Rodrigues()) - (self.Rodrigues()<other.Rodrigues())
def magnitude_squared(self): def magnitude_squared(self):
return self.w ** 2 + \ return self.q ** 2 + np.dot(self.p,self.p)
self.x ** 2 + \
self.y ** 2 + \
self.z ** 2
def identity(self): def identity(self):
self.w = 1. self.q = 1.
self.x = 0. self.p = np.zeros(3,dtype=float)
self.y = 0.
self.z = 0.
return self return self
def normalize(self): def normalize(self):
@ -278,9 +207,7 @@ class Quaternion:
return self return self
def conjugate(self): def conjugate(self):
self.x = -self.x self.p = -self.p
self.y = -self.y
self.z = -self.z
return self return self
def inverse(self): def inverse(self):
@ -291,11 +218,9 @@ class Quaternion:
return self return self
def homomorph(self): def homomorph(self):
if self.w < 0.0: if self.q < 0.0:
self.w = -self.w self.q = -self.q
self.x = -self.x self.p = -self.p
self.y = -self.y
self.z = -self.z
return self return self
def normalized(self): def normalized(self):
@ -311,29 +236,35 @@ class Quaternion:
return self.copy().homomorph() return self.copy().homomorph()
def asList(self): def asList(self):
return [i for i in self] return [self.q]+list(self.p)
def asM(self): # to find Averaging Quaternions (see F. Landis Markley et al.) def asM(self): # to find Averaging Quaternions (see F. Landis Markley et al.)
return np.outer([i for i in self],[i for i in self]) return np.outer(self.asList(),self.asList())
def asMatrix(self): def asMatrix(self):
# Rowenhorst_etal2015 MSMSE: value of P is selected as -1 # Rowenhorst_etal2015 MSMSE: value of P is selected as -1
P = -1.0 P = -1.0
qbarhalf = 0.5*(self.w**2 - self.x**2 - self.y**2 - self.z**2) qbarhalf = 0.5*(self.q**2 - np.dot(self.p,self.p))
return 2.0*np.array( return 2.0*np.array(
[[ qbarhalf + self.x**2 , self.x*self.y -P* self.w*self.z, self.x*self.z +P* self.w*self.y], [[ qbarhalf + self.p[0]**2 ,
[ self.x*self.y +P* self.w*self.z, qbarhalf + self.y**2 , self.y*self.z -P* self.w*self.x], self.p[0]*self.p[1] -P* self.q*self.p[2],
[ self.x*self.z -P* self.w*self.y, self.y*self.z +P* self.w*self.x, qbarhalf + self.z**2 ], self.p[0]*self.p[2] +P* self.q*self.p[1] ],
[ self.p[0]*self.p[1] +P* self.q*self.p[2],
qbarhalf + self.p[1]**2 ,
self.p[1]*self.p[2] -P* self.q*self.p[0] ],
[ self.p[0]*self.p[2] -P* self.q*self.p[1],
self.p[1]*self.p[2] +P* self.q*self.p[0],
qbarhalf + self.p[2]**2 ],
]) ])
def asAngleAxis(self, def asAngleAxis(self,
degrees = False): degrees = False):
if self.w > 1: if self.q > 1.:
self.normalize() self.normalize()
s = math.sqrt(1. - self.w**2) s = math.sqrt(1. - self.q**2)
x = 2*self.w**2 - 1. x = 2*self.q**2 - 1.
y = 2*self.w * s y = 2*self.q * s
angle = math.atan2(y,x) angle = math.atan2(y,x)
if angle < 0.0: if angle < 0.0:
@ -341,28 +272,28 @@ class Quaternion:
s *= -1. s *= -1.
return (np.degrees(angle) if degrees else angle, return (np.degrees(angle) if degrees else angle,
np.array([1.0, 0.0, 0.0] if np.abs(angle) < 1e-6 else [self.x / s, self.y / s, self.z / s])) np.array([1.0, 0.0, 0.0] if np.abs(angle) < 1e-6 else self.p / s))
def asRodrigues(self): def asRodrigues(self):
return np.inf*np.ones(3) if self.w == 0.0 else np.array([self.x, self.y, self.z])/self.w return np.inf*np.ones(3) if self.q == 0.0 else self.p/self.q
def asEulers(self, def asEulers(self,
degrees = False): degrees = False):
"""Orientation as Bunge-Euler angles.""" """Orientation as Bunge-Euler angles."""
# Rowenhorst_etal2015 MSMSE: value of P is selected as -1 # Rowenhorst_etal2015 MSMSE: value of P is selected as -1
P = -1.0 P = -1.0
q03 = self.w**2 + self.z**2 q03 = self.q**2 + self.p[2]**2
q12 = self.x**2 + self.y**2 q12 = self.p[0]**2 + self.p[1]**2
chi = np.sqrt(q03*q12) chi = np.sqrt(q03*q12)
if abs(chi) < 1e-10 and abs(q12) < 1e-10: if abs(chi) < 1e-10 and abs(q12) < 1e-10:
eulers = np.array([math.atan2(-2*P*self.w*self.z,self.w**2-self.z**2),0,0]) eulers = np.array([math.atan2(-2*P*self.q*self.p[2],self.q**2-self.p[2]**2),0,0])
elif abs(chi) < 1e-10 and abs(q03) < 1e-10: elif abs(chi) < 1e-10 and abs(q03) < 1e-10:
eulers = np.array([math.atan2( 2 *self.x*self.y,self.x**2-self.y**2),np.pi,0]) eulers = np.array([math.atan2( 2 *self.p[0]*self.p[1],self.p[0]**2-self.p[1]**2),np.pi,0])
else: else:
eulers = np.array([math.atan2((self.x*self.z-P*self.w*self.y)/chi,(-P*self.w*self.x-self.y*self.z)/chi), eulers = np.array([math.atan2((self.p[0]*self.p[2]-P*self.q*self.p[1])/chi,(-P*self.q*self.p[0]-self.p[1]*self.p[2])/chi),
math.atan2(2*chi,q03-q12), math.atan2(2*chi,q03-q12),
math.atan2((P*self.w*self.y+self.x*self.z)/chi,( self.y*self.z-P*self.w*self.x)/chi), math.atan2((P*self.q*self.p[1]+self.p[0]*self.p[2])/chi,( self.p[1]*self.p[2]-P*self.q*self.p[0])/chi),
]) ])
return np.degrees(eulers) if degrees else eulers return np.degrees(eulers) if degrees else eulers
@ -385,7 +316,7 @@ class Quaternion:
x = math.sin(2.0*math.pi*r[1])*math.sqrt(1.0-r[2]) x = math.sin(2.0*math.pi*r[1])*math.sqrt(1.0-r[2])
y = math.cos(2.0*math.pi*r[1])*math.sqrt(1.0-r[2]) y = math.cos(2.0*math.pi*r[1])*math.sqrt(1.0-r[2])
z = math.sin(2.0*math.pi*r[0])*math.sqrt(r[2]) z = math.sin(2.0*math.pi*r[0])*math.sqrt(r[2])
return cls([w,x,y,z]) return cls(quat=[w,x,y,z])
@classmethod @classmethod
@ -393,9 +324,7 @@ class Quaternion:
if not isinstance(rodrigues, np.ndarray): rodrigues = np.array(rodrigues) if not isinstance(rodrigues, np.ndarray): rodrigues = np.array(rodrigues)
halfangle = math.atan(np.linalg.norm(rodrigues)) halfangle = math.atan(np.linalg.norm(rodrigues))
c = math.cos(halfangle) c = math.cos(halfangle)
w = c return cls(q=c,p=rodrigues/c)
x,y,z = rodrigues/c
return cls([w,x,y,z])
@classmethod @classmethod
@ -403,22 +332,19 @@ class Quaternion:
angle, angle,
axis, axis,
degrees = False): degrees = False):
if not isinstance(axis, np.ndarray): axis = np.array(axis,dtype='d') if not isinstance(axis, np.ndarray): axis = np.array(axis,dtype=float)
axis = axis.astype(float)/np.linalg.norm(axis) axis = axis.astype(float)/np.linalg.norm(axis)
angle = np.radians(angle) if degrees else angle angle = np.radians(angle) if degrees else angle
s = math.sin(0.5 * angle) s = math.sin(0.5 * angle)
w = math.cos(0.5 * angle) c = math.cos(0.5 * angle)
x = axis[0] * s return cls(q=c,p=axis*s)
y = axis[1] * s
z = axis[2] * s
return cls([w,x,y,z])
@classmethod @classmethod
def fromEulers(cls, def fromEulers(cls,
eulers, eulers,
degrees = False): degrees = False):
if not isinstance(eulers, np.ndarray): eulers = np.array(eulers,dtype='d') if not isinstance(eulers, np.ndarray): eulers = np.array(eulers,dtype=float)
eulers = np.radians(eulers) if degrees else eulers eulers = np.radians(eulers) if degrees else eulers
sigma = 0.5*(eulers[0]+eulers[2]) sigma = 0.5*(eulers[0]+eulers[2])
@ -432,7 +358,7 @@ class Quaternion:
x = -P * s * np.cos(delta) x = -P * s * np.cos(delta)
y = -P * s * np.sin(delta) y = -P * s * np.sin(delta)
z = -P * c * np.sin(sigma) z = -P * c * np.sin(sigma)
return cls([w,x,y,z]) return cls(quat=[w,x,y,z])
# Modified Method to calculate Quaternion from Orientation Matrix, # Modified Method to calculate Quaternion from Orientation Matrix,
@ -454,7 +380,7 @@ class Quaternion:
y *= -1 if m[0,2] < m[2,0] else 1 y *= -1 if m[0,2] < m[2,0] else 1
z *= -1 if m[1,0] < m[0,1] else 1 z *= -1 if m[1,0] < m[0,1] else 1
return cls(np.array([w,x,y,z])/math.sqrt(w**2 + x**2 + y**2 + z**2)) return cls(quat=np.array([w,x,y,z])/math.sqrt(w**2 + x**2 + y**2 + z**2))
@classmethod @classmethod
@ -468,36 +394,30 @@ class Quaternion:
assert isinstance(q1, Quaternion) and isinstance(q2, Quaternion) assert isinstance(q1, Quaternion) and isinstance(q2, Quaternion)
Q = cls() Q = cls()
costheta = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z costheta = q1.q*q2.q + np.dot(q1.p,q2.p)
if costheta < 0.: if costheta < 0.:
costheta = -costheta costheta = -costheta
q1 = q1.conjugated() q1 = q1.conjugated()
elif costheta > 1: elif costheta > 1.:
costheta = 1 costheta = 1.
theta = math.acos(costheta) theta = math.acos(costheta)
if abs(theta) < 0.01: if abs(theta) < 0.01:
Q.w = q2.w Q.q = q2.q
Q.x = q2.x Q.p = q2.p
Q.y = q2.y
Q.z = q2.z
return Q return Q
sintheta = math.sqrt(1.0 - costheta * costheta) sintheta = math.sqrt(1.0 - costheta * costheta)
if abs(sintheta) < 0.01: if abs(sintheta) < 0.01:
Q.w = (q1.w + q2.w) * 0.5 Q.q = (q1.q + q2.q) * 0.5
Q.x = (q1.x + q2.x) * 0.5 Q.p = (q1.p + q2.p) * 0.5
Q.y = (q1.y + q2.y) * 0.5
Q.z = (q1.z + q2.z) * 0.5
return Q return Q
ratio1 = math.sin((1 - t) * theta) / sintheta ratio1 = math.sin((1.0 - t) * theta) / sintheta
ratio2 = math.sin( t * theta) / sintheta ratio2 = math.sin( t * theta) / sintheta
Q.w = q1.w * ratio1 + q2.w * ratio2 Q.q = q1.q * ratio1 + q2.q * ratio2
Q.x = q1.x * ratio1 + q2.x * ratio2 Q.p = q1.p * ratio1 + q2.p * ratio2
Q.y = q1.y * ratio1 + q2.y * ratio2
Q.z = q1.z * ratio1 + q2.z * ratio2
return Q return Q
@ -523,7 +443,7 @@ class Symmetry:
def __repr__(self): def __repr__(self):
"""Readbable string""" """Readbable string"""
return '%s' % (self.lattice) return '{}'.format(self.lattice)
def __eq__(self, other): def __eq__(self, other):
@ -747,10 +667,10 @@ class Symmetry:
if color: # have to return color array if color: # have to return color array
if inSST: if inSST:
rgb = np.power(theComponents/np.linalg.norm(theComponents),0.5) # smoothen color ramps rgb = np.power(theComponents/np.linalg.norm(theComponents),0.5) # smoothen color ramps
rgb = np.minimum(np.ones(3,'d'),rgb) # limit to maximum intensity rgb = np.minimum(np.ones(3,dtype=float),rgb) # limit to maximum intensity
rgb /= max(rgb) # normalize to (HS)V = 1 rgb /= max(rgb) # normalize to (HS)V = 1
else: else:
rgb = np.zeros(3,'d') rgb = np.zeros(3,dtype=float)
return (inSST,rgb) return (inSST,rgb)
else: else:
return inSST return inSST
@ -790,8 +710,9 @@ class Orientation:
self.quaternion = Quaternion.fromRodrigues(Rodrigues) self.quaternion = Quaternion.fromRodrigues(Rodrigues)
elif isinstance(quaternion, Quaternion): # based on given quaternion elif isinstance(quaternion, Quaternion): # based on given quaternion
self.quaternion = quaternion.homomorphed() self.quaternion = quaternion.homomorphed()
elif isinstance(quaternion, np.ndarray) and quaternion.shape == (4,): # based on given quaternion-like array elif (isinstance(quaternion, np.ndarray) and quaternion.shape == (4,)) or \
self.quaternion = Quaternion(quaternion).homomorphed() (isinstance(quaternion, list) and len(quaternion) == 4 ): # based on given quaternion-like array
self.quaternion = Quaternion(quat=quaternion).homomorphed()
self.symmetry = Symmetry(symmetry) self.symmetry = Symmetry(symmetry)
@ -804,10 +725,12 @@ class Orientation:
def __repr__(self): def __repr__(self):
"""Value as all implemented representations""" """Value as all implemented representations"""
return 'Symmetry: %s\n' % (self.symmetry) + \ return '\n'.join([
'Quaternion: %s\n' % (self.quaternion) + \ 'Symmetry: {}'.format(self.symmetry),
'Matrix:\n%s\n' % ( '\n'.join(['\t'.join(map(str,self.asMatrix()[i,:])) for i in range(3)]) ) + \ 'Quaternion: {}'.format(self.quaternion),
'Bunge Eulers / deg: %s' % ('\t'.join(map(str,self.asEulers(degrees=True))) ) '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): def asQuaternion(self):
return self.quaternion.asList() return self.quaternion.asList()