def testRotationalQuaternion(self): 'Test the quaternion representation of a rotation.' axis = Vector(1, 1, 1).normalize() angle = 2.0 # radians! q1 = Quaternion.forRotation(axis, angle) vv = math.sin(1.0) / (math.sqrt(3.0)) cc = math.cos(1.0) q2 = Quaternion(cc, vv, vv, vv) assert q1.__str__() == q2.__str__(), '%s %s' % (q1, q2) hitError = False axis = axis.mults(1.2) try: q1 = Quaternion.forRotation(axis, angle) except ValueError, e: assert e.message == 'rotation axis must be a unit vector!' hitError = True
class Quaternion: """Representation of a quaternion, defined as: s + ai + bj + ck or [s,v] where s,a,b,c are scalars, v is a vector, and i, j, k are defined such that: i^2 = j^2 = k^2 = ijk = -1 ij = k, jk = i, ki = j ji = -k, kj = -i, ik = -j """ def __init__(self, s, a, b, c): self.mPrintSpec = '%f' self.mScalar = s self.mVector = Vector(a, b, c) @staticmethod def fromScalarVector(scalar, vector): """Define a quaternion from a scalar and a vector.""" # TODO: Refactor for performance. return Quaternion(scalar, vector[0], vector[1], vector[2]) def clone(self): v = self.mVector[:] return Quaternion(self.mScalar, v[0], v[1], v[2]) def __str__(self): return '[ %s, %s ]' % (self.mPrintSpec % self.mScalar, self.mVector) def str2(self): """Alternate way to represent a Quaternion as a string.""" signs = [ ('+' if f >= 0 else '-') for f in self.mVector ] vals = [ abs(f) for f in self.mVector ] return '%s %s %si %s %sj %s %sk' % (self.mScalar, signs[0], vals[0], signs[1], vals[1], signs[2], vals[2]) def __eq__(self, q): 'Equality operator.' return self.mScalar == q.mScalar and self.mVector == q.mVector def __ne__(self, q): 'Not equals' return not self.__eq__(q) def compare(self, seq): """Compare the quaternion to a sequence assumed to be in the form [ s, a, b, c ].""" return (len(seq) == 4 and self.mScalar == seq[0] and self.mVector[0] == seq[1] and self.mVector[1] == seq[2] and self.mVector[2] == seq[3]) def __add__(self, q): 'Return self + q' return Quaternion(self.mScalar + q.mScalar, self.mVector[0] + q.mVector[0], self.mVector[1] + q.mVector[1], self.mVector[2] + q.mVector[2]) def __sub__(self, q): 'Return self - q' return Quaternion(self.mScalar - q.mScalar, self.mVector[0] - q.mVector[0], self.mVector[1] - q.mVector[1], self.mVector[2] - q.mVector[2]) def scale(self, s): 'Scale this quaternion by scalar s in-place.' self.mScalar = self.mScalar * float(s) self.mVector.scale(s) def mults(self, s): 'Return self * scalar as a new Quaternion.' r = Quaternion.fromScalarVector(self.mScalar, self.mVector) r.scale(s) return r def mul1(self, q): """Multiplication Algorithm 1: This is a very nice definition of the quaternion multiplication operator, but it is terribly inefficient.""" s = self.mScalar * q.mScalar - self.mVector.dot(q.mVector) v = q.mVector.mults(self.mScalar) + \ self.mVector.mults(q.mScalar) + \ self.mVector.cross(q.mVector) return Quaternion.fromScalarVector(s, v) def mul2(self, q): """Multiplication Algorithm 2: This is a much more efficient implementation of quaternion multiplication. It isover 3x faster than mul1.""" s = (self.mScalar * q.mScalar - self.mVector[0] * q.mVector[0] - self.mVector[1] * q.mVector[1] - self.mVector[2] * q.mVector[2]) a = (self.mScalar * q.mVector[0] + self.mVector[0] * q.mScalar + self.mVector[1] * q.mVector[2] - self.mVector[2] * q.mVector[1]) b = (self.mScalar * q.mVector[1] - self.mVector[0] * q.mVector[2] + self.mVector[1] * q.mScalar + self.mVector[2] * q.mVector[0]) c = (self.mScalar * q.mVector[2] + self.mVector[0] * q.mVector[1] - self.mVector[1] * q.mVector[0] + self.mVector[2] * q.mScalar) return Quaternion(s, a, b, c) def mulq(self, q): "Multiply two quaternions and return a new quaternion product." s = (self.mScalar * q.mScalar - self.mVector[0] * q.mVector[0] - self.mVector[1] * q.mVector[1] - self.mVector[2] * q.mVector[2]) a = (self.mScalar * q.mVector[0] + self.mVector[0] * q.mScalar + self.mVector[1] * q.mVector[2] - self.mVector[2] * q.mVector[1]) b = (self.mScalar * q.mVector[1] - self.mVector[0] * q.mVector[2] + self.mVector[1] * q.mScalar + self.mVector[2] * q.mVector[0]) c = (self.mScalar * q.mVector[2] + self.mVector[0] * q.mVector[1] - self.mVector[1] * q.mVector[0] + self.mVector[2] * q.mScalar) return Quaternion(s, a, b, c) def conj(self): 'return the conjugate of a quaternion.' return Quaternion(self.mScalar, -self.mVector[0], -self.mVector[1], -self.mVector[2]) def norm(self): 'return the norm of a quaternion.' return math.sqrt(sum([x*x for x in self.mVector]) + self.mScalar * self.mScalar) def normalize(self): 'reset the quaternion so that it has norm = 1' n_reciprocal = 1.0 / self.norm() self.mScalar = self.mScalar * n_reciprocal self.mVector.scale(n_reciprocal) def inverse(self): """Invert the quaternion and return the inverse. inverse = conjugate / (norm^2) """ n = self.norm() c = self.conj() d = 1.0 / (n * n) c.scale(d) return c def invert(self): 'Invert in place.' n = self.norm() d = 1.0 / (n * n) for i in range(0, 3) : self.mVector[i] *= -d self.mScalar *= d @staticmethod def forRotation(axis, angle): """ Return the quaternion which represents a rotation about the provided axis (vector) by angle (in radians). """ if round(axis.norm(),6) != 1.0: raise ValueError('rotation axis must be a unit vector!') half_angle = angle * 0.5 c = math.cos(half_angle) s = math.sin(half_angle) return Quaternion.fromScalarVector(c, axis.mults(s))