def __mul__(self, other): res = Frame() res.translation = self.translation + quat.rotate( self.rotation, other.translation) res.rotation = quat.prod(self.rotation, other.rotation) return res
def prod(x, y): print x.inv() res = np.zeros(7) res[:3] = x[:3] + quat.rotate(x[3:], y[:3]) res[3:] = quat.prod(x[3:], y[3:]) return res
def prod(x, y): print x.inv() res = np.zeros(7) res[:3] = x[:3] + quat.rotate(x[3:], y[:3]) res[3:] = quat.prod(x[3:], y[3:]) return res
def __mul__(self, other): res = Frame() res.translation = vec.sum(self.translation, quat.rotate(self.rotation, other.translation)) res.rotation = quat.prod( self.rotation, other.rotation) return res
def test_inv(q): q_inv = Quaternion.inv(q) return EXPECT_VEC_EQ(Quaternion.id(), Quaternion.prod(q, q_inv), "test_inv")
def rotateAround(self, axis, angle, field="position"): p = self.rigidobject.getData(field) pq = p.value[0] p.value = pq[:3] + list( Quaternion.prod(axisToQuat(axis, angle), pq[3:]))
def rotateAround(self, axis, angle): pq = self.rigidobject.position[0] self.rigidobject.position = pq[:3] + list( Quaternion.prod(axisToQuat(axis, angle), pq[3:]))
def test_inv(q): q_inv=Quaternion.inv(q) return EXPECT_VEC_EQ(Quaternion.id(), Quaternion.prod(q,q_inv), "test_inv")