fixed shallow copy problem of quaternion.p object
This commit is contained in:
parent
a7554891a4
commit
e48fd338bc
|
@ -62,8 +62,7 @@ class Quaternion:
|
|||
|
||||
def __copy__(self):
|
||||
"""Copy"""
|
||||
Q = Quaternion(q=self.q,p=self.p)
|
||||
return Q
|
||||
return self.__class__(q=self.q,p=self.p.copy())
|
||||
|
||||
copy = __copy__
|
||||
|
||||
|
@ -91,10 +90,8 @@ class Quaternion:
|
|||
# Rowenhorst_etal2015 MSMSE: value of P is selected as -1
|
||||
P = -1.0
|
||||
try: # quaternion
|
||||
Q = Quaternion()
|
||||
Q.q = self.q*other.q - np.dot(self.p,other.p)
|
||||
Q.p = self.q*other.p + other.q*self.p + P * np.cross(self.p,other.p)
|
||||
return Q
|
||||
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))
|
||||
except: pass
|
||||
try: # vector (perform passive rotation)
|
||||
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])
|
||||
except: pass
|
||||
try: # scalar
|
||||
Q = self.copy()
|
||||
Q.q *= other
|
||||
Q.p *= other
|
||||
return Q
|
||||
return self.__class__(q=self.q*other,
|
||||
p=self.p*other)
|
||||
except:
|
||||
return self.copy()
|
||||
|
||||
|
@ -122,9 +117,8 @@ class Quaternion:
|
|||
def __div__(self, other):
|
||||
"""Division"""
|
||||
if isinstance(other, (int,float)):
|
||||
q = self.q / other
|
||||
p = self.p / other
|
||||
return self.__class__(q=q,p=p)
|
||||
return self.__class__(q=self.q / other,
|
||||
p=self.p / other)
|
||||
else:
|
||||
return NotImplemented
|
||||
|
||||
|
@ -138,9 +132,8 @@ class Quaternion:
|
|||
def __add__(self, other):
|
||||
"""Addition"""
|
||||
if isinstance(other, Quaternion):
|
||||
q = self.q + other.q
|
||||
p = self.p + other.p
|
||||
return self.__class__(q=q,p=p)
|
||||
return self.__class__(q=self.q + other.q,
|
||||
p=self.p + other.p)
|
||||
else:
|
||||
return NotImplemented
|
||||
|
||||
|
@ -154,12 +147,10 @@ class Quaternion:
|
|||
def __sub__(self, other):
|
||||
"""Subtraction"""
|
||||
if isinstance(other, Quaternion):
|
||||
Q = self.copy()
|
||||
Q.q -= other.q
|
||||
Q.p -= other.p
|
||||
return Q
|
||||
return self.__class__(q=self.q - other.q,
|
||||
p=self.p - other.p)
|
||||
else:
|
||||
return self.copy()
|
||||
return NotImplemented
|
||||
|
||||
def __isub__(self, other):
|
||||
"""In-place subtraction"""
|
||||
|
@ -190,7 +181,8 @@ class Quaternion:
|
|||
|
||||
def __cmp__(self,other):
|
||||
"""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):
|
||||
return self.q ** 2 + np.dot(self.p,self.p)
|
||||
|
@ -203,7 +195,8 @@ class Quaternion:
|
|||
def normalize(self):
|
||||
d = self.magnitude()
|
||||
if d > 0.0:
|
||||
self /= d
|
||||
self.q /= d
|
||||
self.p /= d
|
||||
return self
|
||||
|
||||
def conjugate(self):
|
||||
|
@ -322,9 +315,11 @@ class Quaternion:
|
|||
@classmethod
|
||||
def fromRodrigues(cls, 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)
|
||||
return cls(q=c,p=rodrigues/c)
|
||||
return cls(q=c,p=s*rodrigues/norm)
|
||||
|
||||
|
||||
@classmethod
|
||||
|
|
Loading…
Reference in New Issue