curveopt.py

   1
   2 from __future__ import print_function
   3
   4 import numpy as np
   5 from numpy import cos, sin
   6
   7 import sys
   8 import subprocess
   9
  10 from moedebug import *
  11 from moenp import *
  12 from moebez import *
  13
  14 from math import atan2, atan, sqrt
  15
  16 import symbolic
  17
  18 findcurve_subprocs = { }
  19
  20 class OptimisedCurve():
  21   def __init__(oc, cp, nt):
  22     db = DiscreteBezier(cp, nt, constructor=BezierSegment)
  23     fc_input = map(db.point_at_it, range(0, nt+1))
  24     for end in (False,True):
  25       ei = end ? nt : 0
  26       fi = end ? nt-1 : 1
  27       cp0i = end ? 0 : 3
  28       cp1i = end ? 1 : 2
  29       e = cp[cp0i]
  30       ef_dirn =unit_v(cp[cp1i] - cp[cp0i])
  31       ef_len = np.linalg.norm(fc_input[fi] - fc_input[ei])
  32       f = e + ef_dirn * ef_len
  33       fc_input[ei] = e
  34       fc_input[fi] = f
  35
  36     findcurve_epsilon = dist_pq_plane * 0.01
  37
  38     try:
  39       subproc = findcurve_subprocs[nt]
  40     except KeyError:
  41       dbg('STARTING FINDCURVE %d' % nt)
  42       subproc = subprocess.Popen(
  43         ['./findcurve', '%d' % nt],
  44         bufsize=1,
  45         stdin=subprocess.PIPE,
  46         stdout=subprocess.PIPE,
  47         stderr=None,
  48         close_fds=False,
  49         # restore_signals=True, // want python2 compat, nnng
  50         universal_newlines=True,
  51       )
  52       findcurve_subprocs[nt] = subproc
  53
  54     dbg(('RUNNING FINDCURVE')
  55
  56     findcurve_input = np.hstack(*findcurve_input)
  57     s = ' '.join(map(str, findcurve_input))
  58
  59     dbg(('>> %s' % s))
  60
  61     print(s, file=findcurve_subproc.stdin)
  62     findcurve_subproc.stdin.flush()
  63
  64     commentary = ''
  65
  66     while True:
  67       l = findcurve_subproc.stdout.readline()
  68       l = l.rstrip()
  69       dbg('<< ', l)
  70       if not l: vdbg().crashing('findcurve EOF')
  71       if not l.startswith('['):
  72         commentary += ' '
  73         commentary += l
  74         continue
  75
  76       l = eval(l)
  77       if not l: break
  78
  79       dbg('[%s] %s' % (l, commentary))
  80       commentary = ''
  81
  82       findcurve_result = l
  83
  84     oc._result = np.reshape(findcurve_result, (3,-1))
  85
  86   def point_at_ti(oc, ti):
  87     return oc._result[ti]