raw snippet

A quaternion is represented as a 4D array. The implementations serve as a means to derive optimized formulae for special cases on quaternions.

from sympy import *

# Gets a quaternion out of a 3D axis and an angle in radians
def axisAngle(axis, angle):

    halfAngle = angle * 0.5

    a = axis[0]
    b = axis[1]
    c = axis[2]

    sin_2 = sin(halfAngle)
    cos_2 = cos(halfAngle)

    return [cos_2, a * sin_2, b * sin_2, c * sin_2]

# Multiplies two quaternions
def mul(a, b):

    w1 = a[0]
    x1 = a[1]
    y1 = a[2]
    z1 = a[3]

    w2 = b[0]
    x2 = b[1]
    y2 = b[2]
    z2 = b[3]

    return [
        w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
        w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
        w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2,
        w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2]

# Gets quaternion to rotate around the X axis
def RotX(a):
    return axisAngle([1, 0, 0], a)
    
# Gets quaternion to rotate around the Y axis
def RotY(a):
    return axisAngle([0, 1, 0], a)

# Gets quaternion to rotate around the Z axis
def RotZ(a):
    return axisAngle([0, 0, 1], a)

Examples

x,y,z = symbols("X,Y,Z")

print(mul(mul(RotZ(x), RotY(y)), RotX(z)))