SharedQuaternions.sagewsOpen in CoCalc
# 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.
(axisX, axisY, axisZ) = (vec[0], vec[1], vec[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])) )


$\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^{\circ})$ $\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ \frac{1}{2} \, \sqrt{2} \\ 0 \\ 0 \end{array}\right)$ $y(90^{\circ})$ $\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ 0 \\ \frac{1}{2} \, \sqrt{2} \\ 0 \end{array}\right)$ $z(90^{\circ})$ $\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)))


$\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)$ $\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)$ $\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)$
$\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ \frac{1}{2} \, \sqrt{2} \\ 0 \\ 0 \end{array}\right)$ $\displaystyle \left(\begin{array}{r} \frac{1}{2} \, \sqrt{2} \\ 0 \\ \frac{1}{2} \, \sqrt{2} \\ 0 \end{array}\right)$ $\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))


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


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


$\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
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