9 if sys.stdout.encoding is None:
10 sys.stdout = codecs.open("/dev/stdout", "w", 'utf-8')
12 init_printing(use_unicode=True, num_columns=280)
14 r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa')
16 # start original formulation
25 p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la )
29 print('\n ' + vn + '\n')
36 p_dirn_rightvars = diff(p_rightvars, s)
38 dbg('p_dirn_rightvars')
42 p_nosing = (p_rightvars
43 .replace( 1-cos(zeta) , 2*sin(zeta/2)**2 )
44 .replace( sin(zeta)**2 , zeta*sinc(zeta)*sin(zeta) )
46 p_nosing[1] = (p_nosing[1]
47 .replace( sin(zeta) , zeta * sinc(zeta) )
54 q_owncoords = p_nosing.replace(s,t).replace(la,-la)
55 q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
57 dbg('q_owncoords','q_dirn_owncoords')
58 dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
60 p2q_translate = p_nosing
61 #p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
64 #p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
65 #p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
67 p2q_rotate = Matrix([[ cos(theta), sin(theta), 0 ],
68 [ -sin(theta), cos(theta), 0 ],
69 [ 0 , 0, 1 ]]).subs(theta,la*s)
70 #p2q_rotate.add_col([0,0])
71 #p2q_rotate.add_row([0,0,1])
75 q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
76 q_maincoords = p2q_rotate * q_owncoords + p2q_translate
78 dbg('diff(p_dirn_rightvars,s)')
79 dbg('diff(q_dirn_maincoords,t)')
80 dbg('diff(q_dirn_maincoords,t).replace(t,0)')
82 assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
84 #for v in 's','t','la','mu':
85 # dbg('diff(q_maincoords,%s)' % v)
87 #print('\n eye3 subs etc.\n')
88 #dbg('''Eq(eye(3) * Matrix([1,0,mu]),
89 # p_dirn_rightvars .cross(Matrix([0,0,1]) .subs(s,0)))''')
91 #dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
92 # p_dirn_rightvars .cross(Matrix([0,0,1])))''')
94 #eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
99 dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
101 dbg('q_maincoords','q_dirn_maincoords')
103 sinof_mu = sin(atan(mu))
104 cosof_mu = cos(atan(mu))
106 dbg('cosof_mu','sinof_mu')
108 o2p_rotate1 = Matrix([[ 1, 0, 0 ],
109 [ 0, cosof_mu, +sinof_mu ],
110 [ 0, -sinof_mu, cosof_mu ]])
112 check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0)
113 check_dirn_p_s0.simplify()
114 dbg('check_dirn_p_s0')
116 o2p_rotate2 = Matrix([[ cos(kappa), 0, -sin(kappa) ],
118 [ +sin(kappa), 0, cos(kappa) ]])
120 p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
122 check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
123 check_dirn_p_s0.simplify()
124 dbg('check_dirn_p_s0')
126 check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0)
127 check_accel_p_s0.simplify()
128 dbg('check_accel_p_s0')
130 q_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * q_dirn_maincoords;
131 q_orgcoords = o2p_rotate2 * o2p_rotate1 * q_maincoords;
132 dbg('q_orgcoords','q_dirn_orgcoords')
134 sh, th = symbols('alpha beta')
136 q_dirn_sqparm = q_dirn_orgcoords.replace(s, sh**2).replace(t, th**2)
137 q_sqparm = q_orgcoords .replace(s, sh**2).replace(t, th**2)
139 print('----------------------------------------')
140 dbg('q_sqparm', 'q_dirn_sqparm')
141 print('----------------------------------------')
142 for v in 'sh','th','la','mu':
143 dbg('diff(q_sqparm,%s)' % v)
144 dbg('diff(q_dirn_sqparm,%s)' % v)
145 print('----------------------------------------')
147 gamma = symbols('gamma')
149 q_dirn_dirnscaled = q_dirn_sqparm * gamma
151 result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled)
152 dbg('result_dirnscaled')
154 params = ('sh','th','la','mu','gamma','kappa')
156 def gen_diff(current, smalls):
161 d = diff(current, paramv)
163 print(ccode(d, assign_to='FOO'))
164 (defs, v) = cse(d, symbols=map((lambda i: symbols('tmp%d' % i)),
167 print(ccode(val, assign_to=vn))
168 print(ccode(v[0], assign_to='BAR'))
172 print('if ',small,' nonsmall {')
173 gen_diff(current, smalls)
174 print('} else { /* %s small */' % small)
175 gen_diff(current.replace(
177 1 - small*small/factorial(3) - small**4/factorial(5),
181 print('} /* %s small */' % small)
183 gen_diff(result_dirnscaled, (sh*sh*la, th*th*la))
185 #bad = q_orgcoords[0]
186 #badd = diff(bad, la)