helixish.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 from moebdebug import dbg
   9
  10 import symbolic
  11
  12 def augment(v): return np.append(v, 1)
  13 def augment0(v): return np.append(v, 0)
  14 def unaugment(v): return v[0:3]
  15
  16 findcurve_subproc = None
  17
  18 class HelixishCurve():
  19   def __init__(hc, cp):
  20     symbolic.calculate()
  21
  22     p = cp[0]
  23     q = cp[3]
  24     dp = unit_v(cp[1]-cp[0])
  25     dq = unit_v(cp[3]-cp[2])
  26
  27     dbg('HelixishCurve __init__', cp)
  28
  29     # the initial attempt
  30     #   - solve in the plane containing dP and dQ
  31     #   - total distance normal to that plane gives mu
  32     #   - now resulting curve is not parallel to dP at P
  33     #     nor dQ at Q, so tilt it
  34     #   - [[ pick as the hinge point the half of the curve
  35     #     with the larger s or t ]] not yet implemented
  36     #   - increase the other distance {t,s} by a bodge factor
  37     #     approx distance between {Q,P} and {Q,P}' due to hinging
  38     #     but minimum is 10% of (wlog) {s,t} [[ not quite like this ]]
  39
  40     dPQplane_normal = np.cross(dp, dq)
  41     if (np.norm(dPQplane_normal) < 1E6):
  42       dPQplane_normal += [0, 0, 1E5]
  43     dPQplane_normal = unit_v(dPQplane_normal)
  44
  45     dPQplane_basis = np.column_stack(np.cross(dp, dPQplane_normal),
  46                                      dp,
  47                                      dPQplane_normal,
  48                                      p);
  49     dPQplane_basis = np.vstack(dPQplane_basis, [0,0,0,1])
  50     dPQplane_into = np.linalg.inv(dPQplane_basis)
  51
  52     dp_plane = unaugment(dPQplane_into * augment0(dp))
  53     dq_plane = unaugment(dPQplane_into * augment0(dq))
  54     q_plane  = unaugment(dPQplane_into * augment(q))
  55     dist_pq_plane = np.linalg.norm(q_plane)
  56
  57     # two circular arcs of equal maximum possible radius
  58     # algorithm courtesy of Simon Tatham (`Railway problem',
  59     # pers.comm. to ijackson@chiark 23.1.2004)
  60     railway_angleoffset = atan2(*q_plane[0:1])
  61     railway_theta =                      tau/4 - railway_angleoffset
  62     railway_phi   = atan2(*dq_plane[0:1]) - railway_angleoffset
  63     railway_cos_theta = cos(railway_theta)
  64     railway_cos_phi   = cos(railway_phi)
  65     if railway_cos_theta**2 + railway_cos_phi**2 > 1E6:
  66       railway_roots = np.roots([
  67         2 * (1 + cos(railway_theta - railway_phi)),
  68         2 * (railway_cos_theta - railway_cos_phi),
  69         -1
  70         ])
  71       for railway_r in railway_roots:
  72         def railway_CPQ(pq, dpq):
  73           nonlocal railway_r
  74           return pq + railway_r * [-dpq[1], dpq[0]]
  75
  76         railway_CP = railway_CPQ([0,0,0],       dp_plane)
  77         railway_QP = railway_CPQ(q_plane[0:2], -dq_plane)
  78         railway_midpt = 0.5 * (railway_CP + railway_QP)
  79
  80         best_st = None
  81         def railway_ST(C, start, end):
  82           nonlocal railway_r
  83           delta = atan2(*(end - C)[0:2]) - atan2(start - C)[0:2]
  84           s = delta * railway_r
  85
  86         try_s = railway_ST(railway_CP, [0,0], midpt)
  87         try_t = railway_ST(railway_CP, midpt, q_plane)
  88         try_st = try_s + try_t
  89         if best_st is None or try_st < best_st:
  90           start_la = 1/r
  91           start_s = try_s
  92           start_t = try_t
  93           best_st = try_st
  94           start_mu = q_plane[2] / (start_s + start_t)
  95
  96     else: # twoarcs algorithm is not well defined
  97       start_la = 0.1
  98       start_s = dist_pq_plane * .65
  99       start_t = dist_pq_plane * .35
 100       start_mu = 0.05
 101
 102     bodge = max( q_plane[2] * mu,
 103                  (start_s + start_t) * 0.1 )
 104     start_s += 0.5 * bodge
 105     start_t += 0.5 * bodge
 106     start_kappa = 0
 107     start_gamma = 1
 108
 109     tilt = atan(mu)
 110     tilt_basis = np.array([
 111       1,     0,           0,         0,
 112       0,   cos(tilt), -sin(tilt),    0,
 113       0,   sin(tilt),  cos(tilt),    0,
 114       0,     0,           0,         1,
 115     ])
 116     findcurve_basis = dPQplane_basis * tilt_basis
 117     findcurve_into = np.linalg.inv(findcurve_basis)
 118
 119     q_findcurve = unaugment(findcurve_into, augment(q))
 120     dq_findcurve = unaugment(findcurve_into, augment0(dq))
 121
 122     findcurve_target = np.concatenate(q_findcurve, dq_findcurve)
 123     findcurve_start = (sqrt(start_s), sqrt(start_t), start_la,
 124                        start_mu, start_gamma, start_kappa)
 125
 126     findcurve_epsilon = dist_pq_plane * 0.01
 127
 128     if findcurve_subproc is None:
 129       findcurve_subproc = subprocess.Popen(
 130         ['./findcurve'],
 131         bufsize=1,
 132         stdin=subprocess.PIPE,
 133         stdout=subprocess.PIPE,
 134         stderr=None,
 135         close_fds=False,
 136         restore_signals=True,
 137         universal_newlines=True,
 138       )
 139
 140     findcurve_input = np.hstack((findcurve_target,
 141                                  findcurve_start,
 142                                  [findcurve_epsilon])))
 143     dbg('RUNNING FINDCURVE', *findcurve_input)
 144     print(findcurve_subproc.stdin, *findcurve_input)
 145     findcurve_subproc.stdin.flush()
 146
 147     while True:
 148       l = findcurve_subproc.stdout.readline()
 149       l = l.rstrip()
 150       dbg('GOT ', l)
 151       l = eval(l)
 152       if l is None: break
 153       findcurve_result = l[0:5]
 154
 155     symbolic.get_python(something)