eic-opticks/ana/translate_rotate.py

0001 #!/usr/bin/env python
0002 """
0003 translate_rotate.py
0004 =====================
0005
0006 Insights from this sympy checking used in::
0007
0008    U4Transform::Convert_RotateThenTranslate
0009    U4Transform::Convert_TranslateThenRotate
0010
0011
0012 """
0013 import os, sympy as sp, numpy as np
0014 from sympy import pprint as pp
0015
0016 def pr(title):
0017     print("\n\n%s\n" % title)
0018
0019
0020 def translate_rotate():
0021     """
0022     P1
0023      x  y  z  1
0024     TR
0025      rxx  ryx  rzx  0
0026
0027      rxy  ryy  rzy  0
0028
0029      rxz  ryz  rzz  0
0030
0031      tx   ty   tz   tw
0032     P*TR
0033      rxx x + rxy y + rxz z + tx w  ryx x + ryy y + ryz z + ty w  rzx x + rzy y + rzz z + tz w  tw w
0034     P*TR.subs(v_rid)
0035      tx w + x  ty w + y  tz w + z  tw w
0036
0037     """
0038
0039     pr("R")
0040     pp(R)
0041
0042     pr("T")
0043     pp(T)
0044
0045     pr("T*R : row3 has translation and rotation mixed up : ie translation first and then rotation")
0046     pp(T*R)
0047
0048     pr("R*T : familiar row3 as translation : that means rotate then translate ")
0049     pp(R*T)
0050
0051     pr("RT")
0052     pp(RT)
0053
0054     assert RT == R*T
0055
0056     pr("P1")
0057     pp(P1)
0058
0059
0060     pr("P1*RT : notice that the translation just gets added to rotated coordinates : ie rotation first and then translation")
0061     pp(P1*RT)
0062
0063
0064     P_RT = P*RT
0065     P1_RT = P1*RT
0066
0067     pr("P1*RT.subs(v_rid) : setting rotation to identity ")
0068     pp(P1_RT.subs(v_rid))
0069
0070
0071
0072 if __name__ == '__main__':
0073
0074     row0 = rxx,ryx,rzx,rwx = sp.symbols("rxx,ryx,rzx,rwx")
0075     row1 = rxy,ryy,rzy,rwy = sp.symbols("rxy,ryy,rzy,rwy")
0076     row2 = rxz,ryz,rzz,rwz = sp.symbols("rxz,ryz,rzz,rwz")
0077     row3 = tx,ty,tz,tw     = sp.symbols("tx,ty,tz,tw")
0078
0079     RTxyz = sp.Matrix([row0,row1,row2,row3])
0080
0081     v_rid = [
0082        (rxx,1),(ryx,0),(rzx,0),
0083        (rxy,0),(ryy,1),(rzy,0),
0084        (rxz,0),(ryz,0),(rzz,1) ]    # identity rotation
0085
0086     v_rw = [(rwx,0),(rwy,0),(rwz,0)]
0087     v_t0 = [(tx,0),(ty,0),(tz,0),(tw,1)] # identity translation
0088     v_tw = [(tw,1),]
0089
0090
0091     RT = RTxyz.subs(v_rw+v_tw)
0092
0093     R = RTxyz.subs(v_rw+v_t0)
0094
0095     T = RTxyz.subs(v_rid+v_rw+v_tw)
0096
0097
0098     x,y,z,w = sp.symbols("x,y,z,w")
0099     P = sp.Matrix([[x,y,z,w]])
0100
0101     assert P.shape == (1,4)
0102     P1 = P.subs([(w,1)])    # position
0103     P0 = P.subs([(w,0)])    # direction vector
0104
0105     translate_rotate()
0106
0107