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")
