class Transform (object): def __init__ (self, *args): if len (args) == 7: self.translation = np.array (args [0:3]) self.quaternion = Quaternion (args [3:7]) if len (args) == 2: self.quaternion = Quaternion (args [0]) self.translation = np.array (args [1]) if len (args) == 1: if isinstance (args [0], Transform) : self.quaternion = args [0].quaternion self.translation = args [0].translation elif isinstance (args [0], list): self.translation = np.array (args [0][0:3]) self.quaternion = Quaternion (args [0][3:7]) self.quaternion.normalize () def __mul__ (self, other): rot = self.quaternion * other.quaternion trans = self.quaternion.transform (other.translation) +\ self.translation return Transform (rot, trans) def inverse (self): rot = self.quaternion.conjugate() trans = - self.quaternion.conjugate().transform (self.translation) return Transform (rot, trans) def __str__ (self): return "quaternion: %s,\ntranslation: %s"%(self.quaternion, self.translation)
class Transform(object): def __init__(self, *args): if len(args) == 0: self.translation = np.array([0.0, 0.0, 0.0]) self.quaternion = Quaternion() if len(args) == 7: self.translation = np.array(args[0:3]) self.quaternion = Quaternion(args[3:7]) if len(args) == 2: self.quaternion = Quaternion(args[0]) self.translation = np.array(args[1]) if len(args) == 1: if isinstance(args[0], Transform): self.quaternion = args[0].quaternion self.translation = args[0].translation elif isinstance(args[0], (list, tuple)): self.translation = np.array(args[0][0:3]) self.quaternion = Quaternion(args[0][3:7]) self.quaternion.normalize() def __mul__(self, other): rot = self.quaternion * other.quaternion trans = self.quaternion.transform(other.translation) + self.translation return Transform(rot, trans) def inverse(self): rot = self.quaternion.conjugate() trans = -self.quaternion.conjugate().transform(self.translation) return Transform(rot, trans) def transform(self, v): res = self.quaternion.transform(v) + self.translation return res def __str__(self): return "quaternion: %s,\ntranslation: %s" % (self.quaternion, self.translation) def __getitem__(self, i): if i < 3: return self.translation[i] if i < 7: return self.quaternion.toTuple()[i - 3] raise IndexError("index out of range") def __len__(self): return 7 def toHomogeneousMatrix(self): H = np.eye(4) H[:3, :3] = self.quaternion.toRotationMatrix() H[:3, 3] = self.translation return H def toTuple(self): return tuple(self.translation) + self.quaternion.toTuple()
class Transform (object): def __init__ (self, *args): if len (args) == 0: self.translation = np.array ([0.,0.,0.]) self.quaternion = Quaternion () if len (args) == 7: self.translation = np.array (args [0:3]) self.quaternion = Quaternion (args [3:7]) if len (args) == 2: self.quaternion = Quaternion (args [0]) self.translation = np.array (args [1]) if len (args) == 1: if isinstance (args [0], Transform) : self.quaternion = args [0].quaternion self.translation = args [0].translation elif isinstance (args [0], list): self.translation = np.array (args [0][0:3]) self.quaternion = Quaternion (args [0][3:7]) self.quaternion.normalize () def __mul__ (self, other): rot = self.quaternion * other.quaternion trans = self.quaternion.transform (other.translation) +\ self.translation return Transform (rot, trans) def inverse (self): rot = self.quaternion.conjugate() trans = - self.quaternion.conjugate().transform (self.translation) return Transform (rot, trans) def transform (self, v): res = self.quaternion.transform (v) + self.translation return res def __str__ (self): return "quaternion: %s,\ntranslation: %s"%(self.quaternion, self.translation) def __getitem__ (self, i): if i<3: return self.translation [i] if i<7: return self.quaternion.toTuple () [i-3] raise IndexError ("index out of range") def __len__ (self): return 7 def toHomogeneousMatrix (self): H = np.eye(4) H[:3,:3] = self.quaternion.toRotationMatrix() H[:3,3] = self.translation return H def toTuple (self): return tuple (self.translation) + self.quaternion.toTuple ()