| Hosted by CoCalc | Download
# Adapted from http://www.wstein.org/edu/2010/414/projects/hoon_kwon.pdf
N.<A_w,A_x,A_y,A_z,B_w,B_x,B_y,B_z> = QQ[]

# Yields variables i,j,k that comprise the Hamiltonian quaternion basis.
H.<i,j,k> = QuaternionAlgebra(Frac(N),-1,-1)

quatA = A_w + A_x*i + A_y*j + A_z*k
quatB = B_w + B_x*i + B_y*j + B_z*k

# This is a neat trick, but I can't seem to do much else with these QuaternionAlgebra[Element] types.
quatA * quatB
A_w*B_w - A_x*B_x - A_y*B_y - A_z*B_z + (A_x*B_w + A_w*B_x - A_z*B_y + A_y*B_z)*i + (A_y*B_w + A_z*B_x + A_w*B_y - A_x*B_z)*j + (A_z*B_w - A_y*B_x + A_x*B_y + A_w*B_z)*k 

# Constructs a unit quaternion using Euler parameters.
def QuatFromAxisAngle(vec, thetaRad):
    (axisX, axisY, axisZ) = (vec[0], vec[1], vec[2])
    halfTheta = thetaRad / 2
    w = cos(halfTheta)
    x = axisX * sin(halfTheta)
    y = axisY * sin(halfTheta)
    z = axisZ * sin(halfTheta)
    return vector([w, x, y, z])

def AxisAngleFromQuat(quat):
    theta = 2 * arccos(quat[0])
    axis = vector([quat[1], quat[2], quat[3]])
    axis /= sqrt(1 - (quat[0] * quat[0]))
    return vector([theta, axis[0], axis[1], axis[2]])

show( AxisAngleFromQuat(vector([0,0,1,0])) )
(π, 0, 1, 0)\displaystyle \left(\pi,\,0,\,1,\,0\right)


xAxis = vector([1, 0, 0])
yAxis = vector([0, 1, 0])
zAxis = vector([0, 0, 1])

qx90 = QuatFromAxisAngle(xAxis, pi / 2)
qy90 = QuatFromAxisAngle(yAxis, pi / 2)
qz90 = QuatFromAxisAngle(zAxis, pi / 2)
halfSqrt2 = sqrt(2) / 2
assert qx90 == vector([halfSqrt2, halfSqrt2, 0, 0])
assert qy90 == vector([halfSqrt2, 0, halfSqrt2, 0])
assert qz90 == vector([halfSqrt2, 0, 0, halfSqrt2])
show('$x(90^{\circ})$', column_matrix(qx90), '$y(90^{\circ})$', column_matrix(qy90), '$z(90^{\circ})$', column_matrix(qz90))
x(90∘)x(90^{\circ}) (12 212 200)\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ \frac{1}{2} \, \sqrt{2} \\ 0 \\ 0 \end{array}\right) y(90∘)y(90^{\circ}) (12 2012 20)\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ 0 \\ \frac{1}{2} \, \sqrt{2} \\ 0 \end{array}\right) z(90∘)z(90^{\circ}) (12 20012 2)\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ 0 \\ 0 \\ \frac{1}{2} \, \sqrt{2} \end{array}\right)

# The Hamilton product of two quaternions.
def quat_product(lhs, rhs):
    (r1, v1) = (lhs[0], lhs[1:4])
    (r2, v2) = (rhs[0], rhs[1:4])

    scalarProduct = r1 * r2
    dotProduct = v1.dot_product(v2)
    crossProduct = v1.cross_product(v2)

    resultScalar = scalarProduct - dotProduct
    resultVector = (r1 * v2) + (r2 * v1) + crossProduct

    return vector([resultScalar, resultVector[0], resultVector[1], resultVector[2]])


# Yields the rotation corresponding to the combination of the entire input list of rotation quaternions.
def compose_quats(quat_list):
    return reduce(quat_product, quat_list, vector([1, 0, 0, 0]))


qidentity = vector([1, 0, 0, 0])

assert quat_product(qidentity, qidentity) == qidentity

assert compose_quats([]) == qidentity
assert compose_quats([qidentity]) == qidentity
assert compose_quats([qidentity, qidentity]) == qidentity


(theta_x, theta_y, theta_z) = var('theta_x, theta_y, theta_z')

qx = QuatFromAxisAngle(xAxis, theta_x)
qy = QuatFromAxisAngle(yAxis, theta_y)
qz = QuatFromAxisAngle(zAxis, theta_z)

