import sympy as sp
dphi, dpsi, dtheta = sp.var("dphi dpsi dtheta")
phi, psi, theta = sp.var("phi psi theta")
x, y, z = sp.var("x y z")
qx, qy, qz = sp.var("qx qy qz")

def Rx(a):
    return sp.Matrix([[1, 0, 0], [0, sp.cos(a), sp.sin(a)], [0, -sp.sin(a), sp.cos(a)]])
def Ry(a):
    return sp.Matrix([[sp.cos(a), 0, sp.sin(a)], [0, 1, 0], [-sp.sin(a), 0, sp.cos(a)]])
def Rz(a):
    return sp.Matrix([[sp.cos(a), sp.sin(a), 0], [-sp.sin(a), sp.cos(a), 0], [0, 0, 1]])

print("==== asymmetric ====")
q = sp.Matrix([[qx],[qy],[0]])
p = sp.Matrix([[x],[y],[z]])
jitter = Rx(dphi)*Ry(dtheta)*Rz(dpsi)
view = Rz(phi)*Ry(theta)*Rz(psi)
jitter_inv = Rz(-dpsi)*Ry(-dtheta)*Rx(-dphi)
view_inv = Rz(-psi)*Ry(-theta)*Rz(-phi)
print(">> jitter rotation")
for row in jitter*p: print(row)
print("\n>> jitter plus view")
for row in view*jitter*p: print(row)
print("\n>> view reverse")
for row in view_inv*q: print(row)
print("\n>> jitter reverse")
for row in jitter_inv*view_inv*q: print(row)

print("\n\n==== symmetric ====")
q = sp.Matrix([[qx],[0],[0]])
p = sp.Matrix([[x],[y],[z]])
jitter = Rx(dphi)*Ry(dtheta)
view = Rz(phi)*Ry(theta)
jitter_inv = Ry(-dtheta)*Rx(-dphi)
view_inv = Ry(-theta)*Rz(-phi)
print(">> jitter rotation")
for row in jitter*p: print(row)
print("\n>> jitter plus view")
for row in view*jitter*p: print(row)
print("\n>> view reverse")
for row in view_inv*q: print(row)
print("\n>> jitter reverse")
for row in jitter_inv*view_inv*q: print(row)