chiark / gitweb /
helixish: convert r from polynomial back to our unscaled plane coordinates
[moebius3.git] / 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 import subprocess
9
10 from moedebug import *
11 from moenp import *
12
13 from math import atan2, atan, sqrt
14
15 import symbolic
16
17 findcurve_subproc = None
18
19 class HelixishCurve():
20   def __init__(hc, cp):
21     symbolic.calculate()
22
23     p = cp[0]
24     q = cp[3]
25     dp = unit_v(cp[1]-cp[0])
26     dq = unit_v(cp[3]-cp[2])
27
28     dbg('HelixishCurve __init__', cp)
29     dbg(dp, dq)
30
31     #vdbg().arrow(p,dp)
32     #vdbg().arrow(q,dq)
33
34     # the initial attempt
35     #   - solve in the plane containing dP and dQ
36     #   - total distance normal to that plane gives mu
37     #   - now resulting curve is not parallel to dP at P
38     #     nor dQ at Q, so tilt it
39     #   - [[ pick as the hinge point the half of the curve
40     #     with the larger s or t ]] not yet implemented
41     #   - increase the other distance {t,s} by a bodge factor
42     #     approx distance between {Q,P} and {Q,P}' due to hinging
43     #     but minimum is 10% of (wlog) {s,t} [[ not quite like this ]]
44
45     dPQplane_normal = np.cross(dp, dq)
46
47     if np.linalg.norm(dPQplane_normal) < 1E-6:
48       dbg('dPQplane_normal small')
49       dPQplane_normal = np.cross([1,0,0], dp)
50     if np.linalg.norm(dPQplane_normal) < 1E-6:
51       dbg('dPQplane_normal small again')
52       dPQplane_normal = np.cross([0,1,0], dp)
53
54     dPQplane_normal = unit_v(dPQplane_normal)
55
56     vdbg().arrow([0,0,0], dPQplane_normal, color=(1,1,0))
57
58     dPQplane_basis = np.column_stack((np.cross(dp, dPQplane_normal),
59                                       dp,
60                                       dPQplane_normal,
61                                       p));
62     #dbg(dPQplane_basis)
63     dPQplane_basis = np.vstack((dPQplane_basis, [0,0,0,1]))
64     dbg(dPQplane_basis)
65     
66     vdbg().basis(dPQplane_basis)
67
68     dPQplane_into = np.linalg.inv(dPQplane_basis)
69     dbg(dPQplane_into)
70
71     p_plane_check = augmatmultiply(dPQplane_into, p)
72     dp_plane = augmatmultiply(dPQplane_into, dp, augwith=0)
73     dq_plane = augmatmultiply(dPQplane_into, dq, augwith=0)
74     q_plane  = augmatmultiply(dPQplane_into, q)
75     dist_pq_plane = np.linalg.norm(q_plane)
76
77     vdbg_plane = MatrixVisdebug(vdbg(), dPQplane_basis)
78
79     dbg('plane p', p_plane_check, 'dp', dp_plane, 'dq', dq_plane,
80         'q', q_plane, 'dist_pq_plane', dist_pq_plane)
81     vdbg_plane.arrow(p_plane_check, dp_plane)
82     vdbg_plane.arrow(q_plane,       dq_plane)
83
84     railway_inplane_basis_x = np.hstack((q_plane[0:2], [0]))
85     railway_inplane_basis = np.column_stack((
86       railway_inplane_basis_x,
87       -np.cross([0,0,1], railway_inplane_basis_x),
88       [0,0,1],
89       [0,0,0],
90     ))
91     #dbg('railway_inplane_basis\n', railway_inplane_basis)
92     railway_inplane_basis = np.vstack((railway_inplane_basis,
93                                        [0,0,0,1]))
94     dbg('railway_inplane_basis\n', railway_inplane_basis)
95     railway_basis = matmatmultiply(dPQplane_basis, railway_inplane_basis)
96     dbg('railway_basis\n', railway_basis)
97     vdbg().basis(railway_basis, hue=(1,0,1))
98     vdbg_railway = MatrixVisdebug(vdbg(), railway_basis)
99
100     # two circular arcs of equal maximum possible radius
101     # algorithm courtesy of Simon Tatham (`Railway problem',
102     # pers.comm. to ijackson@chiark 23.1.2004)
103     railway_angleoffset = atan2(*q_plane[0:2])
104     # these two angles are unconventional: clockwise from north
105     railway_theta = tau/4 - (atan2(*dp_plane[0:2]) - railway_angleoffset)
106     railway_phi   = tau/4 - (atan2(*-dq_plane[0:2]) - railway_angleoffset)
107     railway_cos_theta = cos(railway_theta)
108     railway_cos_phi   = cos(railway_phi)
109
110     dbg('railway:', railway_theta, railway_phi, railway_angleoffset)
111
112     def vdbg_railway_angle(start, angle, **kw):
113       vdbg_railway.arrow(start, [sin(angle), cos(angle), 0], **kw)
114     vdbg_railway_angle([0, 0, 0.1], railway_theta, color=(1, 0.5, 0))
115     vdbg_railway_angle([1, 0, 0.1], railway_phi,   color=(1, 0.5, 0))
116     vdbg_railway_angle([1, 0, 0.1], 0,             color=(1, 1.00, 0))
117     vdbg_railway_angle([1, 0, 0.1], tau/4,         color=(1, 0.75, 0))
118
119     if railway_cos_theta**2 + railway_cos_phi**2 > 1E-6:
120       railway_polynomial = [
121         2 * (1 + cos(railway_theta - railway_phi)),
122         2 * (railway_cos_theta - railway_cos_phi),
123         -1,
124         ]
125       railway_roots = np.roots(railway_polynomial)
126       dbg('railway poly, roots:', railway_polynomial, railway_roots)
127       for railway_r_pq1 in railway_roots:
128         # roots for r are calculated based on coordinates where
129         # Q is at (1,0) but our PQ distance is different
130         railway_r = railway_r_pq1 * dist_pq_plane
131         dbg(' twoarcs root r_pq1=', railway_r_pq1, 'r=',railway_r,
132             railway_polynomial[0] * railway_r_pq1 * railway_r_pq1 +
133             railway_polynomial[1] * railway_r_pq1                 +
134             railway_polynomial[2]
135         )
136
137         def railway_CPQ(pq, dpq, railway_r):
138           CPQ = pq + railway_r * np.array([-dpq[1], dpq[0]])
139           dbg('railway_CPQ', railway_r, pq, dpq, CPQ)
140           vdbg_plane.circle( np.hstack((CPQ, [0])),
141                              [0, 0, railway_r],
142                              color = (1,1,1) )
143           return CPQ
144
145         railway_CP = railway_CPQ([0,0],         dp_plane, railway_r)
146         railway_QP = railway_CPQ(q_plane[0:2], -dq_plane, railway_r)
147         railway_midpt = 0.5 * (railway_CP + railway_QP)
148
149         best_st = None
150         def railway_ST(C, start, end, railway_r):
151           delta = atan2(*(end - C)[0:2]) - atan2(*(start - C)[0:2])
152           s = delta * railway_r
153           dbg('railway_ST', C, start, end, railway_r, s)
154           return s
155
156         try_s = railway_ST(railway_CP, [0,0], railway_midpt, railway_r)
157         try_t = railway_ST(railway_CP, railway_midpt, q_plane[0:2], railway_r)
158         dbg('try_s, _t', try_s, try_t)
159         if try_s < 0 or try_t < 0:
160           continue
161
162         try_st = try_s + try_t
163         if best_st is None or try_st < best_st:
164           start_la = 1/railway_r
165           start_s = try_s
166           start_t = try_t
167           best_st = try_st
168           start_mu = q_plane[2] / (start_s + start_t)
169       dbg(' ok twoarcs')
170
171     else: # twoarcs algorithm is not well defined
172       dbg(' no twoarcs')
173       start_la = 0.1
174       start_s = dist_pq_plane * .65
175       start_t = dist_pq_plane * .35
176       start_mu = 0.05
177
178     bodge = max( q_plane[2] * start_mu,
179                  (start_s + start_t) * 0.1 )
180     start_s += 0.5 * bodge
181     start_t += 0.5 * bodge
182     start_kappa = 0
183     start_gamma = 1
184
185     tilt = atan(start_mu)
186     tilt_basis = np.array([
187       [ 1,     0,           0,         0 ],
188       [ 0,   cos(tilt),  sin(tilt),    0 ],
189       [ 0,  -sin(tilt),  cos(tilt),    0 ],
190       [ 0,     0,           0,         1 ],
191     ])
192     findcurve_basis = matmatmultiply(dPQplane_basis, tilt_basis)
193     findcurve_into = np.linalg.inv(findcurve_basis)
194
195     for ax in range(0,3):
196       vdbg().arrow(findcurve_basis[0:3,3], findcurve_basis[0:3,ax])
197
198     q_findcurve = augmatmultiply(findcurve_into, q)
199     dq_findcurve = augmatmultiply(findcurve_into, dq, augwith=0)
200
201     findcurve_target = np.hstack((q_findcurve, dq_findcurve))
202     findcurve_start = (sqrt(start_s), sqrt(start_t), start_la,
203                        start_mu, start_gamma, start_kappa)
204     
205     findcurve_epsilon = dist_pq_plane * 0.01
206
207     global findcurve_subproc
208     if findcurve_subproc is None:
209       dbg('STARTING FINDCURVE')
210       findcurve_subproc = subprocess.Popen(
211         ['./findcurve'],
212         bufsize=1,
213         stdin=subprocess.PIPE,
214         stdout=subprocess.PIPE,
215         stderr=None,
216         close_fds=False,
217         # restore_signals=True, // want python2 compat, nnng
218         universal_newlines=True,
219       )
220
221     findcurve_input = np.hstack((findcurve_target,
222                                  findcurve_start,
223                                  [findcurve_epsilon]))
224
225     def dbg_fmt_params(fcp):
226       return (('s=%10.7f t=%10.7f sh=%10.7f'
227                +' st=%10.7f la=%10.7f mu=%10.7f ga=%10.7f ka=%10.7f')
228               %
229               (( fcp[0]**2, fcp[1]**2 ) + tuple(fcp)))
230
231     #dbg('>> ' + ' '.join(map(str,findcurve_input)))
232
233     dbg(('RUNNING FINDCURVE' +
234          '                                             ' +
235          ' target Q=[%10.7f %10.7f %10.7f] dQ=[%10.7f %10.7f %10.7f]')
236         %
237         tuple(findcurve_input[0:6]))
238     dbg(('%s  initial') % dbg_fmt_params(findcurve_input[6:12]))
239
240     print(*findcurve_input, file=findcurve_subproc.stdin)
241     findcurve_subproc.stdin.flush()
242
243     hc.func = symbolic.get_python()
244     commentary = ''
245
246     while True:
247       l = findcurve_subproc.stdout.readline()
248       l = l.rstrip()
249       dbg('<< ', l)
250       if not l: vdbg().crashing('findcurve EOF')
251       if not l.startswith('['):
252         commentary += ' '
253         commentary += l
254         continue
255
256       l = eval(l)
257       if not l: break
258
259       dbg(('%s Q=[%10.7f %10.7f %10.7f] dQ=[%10.7f %10.7f %10.7f]%s')
260           %
261           (( dbg_fmt_params(l[0:6]), ) + tuple(l[6:12]) + (commentary,) ))
262       commentary = ''
263
264       hc.findcurve_result = l[0:6]
265       hc.threshold = l[0]**2
266       hc.total_dist = hc.threshold + l[1]**2
267       #vdbg().curve( hc.point_at_t )
268
269   def point_at_t(hc, normalised_parameter):
270     dist = normalised_parameter * hc.total_dist
271     ours = list(hc.findcurve_result)
272     if dist <= hc.threshold:
273       ours[0] = sqrt(dist)
274       ours[1] = 0
275     else:
276       ours[1] = sqrt(dist - hc.threshold)
277     asmat = hc.func(*ours)
278     p = asmat[:,0]
279     return p