-dbg('p_rightvars')
-
-p_dirn_rightvars = diff(p_rightvars, s)
-
-dbg('p_dirn_rightvars')
-
-zeta = Wild('zeta')
-
-p_nosing = (p_rightvars
- .replace( 1-cos(zeta) , 2*sin(zeta/2)**2 )
- .replace( sin(zeta)**2 , zeta*sinc(zeta)*sin(zeta) )
- )
-p_nosing[1] = (p_nosing[1]
- .replace( sin(zeta) , zeta * sinc(zeta) )
- )
-
-dbg('p_nosing')
-
-t = symbols('t')
-
-q_owncoords = p_nosing.replace(s,t).replace(la,-la)
-q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
-
-dbg('q_owncoords','q_dirn_owncoords')
-dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
-
-p2q_translate = p_nosing
-#p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
-
-#p2q_rotate = eye(3)
-#p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
-#p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
-
-p2q_rotate = Matrix([[ cos(theta), sin(theta), 0 ],
- [ -sin(theta), cos(theta), 0 ],
- [ 0 , 0, 1 ]]).subs(theta,la*s)
-#p2q_rotate.add_col([0,0])
-#p2q_rotate.add_row([0,0,1])
-
-dbg('p2q_rotate')
-
-q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
-q_maincoords = p2q_rotate * q_owncoords + p2q_translate
-
-dbg('diff(p_dirn_rightvars,s)')
-dbg('diff(q_dirn_maincoords,t)')
-dbg('diff(q_dirn_maincoords,t).replace(t,0)')
-
-assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
-
-#for v in 's','t','la','mu':
-# dbg('diff(q_maincoords,%s)' % v)
-
-#print('\n eye3 subs etc.\n')
-#dbg('''Eq(eye(3) * Matrix([1,0,mu]),
-# p_dirn_rightvars .cross(Matrix([0,0,1]) .subs(s,0)))''')
-
-#dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
-# p_dirn_rightvars .cross(Matrix([0,0,1])))''')
-
-#eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
-#print
-#pprint(eq)
-#solve(eq, Q)
-
-dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
-
-dbg('q_maincoords','q_dirn_maincoords')
-
-sinof_mu = sin(atan(mu))
-cosof_mu = cos(atan(mu))
-
-dbg('cosof_mu','sinof_mu')
-
-o2p_rotate1 = Matrix([[ 1, 0, 0 ],
- [ 0, cosof_mu, +sinof_mu ],
- [ 0, -sinof_mu, cosof_mu ]])
-
-check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0)
-check_dirn_p_s0.simplify()
-dbg('check_dirn_p_s0')
-
-o2p_rotate2 = Matrix([[ cos(kappa), 0, -sin(kappa) ],
- [ 0, 1, 0 ],
- [ +sin(kappa), 0, cos(kappa) ]])
-
-p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
-
-check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
-check_dirn_p_s0.simplify()
-dbg('check_dirn_p_s0')
-
-check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0)
-check_accel_p_s0.simplify()
-dbg('check_accel_p_s0')
-
-q_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * q_dirn_maincoords;
-q_orgcoords = o2p_rotate2 * o2p_rotate1 * q_maincoords;
-dbg('q_orgcoords','q_dirn_orgcoords')
-
-sh, th = symbols('alpha beta')
-
-q_dirn_sqparm = q_dirn_orgcoords.replace(s, sh**2).replace(t, th**2)
-q_sqparm = q_orgcoords .replace(s, sh**2).replace(t, th**2)
-
-dprint('----------------------------------------')
-dbg('q_sqparm', 'q_dirn_sqparm')
-dprint('----------------------------------------')
-for v in 'sh','th','la','mu':
- dbg('diff(q_sqparm,%s)' % v)
- dbg('diff(q_dirn_sqparm,%s)' % v)
-dprint('----------------------------------------')
-
-gamma = symbols('gamma')
-
-q_dirn_dirnscaled = q_dirn_sqparm * gamma
-
-result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled)
-dbg('result_dirnscaled')
-
-params = ('sh','th','la','mu','gamma','kappa')
+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