symbolic.py

   1
   2 from sympy import *
   3 import itertools
   4
   5 from moedebug import dbg_enable
   6
   7 from sympy.utilities.lambdify import lambdify, implemented_function
   8
   9 r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa')
  10
  11 # start      original formulation
  12 # rightvars  replaces
  13
  14 def dprint(*args):
  15   if not dbg_enable: return
  16   print(*args)
  17
  18 def dbg(*args):
  19   if not dbg_enable: return
  20   for vn in args:
  21     print('\n    ' + vn + '\n')
  22     pprint(eval(vn))
  23     print('\n          =\n')
  24     pprint(cse(eval(vn)))
  25
  26 calculated = False
  27
  28 def calculate():
  29   global calculated
  30   if calculated: return
  31
  32   p_start = Matrix([
  33     r * (1 - cos(theta)),
  34     r * sin(theta),
  35     mu * s,
  36   ])
  37
  38   p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la )
  39
  40   dbg('p_rightvars')
  41
  42   p_dirn_rightvars = diff(p_rightvars, s)
  43
  44   dbg('p_dirn_rightvars')
  45
  46   zeta = Wild('zeta')
  47
  48   p_nosing = (p_rightvars
  49               .replace( 1-cos(zeta)  ,   2*sin(zeta/2)**2          )
  50               .replace( sin(zeta)**2 ,   zeta*sinc(zeta)*sin(zeta) )
  51               )
  52   p_nosing[1] = (p_nosing[1]
  53               .replace( sin(zeta) , zeta * sinc(zeta)                )
  54                  )
  55
  56   dbg('p_nosing')
  57
  58   t = symbols('t')
  59
  60   q_owncoords = p_nosing.replace(s,t).replace(la,-la)
  61   q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
  62
  63   dbg('q_owncoords','q_dirn_owncoords')
  64   dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
  65
  66   p2q_translate = p_nosing
  67   #p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
  68
  69   #p2q_rotate = eye(3)
  70   #p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
  71   #p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
  72
  73   p2q_rotate = Matrix([[  cos(theta), sin(theta), 0 ],
  74                        [ -sin(theta), cos(theta), 0 ],
  75                        [  0         , 0,          1 ]]).subs(theta,la*s)
  76   #p2q_rotate.add_col([0,0])
  77   #p2q_rotate.add_row([0,0,1])
  78
  79   dbg('p2q_rotate')
  80
  81   q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
  82   q_maincoords = p2q_rotate * q_owncoords + p2q_translate
  83
  84   dbg('diff(p_dirn_rightvars,s)')
  85   dbg('diff(q_dirn_maincoords,t)')
  86   dbg('diff(q_dirn_maincoords,t).replace(t,0)')
  87
  88   assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
  89
  90   #for v in 's','t','la','mu':
  91   #  dbg('diff(q_maincoords,%s)' % v)
  92
  93   #print('\n eye3 subs etc.\n')
  94   #dbg('''Eq(eye(3) * Matrix([1,0,mu]),
  95   #     p_dirn_rightvars .cross(Matrix([0,0,1]) .subs(s,0)))''')
  96
  97   #dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
  98   #          p_dirn_rightvars .cross(Matrix([0,0,1])))''')
  99
 100   #eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
 101   #print
 102   #pprint(eq)
 103   #solve(eq, Q)
 104
 105   dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
 106
 107   dbg('q_maincoords','q_dirn_maincoords')
 108
 109   sinof_mu = sin(atan(mu))
 110   cosof_mu = cos(atan(mu))
 111
 112   dbg('cosof_mu','sinof_mu')
 113
 114   o2p_rotate1 = Matrix([[ 1,  0,         0        ],
 115                         [ 0,  cosof_mu, +sinof_mu ],
 116                         [ 0, -sinof_mu,  cosof_mu ]])
 117
 118   check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0)
 119   check_dirn_p_s0.simplify()
 120   dbg('check_dirn_p_s0')
 121
 122   o2p_rotate2 = Matrix([[  cos(kappa), 0, -sin(kappa) ],
 123                         [  0,          1,  0          ],
 124                         [ +sin(kappa), 0,  cos(kappa) ]])
 125
 126   p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
 127
 128   check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
 129   check_dirn_p_s0.simplify()
 130   dbg('check_dirn_p_s0')
 131
 132   check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0)
 133   check_accel_p_s0.simplify()
 134   dbg('check_accel_p_s0')
 135
 136   q_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * q_dirn_maincoords;
 137   q_orgcoords = o2p_rotate2 * o2p_rotate1 * q_maincoords;
 138   dbg('q_orgcoords','q_dirn_orgcoords')
 139
 140   global sh, th
 141   sh, th = symbols('alpha beta')
 142
 143   global q_sqparm
 144   q_dirn_sqparm = q_dirn_orgcoords.replace(s, sh**2).replace(t, th**2)
 145   q_sqparm      = q_orgcoords     .replace(s, sh**2).replace(t, th**2)
 146
 147   dprint('----------------------------------------')
 148   dbg('q_sqparm', 'q_dirn_sqparm')
 149   dprint('----------------------------------------')
 150   for v in 'sh','th','la','mu':
 151     dbg('diff(q_sqparm,%s)' % v)
 152     dbg('diff(q_dirn_sqparm,%s)' % v)
 153   dprint('----------------------------------------')
 154
 155   gamma = symbols('gamma')
 156
 157   q_dirn_dirnscaled = q_dirn_sqparm * gamma
 158
 159   global result_dirnscaled
 160   result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled)
 161   dbg('result_dirnscaled')
 162
 163   calculated = True
 164
 165 params = ('sh','th','la','mu','gamma','kappa')
 166
 167 def ourccode(*a, **kw):
 168   return ccode(*a, user_functions={'sinc':'sinc'}, **kw)
 169
 170 def cprintraw(*s):
 171   print(*s)
 172
 173 def cprint(s):
 174   for l in s.split('\n'):
 175     cprintraw(l, '\\')
 176
 177 def cse_prep_cprint(v, tmp_prefix):
 178   # => v, but also having cprint'd the common subexpression assignments
 179   sym_iter = map((lambda i: symbols('%s%d' % (tmp_prefix,i))),
 180                  itertools.count())
 181   (defs, vs) = cse(v, symbols=sym_iter)
 182   for defname, defval in defs:
 183     cprint('double '+ourccode(defval, assign_to=defname))
 184   return vs[0]
 185
 186 def cassign(v, assign_to, tmp_prefix):
 187   v = cse_prep_cprint(v, tmp_prefix)
 188   cprint(ourccode(v, assign_to=assign_to))
 189
 190 def gen_diff(current, smalls):
 191   global j
 192   if not smalls:
 193     j = zeros(len(params),0)
 194     for param in params:
 195       global d
 196       paramv = eval(param)
 197       d = diff(current, paramv)
 198       dbg('d')
 199       j = j.row_join(d)
 200     dbg('j')
 201     j = cse_prep_cprint(j, 'jtmp')
 202     for ix in range(0, j.cols):
 203       cprint(ourccode(j.col(ix), 'J_COL'))
 204       cprint('J_END_COL(%d)' % ix)
 205   else:
 206     small = smalls[0]
 207     smalls = smalls[1:]
 208     cprint('if (!IS_SMALL(' + ourccode(small) + ')) {')
 209     gen_diff(current, smalls)
 210     cprint('} else { /* %s small */' % small)
 211     gen_diff(current.replace(
 212       sinc(small),
 213       1 - small*small/factorial(3) - small**4/factorial(5),
 214       ),
 215       smalls
 216     )
 217     cprint('} /* %s small */' % small)
 218
 219 def gen_misc():
 220   cprintraw('// AUTOGENERATED - DO NOT EDIT\n')
 221   cprintraw('#define N %d\n' % len(params))
 222
 223 def gen_x_extract():
 224   cprint('#define X_EXTRACT')
 225   for ix in range(0, len(params)):
 226     cprint('double %s = X(%d);' % (eval(params[ix]), ix))
 227   cprintraw()
 228
 229 def gen_f_populate():
 230   cprint('#define F_POPULATE')
 231   cassign(result_dirnscaled,'F','ftmp')
 232   cprintraw('')
 233
 234 def gen_j_populate():
 235   cprint('#define J_POPULATE')
 236   gen_diff(result_dirnscaled, (sh*sh*la, th*th*la))
 237   cprintraw('')
 238
 239 def gen_C():
 240   gen_misc()
 241   gen_x_extract()
 242   gen_f_populate()
 243   gen_j_populate()
 244
 245 def get_python():
 246   # https://github.com/sympy/sympy/issues/13642
 247   # "lambdify sinc gives wrong answer!"
 248   out = q_sqparm
 249   sinc_fixed = Function('sinc_fixed')
 250   implemented_function(sinc_fixed, lambda x: np.sinc(x/np.pi))
 251   out = out.subs(sinc,sinc_fixed)
 252   p = list(map(eval,params))
 253   return lambdify(p, out)