from __future__ import print_function
+from sympy.vector.vector import *
from sympy import *
import itertools
from sympy.utilities.lambdify import lambdify, implemented_function
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)
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)
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():
~ian / moebius3.git / commitdiff