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