2 from __future__ import print_function
6 from sympy.utilities.lambdify import lambdify, implemented_function
11 r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa')
13 # start original formulation
17 if not dbg_enabled(): return
21 if not dbg_enabled(): return
23 print('\n ' + vn + '\n')
41 p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la )
44 global p_dirn_rightvars
45 p_dirn_rightvars = diff(p_rightvars, s)
46 dbg('p_dirn_rightvars')
51 p_nosing = (p_rightvars
52 .replace( 1-cos(zeta) , 2*sin(zeta/2)**2 )
53 .replace( sin(zeta)**2 , zeta*sinc(zeta)*sin(zeta) )
55 p_nosing[1] = (p_nosing[1]
56 .replace( sin(zeta) , zeta * sinc(zeta) )
64 global q_owncoords, q_dirn_owncoords
65 q_owncoords = p_nosing.replace(s,t).replace(la,-la)
66 q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
68 dbg('q_owncoords','q_dirn_owncoords')
69 dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
71 global p2q_translate, p2q_rotate
72 p2q_translate = p_nosing
73 #p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
76 #p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
77 #p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
79 p2q_rotate = Matrix([[ cos(theta), sin(theta), 0 ],
80 [ -sin(theta), cos(theta), 0 ],
81 [ 0 , 0, 1 ]]).subs(theta,la*s)
82 #p2q_rotate.add_col([0,0])
83 #p2q_rotate.add_row([0,0,1])
87 global q_dirn_maincoords, q_maincoords
88 q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
89 q_maincoords = p2q_rotate * q_owncoords + p2q_translate
91 dbg('diff(p_dirn_rightvars,s)')
92 dbg('diff(q_dirn_maincoords,t)')
93 dbg('diff(q_dirn_maincoords,t).replace(t,0)')
95 assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
97 #for v in 's','t','la','mu':
98 # dbg('diff(q_maincoords,%s)' % v)
100 #print('\n eye3 subs etc.\n')
101 #dbg('''Eq(eye(3) * Matrix([1,0,mu]),
102 # p_dirn_rightvars .cross(Matrix([0,0,1]) .subs(s,0)))''')
104 #dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
105 # p_dirn_rightvars .cross(Matrix([0,0,1])))''')
107 #eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
112 dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
114 dbg('q_maincoords','q_dirn_maincoords')
116 global sinof_mu, cosof_mu
117 sinof_mu = sin(atan(mu))
118 cosof_mu = cos(atan(mu))
120 dbg('cosof_mu','sinof_mu')
122 o2p_rotate1 = Matrix([[ 1, 0, 0 ],
123 [ 0, cosof_mu, +sinof_mu ],
124 [ 0, -sinof_mu, cosof_mu ]])
126 global check_dirn_p_s0
127 check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0)
128 check_dirn_p_s0.simplify()
129 dbg('check_dirn_p_s0')
131 o2p_rotate2 = Matrix([[ cos(kappa), 0, -sin(kappa) ],
133 [ +sin(kappa), 0, cos(kappa) ]])
135 p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
137 check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
138 check_dirn_p_s0.simplify()
139 dbg('check_dirn_p_s0')
141 global check_accel_p_s0
142 check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0)
143 check_accel_p_s0.simplify()
144 dbg('check_accel_p_s0')
146 global q_dirn_orgcoords, q_orgcoords
147 q_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * q_dirn_maincoords;
148 q_orgcoords = o2p_rotate2 * o2p_rotate1 * q_maincoords;
149 dbg('q_orgcoords','q_dirn_orgcoords')
152 sh, th = symbols('alpha beta')
154 global q_dirn_sqparm, q_sqparm
155 q_dirn_sqparm = q_dirn_orgcoords.replace(s, sh**2).replace(t, th**2)
156 q_sqparm = q_orgcoords .replace(s, sh**2).replace(t, th**2)
158 dprint('----------------------------------------')
159 dbg('q_sqparm', 'q_dirn_sqparm')
160 dprint('----------------------------------------')
161 for v in 'sh','th','la','mu':
162 dbg('diff(q_sqparm,%s)' % v)
163 dbg('diff(q_dirn_sqparm,%s)' % v)
164 dprint('----------------------------------------')
167 gamma = symbols('gamma')
169 q_dirn_dirnscaled = q_dirn_sqparm * gamma
171 global result_dirnscaled
172 result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled)
173 dbg('result_dirnscaled')
177 params = ('sh','th','la','mu','gamma','kappa')
179 def ourccode(*a, **kw):
180 return ccode(*a, user_functions={'sinc':'sinc'}, **kw)
186 for l in s.split('\n'):
189 def cse_prep_cprint(v, tmp_prefix):
190 # => v, but also having cprint'd the common subexpression assignments
191 sym_iter = map((lambda i: symbols('%s%d' % (tmp_prefix,i))),
193 (defs, vs) = cse(v, symbols=sym_iter)
194 for defname, defval in defs:
195 cprint('double '+ourccode(defval, assign_to=defname))
198 def cassign(v, assign_to, tmp_prefix):
199 v = cse_prep_cprint(v, tmp_prefix)
200 cprint(ourccode(v, assign_to=assign_to))
202 def gen_diff(current, smalls):
205 j = zeros(len(params),0)
209 d = diff(current, paramv)
213 j = cse_prep_cprint(j, 'jtmp')
214 for ix in range(0, j.cols):
215 cprint(ourccode(j.col(ix), 'J_COL'))
216 cprint('J_END_COL(%d)' % ix)
220 cprint('if (!IS_SMALL(' + ourccode(small) + ')) {')
221 gen_diff(current, smalls)
222 cprint('} else { /* %s small */' % small)
223 gen_diff(current.replace(
225 1 - small*small/factorial(3) - small**4/factorial(5),
229 cprint('} /* %s small */' % small)
232 cprintraw('// AUTOGENERATED - DO NOT EDIT\n')
233 cprintraw('#define N %d\n' % len(params))
234 cprintraw('static const char *PARAM_NAMES[N]={');
235 for p in params: cprintraw('"%s",' % p)
239 cprint('#define X_EXTRACT')
240 for ix in range(0, len(params)):
241 cprint('double %s = X(%d);' % (eval(params[ix]), ix))
244 def gen_f_populate():
245 cprint('#define F_POPULATE')
246 cassign(result_dirnscaled,'F','ftmp')
249 def gen_j_populate():
250 cprint('#define J_POPULATE')
251 gen_diff(result_dirnscaled, (sh*sh*la, th*th*la))
261 # https://github.com/sympy/sympy/issues/13642
262 # "lambdify sinc gives wrong answer!"
264 sinc_fixed = Function('sinc_fixed')
265 implemented_function(sinc_fixed, lambda x: np.sinc(x/np.pi))
266 out = out.subs(sinc,sinc_fixed)
267 p = list(map(eval,params))
268 return lambdify(p, out)