From: Ian Jackson Date: Sat, 7 Apr 2018 15:55:37 +0000 (+0100) Subject: curveopt: symbolic: wip X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ian/git?a=commitdiff_plain;h=0e10bcb9ecc6246d0a07154ad611ea2a3dbece88;p=moebius3.git curveopt: symbolic: wip Signed-off-by: Ian Jackson --- diff --git a/symbolic.py b/symbolic.py index bb1fb71..38ef5e6 100644 --- a/symbolic.py +++ b/symbolic.py @@ -1,6 +1,7 @@ from __future__ import print_function +from sympy.vector.vector import * from sympy import * import itertools from sympy.utilities.lambdify import lambdify, implemented_function @@ -8,11 +9,6 @@ import numpy as np from moedebug import * -r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa') - -# start original formulation -# rightvars replaces - def dprint(*args): if not dbg_enabled(): return print(*args) @@ -25,162 +21,36 @@ def dbg(*args): print('\n =\n') pprint(cse(eval(vn))) +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') + def calculate(): global calculated if calculated: return - p_start = Matrix([ - r * (1 - cos(theta)), - r * sin(theta), - mu * s, - ]) - - global p_rightvars - p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la ) - dbg('p_rightvars') - - global p_dirn_rightvars - p_dirn_rightvars = diff(p_rightvars, s) - dbg('p_dirn_rightvars') - - zeta = Wild('zeta') - - global p_nosing - 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') - - global t - t = symbols('t') - - global q_owncoords, q_dirn_owncoords - 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)') - - global p2q_translate, p2q_rotate - 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') - - global q_dirn_maincoords, q_maincoords - 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') - - global sinof_mu, cosof_mu - 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 ]]) - - global check_dirn_p_s0 - 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') - - global check_accel_p_s0 - check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0) - check_accel_p_s0.simplify() - dbg('check_accel_p_s0') - - global q_dirn_orgcoords, q_orgcoords - q_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * q_dirn_maincoords; - q_orgcoords = o2p_rotate2 * o2p_rotate1 * q_maincoords; - dbg('q_orgcoords','q_dirn_orgcoords') - - global sh, th - sh, th = symbols('alpha beta') - - global q_dirn_sqparm, q_sqparm - 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('----------------------------------------') - - global gamma - gamma = symbols('gamma') - - q_dirn_dirnscaled = q_dirn_sqparm * gamma - - global result_dirnscaled - result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled) - dbg('result_dirnscaled') - - dprint('----------------------------------------') - for v in params: - dbg('diff(result_dirnscaled,%s)' % v) - dprint('----------------------------------------') + Q = B + 0.5 * (B - A) + R = C + 0.5 * (C - D) + QR = R - Q + BC = C - B + cost_ABCD = (QR & QR) / (BC & BC) + global cost_ABCD + dbg('cost_ABCD') + dprint(A) calculated = True -params = ('sh','th','la','mu','gamma','kappa') - def ourccode(*a, **kw): return ccode(*a, user_functions={'sinc':'sinc'}, **kw) @@ -235,20 +105,13 @@ def gen_diff(current, smalls): def gen_misc(): cprintraw('// AUTOGENERATED - DO NOT EDIT\n') - cprintraw('#define N %d\n' % len(params)) - cprintraw('static const char __attribute__((unused)) *PARAM_NAMES[N]={'); - for p in params: cprintraw('"%s",' % p) - cprintraw('};\n'); def gen_x_extract(): - cprint('#define X_EXTRACT') - for ix in range(0, len(params)): - cprint('double %s = X(%d);' % (eval(params[ix]), ix)) - cprintraw() + pass def gen_f_populate(): cprint('#define F_POPULATE') - cassign(result_dirnscaled,'F','ftmp') + cassign(cost_ABCD,'F','ftmp') cprintraw('') def gen_j_populate():