curveopt.py

   1
   2 from __future__ import print_function
   3
   4 import numpy as np
   5 from numpy import cos, sin
   6
   7 import six
   8 import sys
   9 import subprocess
  10 import math
  11
  12 from moedebug import *
  13 from moenp import *
  14 from moebez import *
  15 from schequeue import ScheduledTask
  16
  17 from math import atan2, atan, sqrt
  18
  19 import symbolic
  20
  21 class OptimisedCurve():
  22   counter = 0
  23
  24   def _dbg(oc, s):
  25     dbg('%s %s' % (oc._desc, s))
  26
  27   def __init__(oc, cp, nt):
  28     oc._desc = 'OC#%04d' % OptimisedCurve.counter
  29     OptimisedCurve.counter += 1
  30
  31     oc._dbg('cp= ' + ' '.join(map(vec2dbg, cp)))
  32
  33     db = DiscreteBezier(cp, nt, bezier_constructor=BezierSegment)
  34
  35     fc_input = map(db.point_at_it, range(0, nt+1))
  36     oc._dbg(repr(fc_input))
  37
  38     for end in (False,True):
  39       ei = nt if end else 0
  40       fi = nt-1 if end else 1
  41       cp0i = 3 if end else 0
  42       cp1i = 2 if end else 1
  43       e = np.array(cp[cp0i])
  44       ef_dirn = unit_v(cp[cp1i] - cp[cp0i])
  45       ef_len = np.linalg.norm(np.array(fc_input[fi]) - np.array(fc_input[ei]))
  46       f = e + ef_dirn * ef_len
  47       oc._dbg(repr((end, e,f, ef_dirn, ef_len)))
  48       fc_input[ei] = e
  49       fc_input[fi] = f
  50
  51     oc._dbg(repr(fc_input))
  52
  53     findcurve_epsilon = 0.01
  54
  55     oc.sched = ScheduledTask(oc._await_subproc, oc._desc)
  56     oc.sched.pre_spawn()
  57
  58     cl = ['./findcurve', '%d' % (nt+1), '%.18g' % findcurve_epsilon]
  59     oc._dbg('STARTING FINDCURVE %s' % cl)
  60     subproc = subprocess.Popen(
  61       cl,
  62       bufsize=1,
  63       stdin=subprocess.PIPE,
  64       stdout=subprocess.PIPE,
  65       stderr=None,
  66       close_fds=False,
  67       # restore_signals=True, // want python2 compat, nnng
  68       universal_newlines=True,
  69     )
  70
  71     oc._dbg('RUNNING FINDCURVE')
  72
  73     fc_input = np.hstack(fc_input)
  74     s = ' '.join(map(str, fc_input))
  75
  76     oc._dbg(('>> %s' % s))
  77
  78     print(s, file=subproc.stdin)
  79     subproc.stdin.flush()
  80
  81     oc.subproc = subproc
  82     oc.nt = nt
  83
  84     oc.sched.post_spawn()
  85     #oc._await_subproc()
  86
  87   def _await_subproc(oc):
  88     subproc = oc.subproc
  89     if subproc is None: return
  90
  91     oc._dbg('(awaiting)')
  92     commentary = ''
  93
  94     while True:
  95       l = subproc.stdout.readline()
  96       if not l:
  97         oc._dbg('findcurve EOF')
  98         vdbg().crashing('findcurve EOF')
  99       l = l.rstrip()
 100       oc._dbg('<< ' + l)
 101       if not l.startswith('['):
 102         commentary += ' '
 103         commentary += l
 104         continue
 105
 106       l = eval(l)
 107       if not l: break
 108
 109       oc._dbg('[%s] %s' % (l, commentary))
 110       commentary = ''
 111
 112       findcurve_result = l
 113
 114     subproc.stdin.close()
 115     subproc.wait()
 116     assert(subproc.returncode == 0)
 117     oc.subproc = None
 118
 119     oc.sched.post_reap()
 120
 121     oc._result = np.reshape(findcurve_result, (-1,3), 'C')
 122     oc._dbg(repr(oc._result))
 123
 124     #vdbg().curve( oc.point_at_t )
 125
 126   def point_at_it(oc, it):
 127     oc._await_subproc()
 128     oc._dbg(repr((it,)))
 129     return oc._result[it]
 130
 131   def point_at_t(oc, t):
 132     itd = t * oc.nt
 133     it0 = int(math.floor(itd))
 134     it1 = int(math.ceil(itd))
 135     p0 = oc.point_at_it(it0)
 136     p1 = oc.point_at_it(it1)
 137     return p0 + (p1-p0) * (itd-it0)
 138