symbolic.py

   1 #!/usr/bin/python3
   2
   3 from sympy import *
   4 import itertools
   5
   6 import sys
   7
   8 import sys, codecs
   9 if sys.stdout.encoding is None:
  10   sys.stdout = codecs.open("/dev/stdout", "w", 'utf-8')
  11
  12 init_printing(use_unicode=True, num_columns=280)
  13
  14 r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa')
  15
  16 # start      original formulation
  17 # rightvars  replaces
  18
  19 p_start = Matrix([
  20   r * (1 - cos(theta)),
  21   r * sin(theta),
  22   mu * s,
  23 ])
  24
  25 p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la )
  26
  27 def dbg(*args):
  28   for vn in args:
  29     print('\n    ' + vn + '\n')
  30     pprint(eval(vn))
  31     print('\n          =\n')
  32     pprint(cse(eval(vn)))
  33
  34 dbg('p_rightvars')
  35
  36 p_dirn_rightvars = diff(p_rightvars, s)
  37
  38 dbg('p_dirn_rightvars')
  39
  40 zeta = Wild('zeta')
  41
  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) )
  45             )
  46 p_nosing[1] = (p_nosing[1]
  47             .replace( sin(zeta) , zeta * sinc(zeta)                )
  48                )
  49
  50 dbg('p_nosing')
  51
  52 t = symbols('t')
  53
  54 q_owncoords = p_nosing.replace(s,t).replace(la,-la)
  55 q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
  56
  57 dbg('q_owncoords','q_dirn_owncoords')
  58 dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
  59
  60 p2q_translate = p_nosing
  61 #p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
  62
  63 #p2q_rotate = eye(3)
  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]
  66
  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])
  72
  73 dbg('p2q_rotate')
  74
  75 q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
  76 q_maincoords = p2q_rotate * q_owncoords + p2q_translate
  77
  78 dbg('diff(p_dirn_rightvars,s)')
  79 dbg('diff(q_dirn_maincoords,t)')
  80 dbg('diff(q_dirn_maincoords,t).replace(t,0)')
  81
  82 assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
  83
  84 #for v in 's','t','la','mu':
  85 #  dbg('diff(q_maincoords,%s)' % v)
  86
  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)))''')
  90
  91 #dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
  92 #          p_dirn_rightvars .cross(Matrix([0,0,1])))''')
  93
  94 #eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
  95 #print
  96 #pprint(eq)
  97 #solve(eq, Q)
  98
  99 dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
 100
 101 dbg('q_maincoords','q_dirn_maincoords')
 102
 103 sinof_mu = sin(atan(mu))
 104 cosof_mu = cos(atan(mu))
 105
 106 dbg('cosof_mu','sinof_mu')
 107
 108 o2p_rotate1 = Matrix([[ 1,  0,         0        ],
 109                       [ 0,  cosof_mu, +sinof_mu ],
 110                       [ 0, -sinof_mu,  cosof_mu ]])
 111
 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')
 115
 116 o2p_rotate2 = Matrix([[  cos(kappa), 0, -sin(kappa) ],
 117                       [  0,          1,  0          ],
 118                       [ +sin(kappa), 0,  cos(kappa) ]])
 119
 120 p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
 121
 122 check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
 123 check_dirn_p_s0.simplify()
 124 dbg('check_dirn_p_s0')
 125
 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')
 129
 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')
 133
 134 sh, th = symbols('alpha beta')
 135
 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)
 138
 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('----------------------------------------')
 146
 147 gamma = symbols('gamma')
 148
 149 q_dirn_dirnscaled = q_dirn_sqparm * gamma
 150
 151 result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled)
 152 dbg('result_dirnscaled')
 153
 154 params = ('sh','th','la','mu','gamma','kappa')
 155
 156 def cprint(s):
 157   print(s)
 158
 159 def cse_prep_cprint(v, tmp_prefix):
 160   # => v, but also having cprint'd the common subexpression assignments
 161   sym_iter = map((lambda i: symbols('%s%d' % (tmp_prefix,i))),
 162                  itertools.count())
 163   (defs, vs) = cse(v, symbols=sym_iter)
 164   for defname, defval in defs:
 165     cprint(ccode(defval, assign_to=defname))
 166   return vs[0]
 167
 168 def cassign(v, assign_to, tmp_prefix):
 169   v = cse_prep_cprint(v, tmp_prefix)
 170   cprint(ccode(v, assign_to=assign_to))
 171
 172 def gen_diff(current, smalls):
 173   global j
 174   if not smalls:
 175     j = zeros(len(params),0)
 176     for param in params:
 177       global d
 178       paramv = eval(param)
 179       d = diff(current, paramv)
 180       dbg('d')
 181       j = j.row_join(d)
 182     dbg('j')
 183     j = cse_prep_cprint(j, 'jtmp')
 184     for ix in range(0, j.cols):
 185       cprint(ccode(j.col(ix), 'J(%d)' % ix))
 186       cprint('J_END_COL(%d)' % ix)
 187   else:
 188     small = smalls[0]
 189     smalls = smalls[1:]
 190     cprint('if (!IS_SMALL(' + ccode(small) + ')) {')
 191     gen_diff(current, smalls)
 192     cprint('} else { /* %s small */' % small)
 193     gen_diff(current.replace(
 194       sinc(small),
 195       1 - small*small/factorial(3) - small**4/factorial(5),
 196       ),
 197       smalls
 198     )
 199     print('} /* %s small */' % small)
 200
 201 gen_diff(result_dirnscaled, (sh*sh*la, th*th*la))
 202
 203 #bad = q_orgcoords[0]
 204 #badd = diff(bad, la)
 205
 206 #dbg('bad','badd')