show(column_matrix(qx), column_matrix(qy), column_matrix(qz))
show(column_matrix(qx.subs(theta_x == pi / 2)), column_matrix(qy.subs(theta_y == pi / 2)), column_matrix(qz.subs(theta_z == pi / 2)))
(cos⁡(12 θx)sin⁡(12 θx)00)\displaystyle \left(\begin{array}{r} \cos\left(\frac{1}{2} \, \theta_{x}\right) \\ \sin\left(\frac{1}{2} \, \theta_{x}\right) \\ 0 \\ 0 \end{array}\right) (cos⁡(12 θy)0sin⁡(12 θy)0)\displaystyle \left(\begin{array}{r} \cos\left(\frac{1}{2} \, \theta_{y}\right) \\ 0 \\ \sin\left(\frac{1}{2} \, \theta_{y}\right) \\ 0 \end{array}\right) (cos⁡(12 θz)00sin⁡(12 θz))\displaystyle \left(\begin{array}{r} \cos\left(\frac{1}{2} \, \theta_{z}\right) \\ 0 \\ 0 \\ \sin\left(\frac{1}{2} \, \theta_{z}\right) \end{array}\right)
(12 212 200)\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ \frac{1}{2} \, \sqrt{2} \\ 0 \\ 0 \end{array}\right) (12 2012 20)\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ 0 \\ \frac{1}{2} \, \sqrt{2} \\ 0 \end{array}\right) (12 20012 2)\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ 0 \\ 0 \\ \frac{1}{2} \, \sqrt{2} \end{array}\right)

qxyz = compose_quats([qx, qy, qz])
show(column_matrix(qxyz))
(cos⁡(12 θx)cos⁡(12 θy)cos⁡(12 θz)−sin⁡(12 θx)sin⁡(12 θy)sin⁡(12 θz)cos⁡(12 θy)cos⁡(12 θz)sin⁡(12 θx)+cos⁡(12 θx)sin⁡(12 θy)sin⁡(12 θz)cos⁡(12 θx)cos⁡(12 θz)sin⁡(12 θy)−cos⁡(12 θy)sin⁡(12 θx)sin⁡(12 θz)cos⁡(12 θz)sin⁡(12 θx)sin⁡(12 θy)+cos⁡(12 θx)cos⁡(12 θy)sin⁡(12 θz))\displaystyle \left(\begin{array}{r} \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{y}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) - \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{y}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) + \cos\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) - \cos\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) + \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \end{array}\right)

qzxy = compose_quats([qz, qx, qy])
show(column_matrix(qzxy))
(cos⁡(12 θx)cos⁡(12 θy)cos⁡(12 θz)−sin⁡(12 θx)sin⁡(12 θy)sin⁡(12 θz)cos⁡(12 θy)cos⁡(12 θz)sin⁡(12 θx)−cos⁡(12 θx)sin⁡(12 θy)sin⁡(12 θz)cos⁡(12 θx)cos⁡(12 θz)sin⁡(12 θy)+cos⁡(12 θy)sin⁡(12 θx)sin⁡(12 θz)cos⁡(12 θz)sin⁡(12 θx)sin⁡(12 θy)+cos⁡(12 θx)cos⁡(12 θy)sin⁡(12 θz))\displaystyle \left(\begin{array}{r} \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{y}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) - \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{y}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) - \cos\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) + \cos\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) + \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \end{array}\right)

qzyx = compose_quats([qz, qy, qx])
show(column_matrix(qzyx))
(cos⁡(12 θx)cos⁡(12 θy)cos⁡(12 θz)+sin⁡(12 θx)sin⁡(12 θy)sin⁡(12 θz)cos⁡(12 θy)cos⁡(12 θz)sin⁡(12 θx)−cos⁡(12 θx)sin⁡(12 θy)sin⁡(12 θz)cos⁡(12 θx)cos⁡(12 θz)sin⁡(12 θy)+cos⁡(12 θy)sin⁡(12 θx)sin⁡(12 θz)−cos⁡(12 θz)sin⁡(12 θx)sin⁡(12 θy)+cos⁡(12 θx)cos⁡(12 θy)sin⁡(12 θz))\displaystyle \left(\begin{array}{r} \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{y}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) + \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{y}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) - \cos\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) + \cos\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \\ -\cos\left(\frac{1}{2} \, \theta_{z}\right) \sin\left(\frac{1}{2} \, \theta_{x}\right) \sin\left(\frac{1}{2} \, \theta_{y}\right) + \cos\left(\frac{1}{2} \, \theta_{x}\right) \cos\left(\frac{1}{2} \, \theta_{y}\right) \sin\left(\frac{1}{2} \, \theta_{z}\right) \end{array}\right)


# This is a whole bunch of me trying to figure out how to reorder the terms so the theta variables always appear in the order x,y,z
# See also: http://ask.sagemath.org/question/10641/can-sage-stop-ordering-terms/?answer=15600#post-id-15600
assert qxyz[0].operator() == sage.symbolic.operators.add_vararg
assert qxyz[0].operands()[0].operator() == sage.symbolic.operators.mul_vararg
assert qxyz[0].operands()[0].operands()[0].operator() == cos
assert qxyz[0].operands()[0].operands()[0].operands()[0].operator() == sage.symbolic.operators.mul_vararg
#qxyz[0].operands()[0].operands()[0].operands()[0].operands()[0] == theta_x
#qxyz[0].operands()[0].operands()[0].operands()[0].operands()[1] == 1/2
#qxyz[0].operands()[0].operands()[0].variables()[0] == theta_x