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):
|
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
|
||||||
|
|
Loading…
Reference in New Issue