rotating coordinate system via a quaternion
This question and the answer given by @senderle really helped me with one of my projects. The answer is minimal and covers the core of most quaternion computations that one might need to perform.
For my own project, I found it tedious to have separate functions for all the operations and import them one by one every time I need one, so I implemented an object oriented version.
quaternion.py:
import numpy as np
from math import sin, cos, acos, sqrt
def normalize(v, tolerance=0.00001):
mag2 = sum(n * n for n in v)
if abs(mag2 - 1.0) > tolerance:
mag = sqrt(mag2)
v = tuple(n / mag for n in v)
return np.array(v)
class Quaternion:
def from_axisangle(theta, v):
theta = theta
v = normalize(v)
new_quaternion = Quaternion()
new_quaternion._axisangle_to_q(theta, v)
return new_quaternion
def from_value(value):
new_quaternion = Quaternion()
new_quaternion._val = value
return new_quaternion
def _axisangle_to_q(self, theta, v):
x = v[0]
y = v[1]
z = v[2]
w = cos(theta/2.)
x = x * sin(theta/2.)
y = y * sin(theta/2.)
z = z * sin(theta/2.)
self._val = np.array([w, x, y, z])
def __mul__(self, b):
if isinstance(b, Quaternion):
return self._multiply_with_quaternion(b)
elif isinstance(b, (list, tuple, np.ndarray)):
if len(b) != 3:
raise Exception(f"Input vector has invalid length {len(b)}")
return self._multiply_with_vector(b)
else:
raise Exception(f"Multiplication with unknown type {type(b)}")
def _multiply_with_quaternion(self, q2):
w1, x1, y1, z1 = self._val
w2, x2, y2, z2 = q2._val
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
result = Quaternion.from_value(np.array((w, x, y, z)))
return result
def _multiply_with_vector(self, v):
q2 = Quaternion.from_value(np.append((0.0), v))
return (self * q2 * self.get_conjugate())._val[1:]
def get_conjugate(self):
w, x, y, z = self._val
result = Quaternion.from_value(np.array((w, -x, -y, -z)))
return result
def __repr__(self):
theta, v = self.get_axisangle()
return f"((%.6f; %.6f, %.6f, %.6f))"%(theta, v[0], v[1], v[2])
def get_axisangle(self):
w, v = self._val[0], self._val[1:]
theta = acos(w) * 2.0
return theta, normalize(v)
def tolist(self):
return self._val.tolist()
def vector_norm(self):
w, v = self.get_axisangle()
return np.linalg.norm(v)
In this version, one can just use the overloaded operators for quaternion-quaternion and quaternion-vector multiplication
from quaternion import Quaternion
import numpy as np
x_axis_unit = (1, 0, 0)
y_axis_unit = (0, 1, 0)
z_axis_unit = (0, 0, 1)
r1 = Quaternion.from_axisangle(np.pi / 2, x_axis_unit)
r2 = Quaternion.from_axisangle(np.pi / 2, y_axis_unit)
r3 = Quaternion.from_axisangle(np.pi / 2, z_axis_unit)
# Quaternion - vector multiplication
v = r1 * y_axis_unit
v = r2 * v
v = r3 * v
print(v)
# Quaternion - quaternion multiplication
r_total = r3 * r2 * r1
v = r_total * y_axis_unit
print(v)
I did not intend to implement a full-fledged quaternion module, so this is again for instructional purposes, as in @senderle's great answer. I hope this helps out to those who want to understand and try out new things with quaternions.
Using quaternions to represent rotation isn't difficult from an algebraic point of view. Personally, I find it hard to reason visually about quaternions, but the formulas involved in using them for rotations are not overly complicated. I'll provide a basic set of reference functions here.1 (See also this lovely answer by hosolmaz, in which he packages these together to create a handy Quaternion class.)
You can think of a quaternion (for our purposes) as a scalar plus a 3-d vector -- abstractly, w + xi + yj + zk, here represented by an ordinary tuple (w, x, y, z). The space of 3-d rotations is represented in full by a sub-space of the quaternions, the space of unit quaternions, so you want to make sure that your quaternions are normalized. You can do so in just the way you would normalize any 4-vector (i.e. magnitude should be close to 1; if it isn't, scale down the values by the magnitude):
def normalize(v, tolerance=0.00001):
mag2 = sum(n * n for n in v)
if abs(mag2 - 1.0) > tolerance:
mag = sqrt(mag2)
v = tuple(n / mag for n in v)
return v
Please note that for simplicity, the following functions assume that quaternion values are already normalized. In practice, you'll need to renormalize them from time to time, but the best way to deal with that will depend on the problem domain. These functions provide just the very basics, for reference purposes only.
Every rotation is represented by a unit quaternion, and concatenations of rotations correspond to multiplications of unit quaternions. The formula2 for this is as follows:
def q_mult(q1, q2):
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
To rotate a vector by a quaternion, you need the quaternion's conjugate too:
def q_conjugate(q):
w, x, y, z = q
return (w, -x, -y, -z)
Quaternion-vector multiplication is then a matter of converting your vector into a quaternion (by setting w = 0 and leaving x, y, and z the same) and then multiplying q * v * q_conjugate(q):
def qv_mult(q1, v1):
q2 = (0.0,) + v1
return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]
Finally, you need to know how to convert from axis-angle rotations to quaternions and back. This is also surprisingly straightforward. It makes sense to "sanitize" input and output here by calling normalize.
def axisangle_to_q(v, theta):
v = normalize(v)
x, y, z = v
theta /= 2
w = cos(theta)
x = x * sin(theta)
y = y * sin(theta)
z = z * sin(theta)
return w, x, y, z
And back:
def q_to_axisangle(q):
w, v = q[0], q[1:]
theta = acos(w) * 2.0
return normalize(v), theta
Here's a quick usage example. A sequence of 90-degree rotations about the x, y, and z axes will return a vector on the y axis to its original position. This code performs those rotations:
x_axis_unit = (1, 0, 0)
y_axis_unit = (0, 1, 0)
z_axis_unit = (0, 0, 1)
r1 = axisangle_to_q(x_axis_unit, numpy.pi / 2)
r2 = axisangle_to_q(y_axis_unit, numpy.pi / 2)
r3 = axisangle_to_q(z_axis_unit, numpy.pi / 2)
v = qv_mult(r1, y_axis_unit)
v = qv_mult(r2, v)
v = qv_mult(r3, v)
print v
# output: (0.0, 1.0, 2.220446049250313e-16)
Keep in mind that this sequence of rotations won't return all vectors to the same position; for example, for a vector on the x axis, it will correspond to a 90 degree rotation about the y axis. (Think of the right-hand-rule here; a positive rotation about the y axis pushes a vector on the x axis into the negative z region.)
v = qv_mult(r1, x_axis_unit)
v = qv_mult(r2, v)
v = qv_mult(r3, v)
print v
# output: (4.930380657631324e-32, 2.220446049250313e-16, -1.0)
As always, please let me know if you find any problems here.
1. These are adapted from an OpenGL tutorial archived here.
2. The quaternion multiplication formula looks like a horrible rat's nest at first, but the derivation is easy, albeit tedious. Working with pencil and paper, you can represent the two quaternions like so: w + xi + yj + zk. Then note that ii = jj = kk = -1; that ij = k, jk = i, ki = j; and that ji = -k, kj = -i, ik = -j. Finally, multiply the two quaternions, distributing out the terms and rearranging them based on the results of each of the 16 multiplications. This helps to illustrate why you can use quaternions to represent rotation; the last six identities follow the right-hand rule, creating bijections between rotations from i to j and rotations around k, and so on.
If you do this, you'll see that the identity ii = jj = kk = -1 explains the last three terms in the w formula; that jk = i explains the third term in the x formula; and that kj = -i explains the fourth term in the x formula. The y and z formulas work the same way. The remaining terms are all examples of ordinary scalar multiplication.