fixed shallow copy problem of quaternion.p object

This commit is contained in:
Philip Eisenlohr 2018-12-05 08:55:26 -05:00
parent a7554891a4
commit e48fd338bc
1 changed files with 20 additions and 25 deletions

View File

@ -62,8 +62,7 @@ class Quaternion:
def __copy__(self): def __copy__(self):
"""Copy""" """Copy"""
Q = Quaternion(q=self.q,p=self.p) return self.__class__(q=self.q,p=self.p.copy())
return Q
copy = __copy__ copy = __copy__
@ -91,10 +90,8 @@ 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
Q = Quaternion() return self.__class__(q=self.q*other.q - np.dot(self.p,other.p),
Q.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))
Q.p = self.q*other.p + other.q*self.p + P * np.cross(self.p,other.p)
return Q
except: pass except: pass
try: # vector (perform passive rotation) try: # vector (perform passive rotation)
return (self.q*self.q - np.dot(self.p,self.p)) * np.array(other[:3]) \ return (self.q*self.q - np.dot(self.p,self.p)) * np.array(other[:3]) \
@ -102,10 +99,8 @@ class Quaternion:
+ 2.0*P*self.q * np.cross(self.p,other[:3]) + 2.0*P*self.q * np.cross(self.p,other[:3])
except: pass except: pass
try: # scalar try: # scalar
Q = self.copy() return self.__class__(q=self.q*other,
Q.q *= other p=self.p*other)
Q.p *= other
return Q
except: except:
return self.copy() return self.copy()
@ -122,9 +117,8 @@ class Quaternion:
def __div__(self, other): def __div__(self, other):
"""Division""" """Division"""
if isinstance(other, (int,float)): if isinstance(other, (int,float)):
q = self.q / other return self.__class__(q=self.q / other,
p = self.p / other p=self.p / other)
return self.__class__(q=q,p=p)
else: else:
return NotImplemented return NotImplemented
@ -138,9 +132,8 @@ class Quaternion:
def __add__(self, other): def __add__(self, other):
"""Addition""" """Addition"""
if isinstance(other, Quaternion): if isinstance(other, Quaternion):
q = self.q + other.q return self.__class__(q=self.q + other.q,
p = self.p + other.p p=self.p + other.p)
return self.__class__(q=q,p=p)
else: else:
return NotImplemented return NotImplemented
@ -154,12 +147,10 @@ class Quaternion:
def __sub__(self, other): def __sub__(self, other):
"""Subtraction""" """Subtraction"""
if isinstance(other, Quaternion): if isinstance(other, Quaternion):
Q = self.copy() return self.__class__(q=self.q - other.q,
Q.q -= other.q p=self.p - other.p)
Q.p -= other.p
return Q
else: else:
return self.copy() return NotImplemented
def __isub__(self, other): def __isub__(self, other):
"""In-place subtraction""" """In-place subtraction"""
@ -190,7 +181,8 @@ class Quaternion:
def __cmp__(self,other): def __cmp__(self,other):
"""Linear ordering""" """Linear ordering"""
return (self.Rodrigues()>other.Rodrigues()) - (self.Rodrigues()<other.Rodrigues()) return (1 if np.linalg.norm(self.asRodrigues()) > np.linalg.norm(other.asRodrigues()) else 0) \
- (1 if np.linalg.norm(self.asRodrigues()) < np.linalg.norm(other.asRodrigues()) else 0)
def magnitude_squared(self): def magnitude_squared(self):
return self.q ** 2 + np.dot(self.p,self.p) return self.q ** 2 + np.dot(self.p,self.p)
@ -203,7 +195,8 @@ class Quaternion:
def normalize(self): def normalize(self):
d = self.magnitude() d = self.magnitude()
if d > 0.0: if d > 0.0:
self /= d self.q /= d
self.p /= d
return self return self
def conjugate(self): def conjugate(self):
@ -322,9 +315,11 @@ class Quaternion:
@classmethod @classmethod
def fromRodrigues(cls, rodrigues): def fromRodrigues(cls, rodrigues):
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)) norm = np.linalg.norm(rodrigues)
halfangle = math.atan(norm)
s = math.sin(halfangle)
c = math.cos(halfangle) c = math.cos(halfangle)
return cls(q=c,p=rodrigues/c) return cls(q=c,p=s*rodrigues/norm)
@classmethod @classmethod