+import itertools
+from sympy.utilities.lambdify import lambdify, implemented_function
+import numpy as np
+
+from moedebug import *
+
+# When cse() is called for something containing BaseVector, it
+# produces infinite recursion.
+#def cse(x, *a, **kw): return ((), (x,))
+
+def dprint(*args):
+ if not dbg_enabled(): return
+ print(*args)
+
+def dbg(*args):
+ if not dbg_enabled(): return
+ for vn in args:
+ print('\n ' + vn + '\n')
+ pprint(eval(vn))
+ print('\n =\n')
+ pprint(cse(eval(vn)))
+
+def sqnorm(v): return v & v
+
+N = CoordSysCartesian('N')
+
+calculated = False
+
+def vector_symbols(vnames):
+ out = []
+ for vname in vnames.split(' '):
+ v = Vector.zero
+ for cname in 'i j k'.split(' '):
+ v += getattr(N, cname) * symbols(vname + '_' + cname)
+ out.append(v)
+ return out
+
+A, B, C, D = vector_symbols('A B C D')
+p = vector_symbols('p')
+
+E, H = vector_symbols('E H')
+F0, G0 = vector_symbols('F0 G0')
+En, Hn = vector_symbols('En Hn')
+
+EFlq, HGlq = symbols('EFlq HGlq')
+
+def vector_component(v, ix):
+ return v.components[N.base_vectors()[ix]]
+
+# x array in numerical algorithm has:
+# N x 3 coordinates of points 0..N-3
+# 1 EFlq = sqrt of length parameter |EF| for point 1
+# 1 HGlq = sqrt of length parameter |HG| for point N-2
+
+# fixed array in numerical algorithm has:
+# 4 x 3 E, En, H, Hn
+
+#def subst_vect():
+
+iterations = []
+
+class SomeIteration():
+ def __init__(ar, names, size, expr):
+ ar.names_string = names
+ ar.names = names.split(' ')
+ ar.name = ar.names[0]
+ ar.size = size
+ ar.expr = expr
+ if dbg_enabled():
+ print('\n ' + ar.name + '\n')
+ print(expr)
+ iterations.append(ar)
+
+ def gen_calculate_cost(ar):
+ ar._gen_array()
+ cprint('for (P=0; P<(%s); P++) {' % ar.size)
+ ar._cassign()
+ cprint('}')
+
+class ScalarArray(SomeIteration):
+ def _gen_array(ar):
+ cprint('double A_%s[%s];' % (ar.name, ar.size))
+ def gen_references(ar):
+ for ai in range(0, len(ar.names)):
+ ar._gen_reference(ai, ar.names[ai])
+ def _gen_reference(ar, ai, an):
+ cprintraw('#define %s A_%s[P%+d]' % (an, ar.name, ai))
+ def _cassign(ar):
+ cassign(ar.expr, ar.name, 'tmp_'+ar.name)
+ def s(ar):
+ return symbols(ar.names_string)
+
+class CoordArray(ScalarArray):
+ def _gen_array(ar):
+ cprint('double A_%s[%s][3];' % (ar.name, ar.size))
+ def _gen_reference(ar, ai, an):
+ ScalarArray._gen_reference(ar, ai, an)
+ gen_point_coords_macro(an)
+ def _cassign(ar):
+ cassign_vector(ar.expr, ar.name, 'tmp_'+ar.name)
+ def s(ar):
+ return vector_symbols(ar.names_string)
+
+class CostComponent(SomeIteration):
+ def __init__(cc, size, expr):
+ cc.size = size
+ cc.expr = expr
+ iterations.append(cc)
+ def gen_references(cc): pass
+ def _gen_array(cc): pass
+ def _cassign(cc):
+ cassign(cc.expr, 'P_cost', 'tmp_cost')
+ cprint('cost += P_cost;')
+
+def calculate():
+ global calculated
+ if calculated: return
+
+ # ---------- actual cost computation formulae ----------
+
+ global F, G
+ F = E + En * pow(EFlq, 2)
+ G = H + Hn * pow(HGlq, 2)
+
+ global a,b, al,bl, au,bu
+ a, b = CoordArray ('a_ b_', 'NP-1', B-A ).s() # [mm]
+ al, bl = ScalarArray('al bl', 'NP-1', a.magnitude() ).s() # [mm]
+ au, bu = CoordArray ('au bu', 'NP-1', a / al ).s() # [1]
+
+ tan_theta = (au ^ bu) / (au & bu) # [1] bending
+
+ global mu, nu
+ mu, nu = CoordArray ('mu nu', 'NP-2', tan_theta ).s() # [1]
+
+ CostComponent('NP-3', sqnorm(mu - nu)) # [1]
+
+ dl2 = pow(al - bl, 2) # [mm^2]
+ CostComponent('NP-2', dl2 / (al*bl)) # [1]
+
+ # ---------- end of cost computation formulae ----------
+
+ calculated = True
+
+def ourccode(*a, **kw):
+ return ccode(*a, user_functions={'sinc':'sinc'}, **kw)
+
+def cprintraw(*s):
+ print(*s)
+
+def cprint(s):
+ for l in s.split('\n'):
+ cprintraw(l, '\\')
+
+def cse_prep_cprint(v, tmp_prefix):
+ # => v, but also having cprint'd the common subexpression assignments
+ sym_iter = map((lambda i: symbols('%s%d' % (tmp_prefix,i))),
+ itertools.count())
+ (defs, vs) = cse(v, symbols=sym_iter)
+ for defname, defval in defs:
+ cprint('double '+ourccode(defval, assign_to=defname))
+ return vs[0]
+
+def cassign(v, assign_to, tmp_prefix):
+ v = cse_prep_cprint(v, tmp_prefix)
+ cprint(ourccode(v, assign_to=assign_to))
+
+def cassign_vector(v, assign_to, tmp_prefix):
+ ijk = 'i j k'.split(' ')
+ for ii in range(0, len(ijk)):
+ x = v & getattr(N, ijk[ii])
+ cassign(x, '%s[%d]' % (assign_to, ii), '%s_%s' % (tmp_prefix, ijk[ii]))
+
+def gen_diff(current, smalls):
+ global j
+ if not smalls:
+ j = zeros(len(params),0)
+ for param in params:
+ global d
+ paramv = eval(param)
+ d = diff(current, paramv)
+ dbg('d')
+ j = j.row_join(d)
+ dbg('j')
+ j = cse_prep_cprint(j, 'jtmp')
+ for ix in range(0, j.cols):
+ cprint(ourccode(j.col(ix), 'J_COL'))
+ cprint('J_END_COL(%d)' % ix)
+ else:
+ small = smalls[0]
+ smalls = smalls[1:]
+ cprint('if (!IS_SMALL(' + ourccode(small) + ')) {')
+ gen_diff(current, smalls)
+ cprint('} else { /* %s small */' % small)
+ gen_diff(current.replace(
+ sinc(small),
+ 1 - small*small/factorial(3) - small**4/factorial(5),
+ ),
+ smalls
+ )
+ cprint('} /* %s small */' % small)
+
+def gen_misc():
+ cprintraw('// AUTOGENERATED - DO NOT EDIT\n')
+
+def gen_point_coords_macro(macro_basename):
+ ijk = 'i j k'.split(' ')
+ for ii in range(0, len(ijk)):
+ cprintraw('#define %s_%s (%s[%d])'
+ % (macro_basename, ijk[ii], macro_basename, ii))