summaryrefslogtreecommitdiff
path: root/third_party/spiro/curves
diff options
context:
space:
mode:
authorRoozbeh Pournader <roozbeh@google.com>2014-07-26 11:20:29 -0700
committerRoozbeh Pournader <roozbeh@google.com>2014-07-26 11:20:29 -0700
commit0f1bd27678fb542f8601bbaf493baa1c22732fe2 (patch)
tree78ecb8f8082fb1b3ae24720549578b9272f36f0c /third_party/spiro/curves
parent5a480b939b43dec473550292501a88518fe1208c (diff)
Add Spiro version 0.01.
Diffstat (limited to 'third_party/spiro/curves')
-rw-r--r--third_party/spiro/curves/band.py85
-rw-r--r--third_party/spiro/curves/bezfigs.py183
-rw-r--r--third_party/spiro/curves/bigmat.py662
-rw-r--r--third_party/spiro/curves/cloth_off.py2
-rw-r--r--third_party/spiro/curves/clothoid.py66
-rw-r--r--third_party/spiro/curves/cornu.py140
-rw-r--r--third_party/spiro/curves/euler-elastica.py29
-rw-r--r--third_party/spiro/curves/fromcubic.py208
-rw-r--r--third_party/spiro/curves/mecsolve.py859
-rw-r--r--third_party/spiro/curves/mvc.py208
-rw-r--r--third_party/spiro/curves/numintsynth.py176
-rw-r--r--third_party/spiro/curves/offset.py21
-rw-r--r--third_party/spiro/curves/pcorn.py160
-rw-r--r--third_party/spiro/curves/plot_solve_clothoid.py164
-rw-r--r--third_party/spiro/curves/poly3.py148
-rw-r--r--third_party/spiro/curves/polymat-bad.py64
-rw-r--r--third_party/spiro/curves/polymat.py399
-rw-r--r--third_party/spiro/curves/tocubic.py461
18 files changed, 4035 insertions, 0 deletions
diff --git a/third_party/spiro/curves/band.py b/third_party/spiro/curves/band.py
new file mode 100644
index 0000000..7e61d35
--- /dev/null
+++ b/third_party/spiro/curves/band.py
@@ -0,0 +1,85 @@
+# A little solver for band-diagonal matrices. Based on NR Ch 2.4.
+
+from math import *
+
+from Numeric import *
+
+do_pivot = True
+
+def bandec(a, m1, m2):
+ n, m = a.shape
+ mm = m1 + m2 + 1
+ if m != mm:
+ raise ValueError('Array has width %d expected %d' % (m, mm))
+ al = zeros((n, m1), Float)
+ indx = zeros(n, Int)
+
+ for i in range(m1):
+ l = m1 - i
+ for j in range(l, mm): a[i, j - l] = a[i, j]
+ for j in range(mm - l, mm): a[i, j] = 0
+
+ d = 1.
+
+ l = m1
+ for k in range(n):
+ dum = a[k, 0]
+ pivot = k
+ if l < n: l += 1
+ if do_pivot:
+ for j in range(k + 1, l):
+ if abs(a[j, 0]) > abs(dum):
+ dum = a[j, 0]
+ pivot = j
+ indx[k] = pivot
+ if dum == 0.: a[k, 0] = 1e-20
+ if pivot != k:
+ d = -d
+ for j in range(mm):
+ tmp = a[k, j]
+ a[k, j] = a[pivot, j]
+ a[pivot, j] = tmp
+ for i in range(k + 1, l):
+ dum = a[i, 0] / a[k, 0]
+ al[k, i - k - 1] = dum
+ for j in range(1, mm):
+ a[i, j - 1] = a[i, j] - dum * a[k, j]
+ a[i, mm - 1] = 0.
+ return al, indx, d
+
+def banbks(a, m1, m2, al, indx, b):
+ n, m = a.shape
+ mm = m1 + m2 + 1
+ l = m1
+ for k in range(n):
+ i = indx[k]
+ if i != k:
+ tmp = b[k]
+ b[k] = b[i]
+ b[i] = tmp
+ if l < n: l += 1
+ for i in range(k + 1, l):
+ b[i] -= al[k, i - k - 1] * b[k]
+ l = 1
+ for i in range(n - 1, -1, -1):
+ dum = b[i]
+ for k in range(1, l):
+ dum -= a[i, k] * b[k + i]
+ b[i] = dum / a[i, 0]
+ if l < mm: l += 1
+
+if __name__ == '__main__':
+ a = zeros((10, 3), Float)
+ for i in range(10):
+ a[i, 0] = 1
+ a[i, 1] = 2
+ a[i, 2] = 1
+ print a
+ al, indx, d = bandec(a, 1, 1)
+ print a
+ print al
+ print indx
+ b = zeros(10, Float)
+ b[5] = 1
+ banbks(a, 1, 1, al, indx, b)
+ print b
diff --git a/third_party/spiro/curves/bezfigs.py b/third_party/spiro/curves/bezfigs.py
new file mode 100644
index 0000000..b7e408b
--- /dev/null
+++ b/third_party/spiro/curves/bezfigs.py
@@ -0,0 +1,183 @@
+import sys
+from math import *
+
+import fromcubic
+import tocubic
+
+import cornu
+
+def eps_prologue(x0, y0, x1, y1, draw_box = False):
+ print '%!PS-Adobe-3.0 EPSF'
+ print '%%BoundingBox:', x0, y0, x1, y1
+ print '%%EndComments'
+ print '%%EndProlog'
+ print '%%Page: 1 1'
+ if draw_box:
+ print x0, y0, 'moveto', x0, y1, 'lineto', x1, y1, 'lineto', x1, y0, 'lineto closepath stroke'
+
+def eps_trailer():
+ print '%%EOF'
+
+def fit_cubic_superfast(z0, z1, arclen, th0, th1, aab):
+ chord = hypot(z1[0] - z0[0], z1[1] - z0[1])
+ cth0, sth0 = cos(th0), sin(th0)
+ cth1, sth1 = -cos(th1), -sin(th1)
+ armlen = .66667 * arclen
+ a = armlen * aab
+ b = armlen - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ return bz
+
+def fit_cubic(z0, z1, arclen, th_fn, fast, aabmin = 0, aabmax = 1.):
+ chord = hypot(z1[0] - z0[0], z1[1] - z0[1])
+ if (arclen < 1.000001 * chord):
+ return [z0, z1], 0
+ th0 = th_fn(0)
+ th1 = th_fn(arclen)
+ imax = 4
+ jmax = 10
+ if fast:
+ imax = 1
+ jmax = 0
+ for i in range(imax):
+ for j in range(jmax + 1):
+ if jmax == 0:
+ aab = 0.5 * (aabmin + aabmax)
+ else:
+ aab = aabmin + (aabmax - aabmin) * j / jmax
+ if fast == 2:
+ bz = fit_cubic_superfast(z0, z1, arclen, th0, th1, aab)
+ else:
+ bz = tocubic.fit_cubic_arclen(z0, z1, arclen, th0, th1, aab)
+ score = tocubic.measure_bz_rk4(bz, arclen, th_fn)
+ print '% aab =', aab, 'score =', score
+ sys.stdout.flush()
+ if j == 0 or score < best_score:
+ best_score = score
+ best_aab = aab
+ best_bz = bz
+ daab = .06 * (aabmax - aabmin)
+ aabmin = max(0, best_aab - daab)
+ aabmax = min(1, best_aab + daab)
+ print '%--- best_aab =', best_aab
+ return best_bz, best_score
+
+def cornu_to_cubic(t0, t1, figno):
+ if figno == 1:
+ aabmin = 0
+ aabmax = 0.4
+ elif figno == 2:
+ aabmin = 0.5
+ aabmax = 1.
+ else:
+ aabmin = 0
+ aabmax = 1.
+ fast = 0
+ if figno == 3:
+ fast = 1
+ elif figno == 4:
+ fast = 2
+ def th_fn(s):
+ return (s + t0) ** 2
+ y0, x0 = cornu.eval_cornu(t0)
+ y1, x1 = cornu.eval_cornu(t1)
+ bz, score = fit_cubic((x0, y0), (x1, y1), t1 - t0, th_fn, fast, aabmin, aabmax)
+ return bz, score
+
+def plot_k_of_bz(bz):
+ dbz = tocubic.bz_deriv(bz)
+ ddbz = tocubic.bz_deriv(dbz)
+ cmd = 'moveto'
+ ss = [0]
+ def arclength_deriv(x, ss):
+ dx, dy = tocubic.bz_eval(dbz, x)
+ return [hypot(dx, dy)]
+ dt = 0.01
+ t = 0
+ for i in range(101):
+ dx, dy = tocubic.bz_eval(dbz, t)
+ ddx, ddy = tocubic.bz_eval(ddbz, t)
+ k = (ddy * dx - dy * ddx) / (dx * dx + dy * dy) ** 1.5
+ print 100 + 500 * ss[0], 100 + 200 * k, cmd
+ cmd = 'lineto'
+
+ dsdx = arclength_deriv(t, ss)
+ tocubic.rk4(ss, dsdx, t, .01, arclength_deriv)
+ t += dt
+ print 'stroke'
+
+def plot_k_nominal(s0, s1):
+ k0 = 2 * s0
+ k1 = 2 * s1
+ print 'gsave 0.5 setlinewidth'
+ print 100, 100 + 200 * k0, 'moveto'
+ print 100 + 500 * (s1 - s0), 100 + 200 * k1, 'lineto'
+ print 'stroke grestore'
+
+def simple_bez():
+ eps_prologue(95, 126, 552, 508, 0)
+ tocubic.plot_prolog()
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ bz, score = cornu_to_cubic(.5, 1.1, 2)
+ fromcubic.plot_bzs([[bz]], (-400, 100), 1000, True)
+ print 'stroke'
+ print '/Times-Roman 12 selectfont'
+ print '95 130 moveto ((x0, y0)) show'
+ print '360 200 moveto ((x1, y1)) show'
+ print '480 340 moveto ((x2, y2)) show'
+ print '505 495 moveto ((x3, y3)) show'
+ print 'showpage'
+ eps_trailer()
+
+def fast_bez(figno):
+ if figno == 3:
+ y1 = 520
+ else:
+ y1 = 550
+ eps_prologue(95, 140, 552, y1, 0)
+ tocubic.plot_prolog()
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ bz, score = cornu_to_cubic(.5, 1.1, figno)
+ fromcubic.plot_bzs([[bz]], (-400, 100), 1000, True)
+ print 'stroke'
+ plot_k_nominal(.5, 1.1)
+ plot_k_of_bz(bz)
+ print 'showpage'
+ eps_trailer()
+
+def bezfig(s1):
+ eps_prologue(95, 38, 510, 550, 0)
+ #print '0.5 0.5 scale 500 100 translate'
+ tocubic.plot_prolog()
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ bz, score = cornu_to_cubic(.5, 0.85, 1)
+ fromcubic.plot_bzs([[bz]], (-400, 0), 1000, True)
+ print 'stroke'
+ plot_k_nominal(.5, 0.85)
+ plot_k_of_bz(bz)
+ bz, score = cornu_to_cubic(.5, 0.85, 2)
+ fromcubic.plot_bzs([[bz]], (-400, 100), 1000, True)
+ print 'stroke'
+ print 'gsave 0 50 translate'
+ plot_k_nominal(.5, .85)
+ plot_k_of_bz(bz)
+ print 'grestore'
+ print 'showpage'
+
+import sys
+
+if __name__ == '__main__':
+ figno = int(sys.argv[1])
+ if figno == 0:
+ simple_bez()
+ elif figno == 1:
+ bezfig(1.0)
+ elif figno == 2:
+ bezfig(0.85)
+ else:
+ fast_bez(figno)
+ #fast_bez(4)
diff --git a/third_party/spiro/curves/bigmat.py b/third_party/spiro/curves/bigmat.py
new file mode 100644
index 0000000..c1fac0c
--- /dev/null
+++ b/third_party/spiro/curves/bigmat.py
@@ -0,0 +1,662 @@
+# Solver based on direct Newton solving of 4 parameters for each curve
+# segment
+
+import sys
+from math import *
+
+from Numeric import *
+import LinearAlgebra as la
+
+import poly3
+import band
+
+class Seg:
+ def __init__(self, chord, th):
+ self.ks = [0., 0., 0., 0.]
+ self.chord = chord
+ self.th = th
+ def compute_ends(self, ks):
+ chord, ch_th = poly3.integ_chord(ks)
+ l = chord / self.chord
+ thl = ch_th - (-.5 * ks[0] + .125 * ks[1] - 1./48 * ks[2] + 1./384 * ks[3])
+ thr = (.5 * ks[0] + .125 * ks[1] + 1./48 * ks[2] + 1./384 * ks[3]) - ch_th
+ k0l = l * (ks[0] - .5 * ks[1] + .125 * ks[2] - 1./48 * ks[3])
+ k0r = l * (ks[0] + .5 * ks[1] + .125 * ks[2] + 1./48 * ks[3])
+ l2 = l * l
+ k1l = l2 * (ks[1] - .5 * ks[2] + .125 * ks[3])
+ k1r = l2 * (ks[1] + .5 * ks[2] + .125 * ks[3])
+ l3 = l2 * l
+ k2l = l3 * (ks[2] - .5 * ks[3])
+ k2r = l3 * (ks[2] + .5 * ks[3])
+ return (thl, k0l, k1l, k2l), (thr, k0r, k1r, k2r), l
+ def set_ends_from_ks(self):
+ self.endl, self.endr, self.l = self.compute_ends(self.ks)
+ def fast_pderivs(self):
+ l = self.l
+ l2 = l * l
+ l3 = l2 * l
+ return [((.5, l, 0, 0), (.5, l, 0, 0)),
+ ((-1./12, -l/2, l2, 0), (1./12, l/2, l2, 0)),
+ ((1./48, l/8, -l2/2, l3), (1./48, l/8, l2/2, l3)),
+ ((-1./480, -l/48, l2/8, -l3/2), (1./480, l/48, l2/8, l3/2))]
+ def compute_pderivs(self):
+ rd = 2e6
+ delta = 1./rd
+ base_ks = self.ks
+ base_endl, base_endr, dummy = self.compute_ends(base_ks)
+ result = []
+ for i in range(4):
+ try_ks = base_ks[:]
+ try_ks[i] += delta
+ try_endl, try_endr, dummy = self.compute_ends(try_ks)
+ deriv_l = (rd * (try_endl[0] - base_endl[0]),
+ rd * (try_endl[1] - base_endl[1]),
+ rd * (try_endl[2] - base_endl[2]),
+ rd * (try_endl[3] - base_endl[3]))
+ deriv_r = (rd * (try_endr[0] - base_endr[0]),
+ rd * (try_endr[1] - base_endr[1]),
+ rd * (try_endr[2] - base_endr[2]),
+ rd * (try_endr[3] - base_endr[3]))
+ result.append((deriv_l, deriv_r))
+ return result
+
+class Node:
+ def __init__(self, x, y, ty, th):
+ self.x = x
+ self.y = y
+ self.ty = ty
+ self.th = th
+ def continuity(self):
+ if self.ty == 'o':
+ return 4
+ elif self.ty in ('c', '[', ']'):
+ return 2
+ else:
+ return 0
+
+def mod_2pi(th):
+ u = th / (2 * pi)
+ return 2 * pi * (u - floor(u + 0.5))
+
+def setup_path(path):
+ segs = []
+ nodes = []
+ nsegs = len(path)
+ if path[0][2] == '{':
+ nsegs -= 1
+ for i in range(nsegs):
+ i1 = (i + 1) % len(path)
+ x0, y0, t0 = path[i]
+ x1, y1, t1 = path[i1]
+ s = Seg(hypot(y1 - y0, x1 - x0), atan2(y1 - y0, x1 - x0))
+ segs.append(s)
+ for i in range(len(path)):
+ x0, y0, t0 = path[i]
+
+ if t0 in ('{', '}', 'v'):
+ th = 0
+ else:
+ s0 = segs[(i + len(path) - 1) % len(path)]
+ s1 = segs[i]
+ th = mod_2pi(s1.th - s0.th)
+
+ n = Node(x0, y0, t0, th)
+ nodes.append(n)
+ return segs, nodes
+
+def count_vec(nodes):
+ jincs = []
+ n = 0
+ for i in range(len(nodes)):
+ i1 = (i + 1) % len(nodes)
+ t0 = nodes[i].ty
+ t1 = nodes[i1].ty
+ if t0 in ('{', '}', 'v', '[') and t1 in ('{', '}', 'v', ']'):
+ jinc = 0
+ elif t0 in ('{', '}', 'v', '[') and t1 == 'c':
+ jinc = 1
+ elif t0 == 'c' and t1 in ('{', '}', 'v', ']'):
+ jinc = 1
+ elif t0 == 'c' and t1 == 'c':
+ jinc = 2
+ else:
+ jinc = 4
+ jincs.append(jinc)
+ n += jinc
+ return n, jincs
+
+thscale, k0scale, k1scale, k2scale = 1, 1, 1, 1
+
+def inversedot_woodbury(m, v):
+ a = zeros((n, 11), Float)
+ for i in range(n):
+ for j in range(max(-7, -i), min(4, n - i)):
+ a[i, j + 7] = m[i, i + j]
+ print a
+ al, indx, d = band.bandec(a, 7, 3)
+ VtZ = identity(4, Float)
+ Z = zeros((n, 4), Float)
+ for i in range(4):
+ u = zeros(n, Float)
+ for j in range(4):
+ u[j] = m[j, n - 4 + i]
+ band.banbks(a, 7, 3, al, indx, u)
+ for k in range(n):
+ Z[k, i] = u[k]
+ #Z[:,i] = u
+ for j in range(4):
+ VtZ[j, i] += u[n - 4 + j]
+ print Z
+ print VtZ
+ H = la.inverse(VtZ)
+ print H
+ band.banbks(a, 7, 3, al, indx, v)
+ return(v - dot(Z, dot(H, v[n - 4:])))
+
+def inversedot(m, v):
+ return dot(la.inverse(m), v)
+ n, nn = m.shape
+ if 1:
+ for i in range(n):
+ sys.stdout.write('% ')
+ for j in range(n):
+ if m[i, j] > 0: sys.stdout.write('+ ')
+ elif m[i, j] < 0: sys.stdout.write('- ')
+ else: sys.stdout.write(' ')
+ sys.stdout.write('\n')
+
+ cyclic = False
+ for i in range(4):
+ for j in range(n - 4, n):
+ if m[i, j] != 0:
+ cyclic = True
+ print '% cyclic:', cyclic
+ if not cyclic:
+ a = zeros((n, 11), Float)
+ for i in range(n):
+ for j in range(max(-5, -i), min(6, n - i)):
+ a[i, j + 5] = m[i, i + j]
+ for i in range(n):
+ sys.stdout.write('% ')
+ for j in range(11):
+ if a[i, j] > 0: sys.stdout.write('+ ')
+ elif a[i, j] < 0: sys.stdout.write('- ')
+ else: sys.stdout.write(' ')
+ sys.stdout.write('\n')
+ al, indx, d = band.bandec(a, 5, 5)
+ print a
+ band.banbks(a, 5, 5, al, indx, v)
+ return v
+ else:
+ #return inversedot_woodbury(m, v)
+ bign = 3 * n
+ a = zeros((bign, 11), Float)
+ u = zeros(bign, Float)
+ for i in range(bign):
+ u[i] = v[i % n]
+ for j in range(-7, 4):
+ a[i, j + 7] = m[i % n, (i + j + 7 * n) % n]
+ #print a
+ if 1:
+ for i in range(bign):
+ sys.stdout.write('% ')
+ for j in range(11):
+ if a[i, j] > 0: sys.stdout.write('+ ')
+ elif a[i, j] < 0: sys.stdout.write('- ')
+ else: sys.stdout.write(' ')
+ sys.stdout.write('\n')
+ #print u
+ al, indx, d = band.bandec(a, 5, 5)
+ band.banbks(a, 5, 5, al, indx, u)
+ #print u
+ return u[n + 2: 2 * n + 2]
+
+def iter(segs, nodes):
+ n, jincs = count_vec(nodes)
+ print '%', jincs
+ v = zeros(n, Float)
+ m = zeros((n, n), Float)
+ for i in range(len(segs)):
+ segs[i].set_ends_from_ks()
+ j = 0
+ j0 = 0
+ for i in range(len(segs)):
+ i1 = (i + 1) % len(nodes)
+ t0 = nodes[i].ty
+ t1 = nodes[i1].ty
+ seg = segs[i]
+
+ derivs = seg.compute_pderivs()
+ print '%derivs:', derivs
+
+ jinc = jincs[i] # the number of params on this seg
+ print '%', t0, t1, jinc, j0
+
+ # The constraints are laid out as follows:
+ # constraints that cross the node on the left
+ # constraints on the left side
+ # constraints on the right side
+ # constraints that cross the node on the right
+
+ jj = j0 # the index into the constraint row we're writing
+ jthl, jk0l, jk1l, jk2l = -1, -1, -1, -1
+ jthr, jk0r, jk1r, jk2r = -1, -1, -1, -1
+
+ # constraints crossing left
+
+ if t0 == 'o':
+ jthl = jj + 0
+ jk0l = jj + 1
+ jk1l = jj + 2
+ jk2l = jj + 3
+ jj += 4
+ elif t0 in ('c', '[', ']'):
+ jthl = jj + 0
+ jk0l = jj + 1
+ jj += 2
+
+ # constraints on left
+
+ if t0 in ('[', 'v', '{') and jinc == 4:
+ jk1l = jj
+ jj += 1
+ if t0 in ('[', 'v', '{', 'c') and jinc == 4:
+ jk2l = jj
+ jj += 1
+
+ # constraints on right
+
+ if t1 in (']', 'v', '}') and jinc == 4:
+ jk1r = jj
+ jj += 1
+ if t1 in (']', 'v', '}', 'c') and jinc == 4:
+ jk2r = jj
+ jj += 1
+
+ # constraints crossing right
+
+ jj %= n
+ j1 = jj
+
+ if t1 == 'o':
+ jthr = jj + 0
+ jk0r = jj + 1
+ jk1r = jj + 2
+ jk2r = jj + 3
+ jj += 4
+ elif t1 in ('c', '[', ']'):
+ jthr = jj + 0
+ jk0r = jj + 1
+ jj += 2
+
+ print '%', jthl, jk0l, jk1l, jk2l, jthr, jk0r, jk1r, jk2r
+
+ if jthl >= 0:
+ v[jthl] += thscale * (nodes[i].th - seg.endl[0])
+ if jinc == 1:
+ m[jthl][j] += derivs[0][0][0]
+ elif jinc == 2:
+ m[jthl][j + 1] += derivs[0][0][0]
+ m[jthl][j] += derivs[1][0][0]
+ elif jinc == 4:
+ m[jthl][j + 2] += derivs[0][0][0]
+ m[jthl][j + 3] += derivs[1][0][0]
+ m[jthl][j + 0] += derivs[2][0][0]
+ m[jthl][j + 1] += derivs[3][0][0]
+ if jk0l >= 0:
+ v[jk0l] += k0scale * seg.endl[1]
+ if jinc == 1:
+ m[jk0l][j] -= derivs[0][0][1]
+ elif jinc == 2:
+ m[jk0l][j + 1] -= derivs[0][0][1]
+ m[jk0l][j] -= derivs[1][0][1]
+ elif jinc == 4:
+ m[jk0l][j + 2] -= derivs[0][0][1]
+ m[jk0l][j + 3] -= derivs[1][0][1]
+ m[jk0l][j + 0] -= derivs[2][0][1]
+ m[jk0l][j + 1] -= derivs[3][0][1]
+ if jk1l >= 0:
+ v[jk1l] += k1scale * seg.endl[2]
+ m[jk1l][j + 2] -= derivs[0][0][2]
+ m[jk1l][j + 3] -= derivs[1][0][2]
+ m[jk1l][j + 0] -= derivs[2][0][2]
+ m[jk1l][j + 1] -= derivs[3][0][2]
+ if jk2l >= 0:
+ v[jk2l] += k2scale * seg.endl[3]
+ m[jk2l][j + 2] -= derivs[0][0][3]
+ m[jk2l][j + 3] -= derivs[1][0][3]
+ m[jk2l][j + 0] -= derivs[2][0][3]
+ m[jk2l][j + 1] -= derivs[3][0][3]
+
+ if jthr >= 0:
+ v[jthr] -= thscale * seg.endr[0]
+ if jinc == 1:
+ m[jthr][j] += derivs[0][1][0]
+ elif jinc == 2:
+ m[jthr][j + 1] += derivs[0][1][0]
+ m[jthr][j + 0] += derivs[1][1][0]
+ elif jinc == 4:
+ m[jthr][j + 2] += derivs[0][1][0]
+ m[jthr][j + 3] += derivs[1][1][0]
+ m[jthr][j + 0] += derivs[2][1][0]
+ m[jthr][j + 1] += derivs[3][1][0]
+ if jk0r >= 0:
+ v[jk0r] -= k0scale * seg.endr[1]
+ if jinc == 1:
+ m[jk0r][j] += derivs[0][1][1]
+ elif jinc == 2:
+ m[jk0r][j + 1] += derivs[0][1][1]
+ m[jk0r][j + 0] += derivs[1][1][1]
+ elif jinc == 4:
+ m[jk0r][j + 2] += derivs[0][1][1]
+ m[jk0r][j + 3] += derivs[1][1][1]
+ m[jk0r][j + 0] += derivs[2][1][1]
+ m[jk0r][j + 1] += derivs[3][1][1]
+ if jk1r >= 0:
+ v[jk1r] -= k1scale * seg.endr[2]
+ m[jk1r][j + 2] += derivs[0][1][2]
+ m[jk1r][j + 3] += derivs[1][1][2]
+ m[jk1r][j + 0] += derivs[2][1][2]
+ m[jk1r][j + 1] += derivs[3][1][2]
+ if jk2r >= 0:
+ v[jk2r] -= k2scale * seg.endr[3]
+ m[jk2r][j + 2] += derivs[0][1][3]
+ m[jk2r][j + 3] += derivs[1][1][3]
+ m[jk2r][j + 0] += derivs[2][1][3]
+ m[jk2r][j + 1] += derivs[3][1][3]
+
+ j += jinc
+ j0 = j1
+ #print m
+ dk = inversedot(m, v)
+ dkmul = 1
+ j = 0
+ for i in range(len(segs)):
+ jinc = jincs[i]
+ if jinc == 1:
+ segs[i].ks[0] += dkmul * dk[j]
+ elif jinc == 2:
+ segs[i].ks[0] += dkmul * dk[j + 1]
+ segs[i].ks[1] += dkmul * dk[j + 0]
+ elif jinc == 4:
+ segs[i].ks[0] += dkmul * dk[j + 2]
+ segs[i].ks[1] += dkmul * dk[j + 3]
+ segs[i].ks[2] += dkmul * dk[j + 0]
+ segs[i].ks[3] += dkmul * dk[j + 1]
+ j += jinc
+
+ norm = 0.
+ for i in range(len(dk)):
+ norm += dk[i] * dk[i]
+ return sqrt(norm)
+
+
+def plot_path(segs, nodes, tol = 1.0, show_cpts = False):
+ if show_cpts:
+ cpts = []
+ j = 0
+ cmd = 'moveto'
+ for i in range(len(segs)):
+ i1 = (i + 1) % len(nodes)
+ n0 = nodes[i]
+ n1 = nodes[i1]
+ x0, y0, t0 = n0.x, n0.y, n0.ty
+ x1, y1, t1 = n1.x, n1.y, n1.ty
+ ks = segs[i].ks
+ abs_ks = abs(ks[0]) + abs(ks[1] / 2) + abs(ks[2] / 8) + abs(ks[3] / 48)
+ n_subdiv = int(ceil(.001 + tol * abs_ks))
+ n_subhalf = (n_subdiv + 1) / 2
+ if n_subdiv > 1:
+ n_subdiv = n_subhalf * 2
+ ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
+ pside = poly3.int_3spiro_poly(ksp, n_subhalf)
+ ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
+ mside = poly3.int_3spiro_poly(ksm, n_subhalf)
+ mside.reverse()
+ for j in range(len(mside)):
+ mside[j] = (-mside[j][0], -mside[j][1])
+ if n_subdiv > 1:
+ pts = mside + pside[1:]
+ else:
+ pts = mside[:1] + pside[1:]
+ chord_th = atan2(y1 - y0, x1 - x0)
+ chord_len = hypot(y1 - y0, x1 - x0)
+ rot = chord_th - atan2(pts[-1][1] - pts[0][1], pts[-1][0] - pts[0][0])
+ scale = chord_len / hypot(pts[-1][1] - pts[0][1], pts[-1][0] - pts[0][0])
+ u, v = scale * cos(rot), scale * sin(rot)
+ xt = x0 - u * pts[0][0] + v * pts[0][1]
+ yt = y0 - u * pts[0][1] - v * pts[0][0]
+ s = -.5
+ for x, y in pts:
+ xp, yp = xt + u * x - v * y, yt + u * y + v * x
+ thp = (((ks[3]/24 * s + ks[2]/6) * s + ks[1] / 2) * s + ks[0]) * s + rot
+ up, vp = scale / (1.5 * n_subdiv) * cos(thp), scale / (1.5 * n_subdiv) * sin(thp)
+ if s == -.5:
+ if cmd == 'moveto':
+ print xp, yp, 'moveto'
+ cmd = 'curveto'
+ else:
+ if show_cpts:
+ cpts.append((xlast + ulast, ylast + vlast))
+ cpts.append((xp - up, yp - vp))
+ print xlast + ulast, ylast + vlast, xp - up, yp - vp, xp, yp, 'curveto'
+ xlast, ylast, ulast, vlast = xp, yp, up, vp
+ s += 1. / n_subdiv
+ if t1 == 'v':
+ j += 2
+ else:
+ j += 1
+ print 'stroke'
+ if show_cpts:
+ for x, y in cpts:
+ print 'gsave 0 0 1 setrgbcolor', x, y, 'translate circle fill grestore'
+
+def plot_ks(segs, nodes, xo, yo, xscale, yscale):
+ j = 0
+ cmd = 'moveto'
+ x = xo
+ for i in range(len(segs)):
+ i1 = (i + 1) % len(nodes)
+ n0 = nodes[i]
+ n1 = nodes[i1]
+ x0, y0, t0 = n0.x, n0.y, n0.ty
+ x1, y1, t1 = n1.x, n1.y, n1.ty
+ ks = segs[i].ks
+ chord, ch_th = poly3.integ_chord(ks)
+ l = chord/segs[i].chord
+ k0 = l * poly3.eval_cubic(ks[0], ks[1], .5 * ks[2], 1./6 * ks[3], -.5)
+ print x, yo + yscale * k0, cmd
+ cmd = 'lineto'
+ k3 = l * poly3.eval_cubic(ks[0], ks[1], .5 * ks[2], 1./6 * ks[3], .5)
+ k1 = k0 + l/3 * (ks[1] - 0.5 * ks[2] + .125 * ks[3])
+ k2 = k3 - l/3 * (ks[1] + 0.5 * ks[2] + .125 * ks[3])
+ print x + xscale / l / 3., yo + yscale * k1
+ print x + 2 * xscale / l / 3., yo + yscale * k2
+ print x + xscale / l, yo + yscale * k3, 'curveto'
+ x += xscale / l
+ if t1 == 'v':
+ j += 2
+ else:
+ j += 1
+ print 'stroke'
+ print xo, yo, 'moveto', x, yo, 'lineto stroke'
+
+def plot_nodes(nodes, segs):
+ for i in range(len(nodes)):
+ n = nodes[i]
+ print 'gsave', n.x, n.y, 'translate'
+ if n.ty in ('c', '[', ']'):
+ th = segs[i].th - segs[i].endl[0]
+ if n.ty == ']': th += pi
+ print 180 * th / pi, 'rotate'
+ if n.ty == 'o':
+ print 'circle fill'
+ elif n.ty == 'c':
+ print '3 4 poly fill'
+ elif n.ty in ('v', '{', '}'):
+ print '45 rotate 3 4 poly fill'
+ elif n.ty in ('[', ']'):
+ print '0 -3 moveto 0 0 3 90 270 arc fill'
+ else:
+ print '5 3 poly fill'
+ print 'grestore'
+
+def prologue():
+ print '/ss 2 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ print '/poly {'
+ print ' dup false exch {'
+ print ' 0 3 index 2 index { lineto } { moveto } ifelse pop true'
+ print ' 360.0 2 index div rotate'
+ print ' } repeat pop pop pop'
+ print '} bind def'
+
+def run_path(path, show_iter = False, n = 10, xo = 36, yo = 550, xscale = .25, yscale = 2000, pl_nodes = True):
+ segs, nodes = setup_path(path)
+ print '.5 setlinewidth'
+ for i in range(n):
+ if i == n - 1:
+ print '0 0 0 setrgbcolor 1 setlinewidth'
+ elif i == 0:
+ print '1 0 0 setrgbcolor'
+ elif i == 1:
+ print '0 0.5 0 setrgbcolor'
+ elif i == 2:
+ print '0.3 0.3 0.3 setrgbcolor'
+ norm = iter(segs, nodes)
+ print '% norm =', norm
+ if show_iter or i == n - 1:
+ #print '1 0 0 setrgbcolor'
+ #plot_path(segs, nodes, tol = 5)
+ #print '0 0 0 setrgbcolor'
+ plot_path(segs, nodes, tol = 1.0)
+ plot_ks(segs, nodes, xo, yo, xscale, yscale)
+ if pl_nodes:
+ plot_nodes(nodes, segs)
+
+if __name__ == '__main__':
+ if 1:
+ path = [(100, 350, 'o'), (225, 350, 'o'), (350, 450, 'o'),
+ (450, 400, 'o'), (315, 205, 'o'), (300, 200, 'o'),
+ (285, 205, 'o')]
+
+ if 1:
+ path = [(100, 350, 'o'), (175, 375, '['), (250, 375, ']'), (325, 425, '['),
+ (325, 450, ']'),
+ (400, 475, 'o'), (350, 200, 'c')]
+
+ if 0:
+ ecc, ty, ty1 = 0.56199, 'c', 'c'
+ ecc, ty, ty1 = 0.49076, 'o', 'o',
+ ecc, ty, ty1 = 0.42637, 'o', 'c'
+ path = [(300 - 200 * ecc, 300, ty), (300, 100, ty1),
+ (300 + 200 * ecc, 300, ty), (300, 500, ty1)]
+
+ # difficult path #3
+ if 0:
+ path = [(100, 300, '{'), (225, 350, 'o'), (350, 500, 'o'),
+ (450, 500, 'o'), (450, 450, 'o'), (300, 200, '}')]
+
+ if 0:
+ path = [(100, 100, '{'), (200, 180, 'v'), (250, 215, 'o'),
+ (300, 200, 'o'), (400, 100, '}')]
+
+ if 0:
+ praw = [
+ (134, 90, 'o'),
+ (192, 68, 'o'),
+ (246, 66, 'o'),
+ (307, 107, 'o'),
+ (314, 154, '['),
+ (317, 323, ']'),
+ (347, 389, 'o'),
+ (373, 379, 'v'),
+ (386, 391, 'v'),
+ (365, 409, 'o'),
+ (335, 419, 'o'),
+ (273, 390, 'v'),
+ (251, 405, 'o'),
+ (203, 423, 'o'),
+ (102, 387, 'o'),
+ (94, 321, 'o'),
+ (143, 276, 'o'),
+ (230, 251, 'o'),
+ (260, 250, 'v'),
+ (260, 220, '['),
+ (258, 157, ']'),
+ (243, 110, 'o'),
+ (159, 99, 'o'),
+ (141, 121, 'o'),
+ (156, 158, 'o'),
+ (121, 184, 'o'),
+ (106, 117, 'o')]
+ if 0:
+ praw = [
+ (275, 56, 'o'),
+ (291, 75, 'v'),
+ (312, 61, 'o'),
+ (344, 50, 'o'),
+ (373, 72, 'o'),
+ (356, 91, 'o'),
+ (334, 81, 'o'),
+ (297, 85, 'v'),
+ (306, 116, 'o'),
+ (287, 180, 'o'),
+ (236, 213, 'o'),
+ (182, 212, 'o'),
+ (157, 201, 'v'),
+ (149, 209, 'o'),
+ (143, 230, 'o'),
+ (162, 246, 'c'),
+ (202, 252, 'o'),
+ (299, 260, 'o'),
+ (331, 282, 'o'),
+ (341, 341, 'o'),
+ (308, 390, 'o'),
+ (258, 417, 'o'),
+ (185, 422, 'o'),
+ (106, 377, 'o'),
+ (110, 325, 'o'),
+ (133, 296, 'o'),
+ (147, 283, 'v'),
+ (117, 238, 'o'),
+ (133, 205, 'o'),
+ (147, 191, 'v'),
+ (126, 159, 'o'),
+ (128, 94, 'o'),
+ (167, 50, 'o'),
+ (237, 39, 'o')]
+
+ path = []
+ for x, y, ty in praw:
+ #if ty == 'o': ty = 'c'
+ path.append((x, 550 - y, ty))
+
+ if 0:
+ path = [(100, 300, 'o'), (300, 100, 'o'), (300, 500, 'o')]
+
+ if 0:
+ # Woodford data set
+ ty = 'o'
+ praw = [(0, 0, '{'), (1, 1.9, ty), (2, 2.7, ty), (3, 2.6, ty),
+ (4, 1.6, ty), (5, .89, ty), (6, 1.2, '}')]
+ path = []
+ for x, y, t in praw:
+ path.append((100 + 80 * x, 100 + 80 * y, t))
+
+ if 0:
+ ycen = 32
+ yrise = 0
+ path = []
+ ty = '{'
+ for i in range(10):
+ path.append((50 + i * 30, 250 + (10 - i) * yrise, ty))
+ ty = 'o'
+ path.append((350, 250 + ycen, ty))
+ for i in range(1, 10):
+ path.append((350 + i * 30, 250 + i * yrise, ty))
+ path.append((650, 250 + 10 * yrise, '}'))
+
+ prologue()
+
+ run_path(path, show_iter = True, n=5)
diff --git a/third_party/spiro/curves/cloth_off.py b/third_party/spiro/curves/cloth_off.py
new file mode 100644
index 0000000..0ebaf4d
--- /dev/null
+++ b/third_party/spiro/curves/cloth_off.py
@@ -0,0 +1,2 @@
+# Fancy new algorithms for computing the offset of a clothoid.
+
diff --git a/third_party/spiro/curves/clothoid.py b/third_party/spiro/curves/clothoid.py
new file mode 100644
index 0000000..b3ae6af
--- /dev/null
+++ b/third_party/spiro/curves/clothoid.py
@@ -0,0 +1,66 @@
+from math import *
+import cornu
+
+def mod_2pi(th):
+ u = th / (2 * pi)
+ return 2 * pi * (u - floor(u + 0.5))
+
+# Given clothoid k(s) = k0 + k1 s, compute th1 - th0 of chord from s = -.5
+# to .5.
+def compute_dth(k0, k1):
+ if k1 < 0:
+ return -compute_dth(k0, -k1)
+ elif k1 == 0:
+ return 0
+ sqrk1 = sqrt(2 * k1)
+ t0 = (k0 - .5 * k1) / sqrk1
+ t1 = (k0 + .5 * k1) / sqrk1
+ (y0, x0) = cornu.eval_cornu(t0)
+ (y1, x1) = cornu.eval_cornu(t1)
+ chord_th = atan2(y1 - y0, x1 - x0)
+ return mod_2pi(t1 * t1 - chord_th) - mod_2pi(chord_th - t0 * t0)
+
+def compute_chord(k0, k1):
+ if k1 == 0:
+ if k0 == 0:
+ return 1
+ else:
+ return sin(k0 * .5) / (k0 * .5)
+ sqrk1 = sqrt(2 * abs(k1))
+ t0 = (k0 - .5 * k1) / sqrk1
+ t1 = (k0 + .5 * k1) / sqrk1
+ (y0, x0) = cornu.eval_cornu(t0)
+ (y1, x1) = cornu.eval_cornu(t1)
+ return hypot(y1 - y0, x1 - x0) / abs(t1 - t0)
+
+# Given th0 and th1 at endpoints (measured from chord), return k0
+# and k1 such that the clothoid k(s) = k0 + k1 s, evaluated from
+# s = -.5 to .5, has the tangents given
+def solve_clothoid(th0, th1, verbose = False):
+ k0 = th0 + th1
+
+ # initial guess
+ k1 = 6 * (th1 - th0)
+ error = (th1 - th0) - compute_dth(k0, k1)
+ if verbose:
+ print k0, k1, error
+
+ k1_old, error_old = k1, error
+ # second guess based on d(dth)/dk1 ~ 1/6
+ k1 += 6 * error
+ error = (th1 - th0) - compute_dth(k0, k1)
+ if verbose:
+ print k0, k1, error
+
+ # secant method
+ for i in range(10):
+ if abs(error) < 1e-9: break
+ k1_old, error_old, k1 = k1, error, k1 + (k1_old - k1) * error / (error - error_old)
+ error = (th1 - th0) - compute_dth(k0, k1)
+ if verbose:
+ print k0, k1, error
+
+ return k0, k1
+
+if __name__ == '__main__':
+ print solve_clothoid(.06, .05, True)
diff --git a/third_party/spiro/curves/cornu.py b/third_party/spiro/curves/cornu.py
new file mode 100644
index 0000000..26f68e9
--- /dev/null
+++ b/third_party/spiro/curves/cornu.py
@@ -0,0 +1,140 @@
+from math import *
+
+# implementation adapted from cephes
+
+def polevl(x, coef):
+ ans = coef[-1]
+ for i in range(len(coef) - 2, -1, -1):
+ ans = ans * x + coef[i]
+ return ans
+
+sn = [
+-2.99181919401019853726E3,
+ 7.08840045257738576863E5,
+-6.29741486205862506537E7,
+ 2.54890880573376359104E9,
+-4.42979518059697779103E10,
+ 3.18016297876567817986E11
+]
+sn.reverse()
+sd = [
+ 1.00000000000000000000E0,
+ 2.81376268889994315696E2,
+ 4.55847810806532581675E4,
+ 5.17343888770096400730E6,
+ 4.19320245898111231129E8,
+ 2.24411795645340920940E10,
+ 6.07366389490084639049E11
+]
+sd.reverse()
+cn = [
+-4.98843114573573548651E-8,
+ 9.50428062829859605134E-6,
+-6.45191435683965050962E-4,
+ 1.88843319396703850064E-2,
+-2.05525900955013891793E-1,
+ 9.99999999999999998822E-1
+]
+cn.reverse()
+cd = [
+ 3.99982968972495980367E-12,
+ 9.15439215774657478799E-10,
+ 1.25001862479598821474E-7,
+ 1.22262789024179030997E-5,
+ 8.68029542941784300606E-4,
+ 4.12142090722199792936E-2,
+ 1.00000000000000000118E0
+]
+cd.reverse()
+
+fn = [
+ 4.21543555043677546506E-1,
+ 1.43407919780758885261E-1,
+ 1.15220955073585758835E-2,
+ 3.45017939782574027900E-4,
+ 4.63613749287867322088E-6,
+ 3.05568983790257605827E-8,
+ 1.02304514164907233465E-10,
+ 1.72010743268161828879E-13,
+ 1.34283276233062758925E-16,
+ 3.76329711269987889006E-20
+]
+fn.reverse()
+fd = [
+ 1.00000000000000000000E0,
+ 7.51586398353378947175E-1,
+ 1.16888925859191382142E-1,
+ 6.44051526508858611005E-3,
+ 1.55934409164153020873E-4,
+ 1.84627567348930545870E-6,
+ 1.12699224763999035261E-8,
+ 3.60140029589371370404E-11,
+ 5.88754533621578410010E-14,
+ 4.52001434074129701496E-17,
+ 1.25443237090011264384E-20
+]
+fd.reverse()
+gn = [
+ 5.04442073643383265887E-1,
+ 1.97102833525523411709E-1,
+ 1.87648584092575249293E-2,
+ 6.84079380915393090172E-4,
+ 1.15138826111884280931E-5,
+ 9.82852443688422223854E-8,
+ 4.45344415861750144738E-10,
+ 1.08268041139020870318E-12,
+ 1.37555460633261799868E-15,
+ 8.36354435630677421531E-19,
+ 1.86958710162783235106E-22
+]
+gn.reverse()
+gd = [
+ 1.00000000000000000000E0,
+ 1.47495759925128324529E0,
+ 3.37748989120019970451E-1,
+ 2.53603741420338795122E-2,
+ 8.14679107184306179049E-4,
+ 1.27545075667729118702E-5,
+ 1.04314589657571990585E-7,
+ 4.60680728146520428211E-10,
+ 1.10273215066240270757E-12,
+ 1.38796531259578871258E-15,
+ 8.39158816283118707363E-19,
+ 1.86958710162783236342E-22
+]
+gd.reverse()
+
+
+def fresnel(xxa):
+ x = abs(xxa)
+ x2 = x * x
+ if x2 < 2.5625:
+ t = x2 * x2
+ ss = x * x2 * polevl(t, sn) / polevl(t, sd)
+ cc = x * polevl(t, cn) / polevl(t, cd)
+ elif x > 36974.0:
+ ss = 0.5
+ cc = 0.5
+ else:
+ t = pi * x2
+ u = 1.0 / (t * t)
+ t = 1.0 / t
+ f = 1.0 - u * polevl(u, fn) / polevl(u, fd)
+ g = t * polevl(u, gn) / polevl(u, gd)
+ t = pi * .5 * x2
+ c = cos(t)
+ s = sin(t)
+ t = pi * x
+ cc = 0.5 + (f * s - g * c) / t
+ ss = 0.5 - (f * c + g * s) / t
+ if xxa < 0:
+ cc = -cc
+ ss = -ss
+ return ss, cc
+
+def eval_cornu(t):
+ spio2 = sqrt(pi * .5)
+ s, c = fresnel(t / spio2)
+ s *= spio2
+ c *= spio2
+ return s, c
diff --git a/third_party/spiro/curves/euler-elastica.py b/third_party/spiro/curves/euler-elastica.py
new file mode 100644
index 0000000..a721d1e
--- /dev/null
+++ b/third_party/spiro/curves/euler-elastica.py
@@ -0,0 +1,29 @@
+from math import *
+
+def plot_elastica(a, c):
+ s = 500
+ cmd = 'moveto'
+ dx = .001
+ x, y = 0, 0
+ if c * c > 2 * a * a:
+ g = sqrt(c * c - 2 * a * a)
+ x = g + .01
+ if c == 0:
+ x = .001
+ try:
+ for i in range(1000):
+ print 6 + s * x, 200 + s * y, cmd
+ cmd = 'lineto'
+ x += dx
+ if 1 and c * c > 2 * a * a:
+ print (c * c - x * x) * (x * x - g * g)
+ dy = dx * (x * x - .5 * c * c - .5 * g * g) / sqrt((c * c - x * x) * (x * x - g * g))
+ else:
+ dy = dx * (a * a - c * c + x * x)/sqrt((c * c - x * x) * (2 * a * a - c * c + x * x))
+ y += dy
+ except ValueError, e:
+ pass
+ print 'stroke'
+
+plot_elastica(1, 0)
+print 'showpage'
diff --git a/third_party/spiro/curves/fromcubic.py b/third_party/spiro/curves/fromcubic.py
new file mode 100644
index 0000000..210f9d4
--- /dev/null
+++ b/third_party/spiro/curves/fromcubic.py
@@ -0,0 +1,208 @@
+# Convert piecewise cubic into piecewise clothoid representation.
+
+from math import *
+
+import clothoid
+import pcorn
+import tocubic
+
+import offset
+
+def read_bz(f):
+ result = []
+ for l in f.xreadlines():
+ s = l.split()
+ if len(s) > 0:
+ cmd = s[-1]
+ #print s[:-1], cmd
+ if cmd == 'm':
+ sp = []
+ result.append(sp)
+ curpt = [float(x) for x in s[0:2]]
+ startpt = curpt
+ elif cmd == 'l':
+ newpt = [float(x) for x in s[0:2]]
+ sp.append((curpt, newpt))
+ curpt = newpt
+ elif cmd == 'c':
+ c1 = [float(x) for x in s[0:2]]
+ c2 = [float(x) for x in s[2:4]]
+ newpt = [float(x) for x in s[4:6]]
+ sp.append((curpt, c1, c2, newpt))
+ curpt = newpt
+ return result
+
+def plot_bzs(bzs, z0, scale, fancy = False):
+ for sp in bzs:
+ for i in range(len(sp)):
+ bz = sp[i]
+ tocubic.plot_bz(bz, z0, scale, i == 0)
+ print 'stroke'
+ if fancy:
+ for i in range(len(sp)):
+ bz = sp[i]
+
+ x0, y0 = z0[0] + scale * bz[0][0], z0[1] + scale * bz[0][1]
+ print 'gsave', x0, y0, 'translate circle fill grestore'
+ if len(bz) == 4:
+ x1, y1 = z0[0] + scale * bz[1][0], z0[1] + scale * bz[1][1]
+ x2, y2 = z0[0] + scale * bz[2][0], z0[1] + scale * bz[2][1]
+ x3, y3 = z0[0] + scale * bz[3][0], z0[1] + scale * bz[3][1]
+ print 'gsave 0.5 setlinewidth', x0, y0, 'moveto'
+ print x1, y1, 'lineto stroke'
+ print x2, y2, 'moveto'
+ print x3, y3, 'lineto stroke grestore'
+ print 'gsave', x1, y1, 'translate 0.75 dup scale circle fill grestore'
+ print 'gsave', x2, y2, 'translate 0.75 dup scale circle fill grestore'
+ print 'gsave', x3, y3, 'translate 0.75 dup scale circle fill grestore'
+
+
+
+def measure_bz_cloth(seg, bz, n = 100):
+ bz_arclen = tocubic.bz_arclength_rk4(bz)
+ arclen_ratio = seg.arclen / bz_arclen
+ dbz = tocubic.bz_deriv(bz)
+
+ def measure_derivs(x, ys):
+ dx, dy = tocubic.bz_eval(dbz, x)
+ ds = hypot(dx, dy)
+ s = ys[0] * arclen_ratio
+ dscore = ds * (tocubic.mod_2pi(atan2(dy, dx) - seg.th(s)) ** 2)
+ #print s, atan2(dy, dx), seg.th(s)
+ return [ds, dscore]
+ dt = 1./n
+ t = 0
+ ys = [0, 0]
+ for i in range(n):
+ dydx = measure_derivs(t, ys)
+ tocubic.rk4(ys, dydx, t, dt, measure_derivs)
+ t += dt
+ return ys[1]
+
+def cubic_bz_to_pcorn(bz, thresh):
+ dx = bz[3][0] - bz[0][0]
+ dy = bz[3][1] - bz[0][1]
+ dx1 = bz[1][0] - bz[0][0]
+ dy1 = bz[1][1] - bz[0][1]
+ dx2 = bz[3][0] - bz[2][0]
+ dy2 = bz[3][1] - bz[2][1]
+ chth = atan2(dy, dx)
+ th0 = tocubic.mod_2pi(chth - atan2(dy1, dx1))
+ th1 = tocubic.mod_2pi(atan2(dy2, dx2) - chth)
+ seg = pcorn.Segment(bz[0], bz[3], th0, th1)
+ err = measure_bz_cloth(seg, bz)
+ if err < thresh:
+ return [seg]
+ else:
+ # de Casteljau
+ x01, y01 = 0.5 * (bz[0][0] + bz[1][0]), 0.5 * (bz[0][1] + bz[1][1])
+ x12, y12 = 0.5 * (bz[1][0] + bz[2][0]), 0.5 * (bz[1][1] + bz[2][1])
+ x23, y23 = 0.5 * (bz[2][0] + bz[3][0]), 0.5 * (bz[2][1] + bz[3][1])
+ xl2, yl2 = 0.5 * (x01 + x12), 0.5 * (y01 + y12)
+ xr1, yr1 = 0.5 * (x12 + x23), 0.5 * (y12 + y23)
+ xm, ym = 0.5 * (xl2 + xr1), 0.5 * (yl2 + yr1)
+ bzl = [bz[0], (x01, y01), (xl2, yl2), (xm, ym)]
+ bzr = [(xm, ym), (xr1, yr1), (x23, y23), bz[3]]
+ segs = cubic_bz_to_pcorn(bzl, 0.5 * thresh)
+ segs.extend(cubic_bz_to_pcorn(bzr, 0.5 * thresh))
+ return segs
+
+def bzs_to_pcorn(bzs, thresh = 1e-9):
+ result = []
+ for sp in bzs:
+ rsp = []
+ for bz in sp:
+ if len(bz) == 2:
+ dx = bz[1][0] - bz[0][0]
+ dy = bz[1][1] - bz[0][1]
+ th = atan2(dy, dx)
+ rsp.append(pcorn.Segment(bz[0], bz[1], 0, 0))
+ else:
+ rsp.extend(cubic_bz_to_pcorn(bz, thresh))
+ result.append(rsp)
+ return result
+
+def plot_segs(segs):
+ for i in range(len(segs)):
+ seg = segs[i]
+ if i == 0:
+ print seg.z0[0], seg.z0[1], 'moveto'
+ print seg.z1[0], seg.z1[1], 'lineto'
+ print 'stroke'
+ for i in range(len(segs)):
+ seg = segs[i]
+ if i == 0:
+ print 'gsave', seg.z0[0], seg.z0[1], 'translate circle fill grestore'
+ print 'gsave', seg.z1[0], seg.z1[1], 'translate circle fill grestore'
+
+import sys
+
+def test_to_pcorn():
+ C1 = 0.55228
+ bz = [(100, 100), (100 + 400 * C1, 100), (500, 500 - 400 * C1), (500, 500)]
+ for i in range(0, 13):
+ thresh = .1 ** i
+ segs = cubic_bz_to_pcorn(bz, thresh)
+ plot_segs(segs)
+ print >> sys.stderr, thresh, len(segs)
+ print '0 20 translate'
+
+if __name__ == '__main__':
+ f = file(sys.argv[1])
+ bzs = read_bz(f)
+ rsps = bzs_to_pcorn(bzs, 1)
+ #print rsps
+ tocubic.plot_prolog()
+ print 'grestore'
+ print '1 -1 scale 0 -720 translate'
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ tot = 0
+ for segs in rsps:
+ curve = pcorn.Curve(segs)
+ #curve = offset.offset(curve, 10)
+ print '%', curve.arclen
+ print '%', curve.sstarts
+ if 0:
+ print 'gsave 1 0 0 setrgbcolor'
+ cmd = 'moveto'
+ for i in range(100):
+ s = i * .01 * curve.arclen
+ x, y = curve.xy(s)
+ th = curve.th(s)
+ sth = 5 * sin(th)
+ cth = 5 * cos(th)
+ print x, y, cmd
+ cmd = 'lineto'
+ print 'closepath stroke grestore'
+ for i in range(100):
+ s = i * .01 * curve.arclen
+ x, y = curve.xy(s)
+ th = curve.th(s)
+ sth = 5 * sin(th)
+ cth = 5 * cos(th)
+ if 0:
+ print x - cth, y - sth, 'moveto'
+ print x + cth, y + sth, 'lineto stroke'
+ if 1:
+ for s in curve.find_breaks():
+ print 'gsave 0 1 0 setrgbcolor'
+ x, y = curve.xy(s)
+ print x, y, 'translate 2 dup scale circle fill'
+ print 'grestore'
+ #plot_segs(segs)
+
+ print 'gsave 0 0 0 setrgbcolor'
+ optim = 3
+ thresh = 1e-2
+ new_bzs = tocubic.pcorn_curve_to_bzs(curve, optim, thresh)
+ tot += len(new_bzs)
+ plot_bzs([new_bzs], (0, 0), 1, True)
+ print 'grestore'
+ print 'grestore'
+ print '/Helvetica 12 selectfont'
+ print '36 720 moveto (thresh=%g optim=%d) show' % (thresh, optim)
+ print '( tot segs=%d) show' % tot
+ print 'showpage'
+
+ #plot_bzs(bzs, (100, 100), 1)
diff --git a/third_party/spiro/curves/mecsolve.py b/third_party/spiro/curves/mecsolve.py
new file mode 100644
index 0000000..3840cea
--- /dev/null
+++ b/third_party/spiro/curves/mecsolve.py
@@ -0,0 +1,859 @@
+from math import *
+import array
+import sys
+import random
+
+import numarray as N
+import numarray.linear_algebra as la
+
+def eps_prologue(x0, y0, x1, y1, draw_box = False):
+ print '%!PS-Adobe-3.0 EPSF'
+ print '%%BoundingBox:', x0, y0, x1, y1
+ print '%%EndComments'
+ print '%%EndProlog'
+ print '%%Page: 1 1'
+ if draw_box:
+ print x0, y0, 'moveto', x0, y1, 'lineto', x1, y1, 'lineto', x1, y0, 'lineto closepath stroke'
+
+def eps_trailer():
+ print '%%EOF'
+
+# One step of 4th-order Runge-Kutta numerical integration - update y in place
+def rk4(y, dydx, x, h, derivs):
+ hh = h * .5
+ h6 = h * (1./6)
+ xh = x + hh
+ yt = []
+ for i in range(len(y)):
+ yt.append(y[i] + hh * dydx[i])
+ dyt = derivs(xh, yt)
+ for i in range(len(y)):
+ yt[i] = y[i] + hh * dyt[i]
+ dym = derivs(xh, yt)
+ for i in range(len(y)):
+ yt[i] = y[i] + h * dym[i]
+ dym[i] += dyt[i]
+ dyt = derivs(x + h, yt)
+ for i in range(len(y)):
+ y[i] += h6 * (dydx[i] + dyt[i] + 2 * dym[i])
+
+def run_elastica_half(sp, k0, lam1, lam2, n):
+ def mec_derivs(x, ys):
+ th, k = ys[2], ys[3]
+ dx, dy = cos(th), sin(th)
+ return [dx, dy, k, lam1 * dx + lam2 * dy, k * k]
+ ys = [0, 0, 0, k0, 0]
+ xyk = [(ys[0], ys[1], ys[3])]
+ n = max(1, int(sp * n))
+ h = float(sp) / n
+ s = 0
+ for i in range(n):
+ dydx = mec_derivs(s, ys)
+ rk4(ys, dydx, s, h, mec_derivs)
+ xyk.append((ys[0], ys[1], ys[3]))
+ return xyk, ys[2], ys[4]
+
+def run_elastica(sm, sp, k0, lam1, lam2, n = 100):
+ xykm, thm, costm = run_elastica_half(-sm, k0, -lam1, lam2, n)
+ xykp, thp, costp = run_elastica_half(sp, k0, lam1, lam2, n)
+ xyk = []
+ for i in range(1, len(xykm)):
+ x, y, k = xykm[i]
+ xyk.append((-x, y, k))
+ xyk.reverse()
+ xyk.extend(xykp)
+ x = xyk[-1][0] - xyk[0][0]
+ y = xyk[-1][1] - xyk[0][1]
+ th = thm + thp
+ sth, cth = sin(thm), cos(thm)
+ x, y = x * cth - y * sth, x * sth + y * cth
+ result = []
+ maxk = 0
+ for xt, yt, kt in xyk:
+ maxk = max(maxk, kt)
+ result.append((xt, yt, kt))
+ cost = costm + costp
+ return result, cost, x, y, th
+
+def run_mec_cos_pos(k, lam1, lam2, n = 1000):
+ cost = 0
+ th = 0
+ x = 0
+ y = 0
+ dt = 1.0 / n
+ result = [(0, 0)]
+ for i in range(n):
+ k1 = lam1 * cos(th) + lam2 * sin(th)
+
+ cost += dt * k * k
+ x += dt * cos(th)
+ y += dt * sin(th)
+ th += dt * k
+
+ k += dt * k1
+ result.append((x, y))
+ return result, cost, x, y, th
+
+def run_mec_cos(k, lam1, lam2, n = 1000):
+ resp, costp, xp, yp, thp = run_mec_cos_pos(k*.5, lam1*.25, lam2*.25, n)
+ resm, costm, xm, ym, thm = run_mec_cos_pos(k*.5, lam1*-.25, lam2*.25, n)
+ cost = (costp + costm) * .5
+ x, y = xp + xm, yp - ym
+ th = thp + thm
+ sth, cth = .5 * sin(thm), .5 * cos(thm)
+ x, y = x * cth - y * sth, x * sth + y * cth
+ result = []
+ for i in range(1, n):
+ xt, yt = resm[n - i - 1]
+ result.append((-xt * cth - yt * sth, -xt * sth + yt * cth))
+ for i in range(n):
+ xt, yt = resp[i]
+ result.append((xt * cth - yt * sth, xt * sth + yt * cth))
+ return result, cost, x, y, th
+
+def cross_prod(a, b):
+ return [a[1] * b[2] - a[2] * b[1],
+ a[2] * b[0] - a[0] * b[2],
+ a[0] * b[1] - a[1] * b[0]]
+
+def solve_mec(constraint_fnl):
+ delta = 1e-3
+ params = [pi, 0, 0]
+ for i in range(20):
+ k, lam1, lam2 = params
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ #print i * .05, 'setgray'
+ #plot(xys)
+ c1c, c2c, costc = constraint_fnl(cost, x, y, th)
+ print '% constraint_fnl =', c1c, c2c, 'cost =', costc
+
+ dc1s = []
+ dc2s = []
+ for j in range(len(params)):
+ params1 = N.array(params)
+ params1[j] += delta
+ k, lam1, lam2 = params1
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1p, c2p, costp = constraint_fnl(cost, x, y, th)
+ params1 = N.array(params)
+ params1[j] -= delta
+ k, lam1, lam2 = params1
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1m, c2m, costm = constraint_fnl(cost, x, y, th)
+ dc1s.append((c1p - c1m) / (2 * delta))
+ dc2s.append((c2p - c2m) / (2 * delta))
+ xp = cross_prod(dc1s, dc2s)
+ xp = N.divide(xp, sqrt(N.dot(xp, xp))) # Normalize to unit length
+
+ print '% dc1s =', dc1s
+ print '% dc2s =', dc2s
+ print '% xp =', xp
+
+ # Compute second derivative wrt orthogonal vec
+ params1 = N.array(params)
+ for j in range(len(params)):
+ params1[j] += delta * xp[j]
+ k, lam1, lam2 = params1
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1p, c2p, costp = constraint_fnl(cost, x, y, th)
+ print '% constraint_fnl+ =', c1p, c2p, 'cost =', costp
+ params1 = N.array(params)
+ for j in range(len(params)):
+ params1[j] -= delta * xp[j]
+ k, lam1, lam2 = params1
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1m, c2m, costm = constraint_fnl(cost, x, y, th)
+ print '% constraint_fnl- =', c1m, c2m, 'cost =', costm
+ d2cost = (costp + costm - 2 * costc) / (delta * delta)
+ dcost = (costp - costm) / (2 * delta)
+
+ print '% dcost =', dcost, 'd2cost =', d2cost
+ if d2cost < 0: d2cost = .1
+ # Make Jacobian matrix to invert
+ jm = N.array([dc1s, dc2s, [x * d2cost for x in xp]])
+ #print jm
+ ji = la.inverse(jm)
+ #print ji
+
+ dp = N.dot(ji, [c1c, c2c, dcost])
+ print '% dp =', dp
+ print '% [right]=', [c1c, c2c, dcost]
+ params -= dp * .1
+ print '%', params
+ sys.stdout.flush()
+ return params
+
+def solve_mec_3constr(constraint_fnl, n = 30, initparams = None):
+ delta = 1e-3
+ if initparams:
+ params = N.array(initparams)
+ else:
+ params = [3.14, 0, 0]
+ for i in range(n):
+ k, lam1, lam2 = params
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1c, c2c, c3c = constraint_fnl(cost, x, y, th)
+ print '% constraint_fnl =', c1c, c2c, c3c
+
+ dc1s = []
+ dc2s = []
+ dc3s = []
+ for j in range(len(params)):
+ params1 = N.array(params)
+ params1[j] += delta
+ k, lam1, lam2 = params1
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1p, c2p, c3p = constraint_fnl(cost, x, y, th)
+ params1 = N.array(params)
+ params1[j] -= delta
+ k, lam1, lam2 = params1
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ c1m, c2m, c3m = constraint_fnl(cost, x, y, th)
+ dc1s.append((c1p - c1m) / (2 * delta))
+ dc2s.append((c2p - c2m) / (2 * delta))
+ dc3s.append((c3p - c3m) / (2 * delta))
+
+ # Make Jacobian matrix to invert
+ jm = N.array([dc1s, dc2s, dc3s])
+ #print jm
+ ji = la.inverse(jm)
+
+ dp = N.dot(ji, [c1c, c2c, c3c])
+ if i < n/2: scale = .25
+ else: scale = 1
+ params -= scale * dp
+ print '%', params
+ return params
+
+def mk_ths_fnl(th0, th1):
+ def fnl(cost, x, y, th):
+ actual_th0 = atan2(y, x)
+ actual_th1 = th - actual_th0
+ print '% x =', x, 'y =', y, 'hypot =', hypot(x, y)
+ return th0 - actual_th0, th1 - actual_th1, cost
+ return fnl
+
+def mk_y_fnl(th0, th1, ytarg):
+ def fnl(cost, x, y, th):
+ actual_th0 = atan2(y, x)
+ actual_th1 = th - actual_th0
+ return th0 - actual_th0, th1 - actual_th1, ytarg - hypot(x, y)
+ return fnl
+
+def solve_mec_nested_inner(th0, th1, y, fnl):
+ innerfnl = mk_y_fnl(th0, th1, y)
+ params = [th0 + th1, 1e-6, 1e-6]
+ params = solve_mec_3constr(innerfnl, 10, params)
+ k, lam1, lam2 = params
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2, 100)
+ c1, c2, c3 = fnl(cost, x, y, th)
+ return c3, params
+
+# Solve (th0, th1) mec to minimize fnl; use solve_mec_3constr as inner loop
+# Use golden section search
+def solve_mec_nested(th0, th1, fnl):
+ R = .61803399
+ C = 1 - R
+ ax, cx = .6, .85
+ bx = .5 * (ax + cx)
+
+ x0, x3 = ax, cx
+ if abs(cx - bx) > abs(bx - ax):
+ x1, x2 = bx, bx + C * (cx - bx)
+ else:
+ x1, x2 = bx - C * (bx - ax), bx
+ f1, p = solve_mec_nested_inner(th0, th1, x1, fnl)
+ f2, p = solve_mec_nested_inner(th0, th1, x2, fnl)
+ for i in range(10):
+ print '%', x0, x1, x2, x3, ':', f1, f2
+ if f2 < f1:
+ x0, x1, x2 = x1, x2, R * x2 + C * x3
+ f1 = f2
+ f2, p = solve_mec_nested_inner(th0, th1, x2, fnl)
+ else:
+ x1, x2, x3 = R * x1 + C * x0, x1, x2
+ f2 = f1
+ f1, p = solve_mec_nested_inner(th0, th1, x1, fnl)
+ if f1 < f2:
+ x = x1
+ else:
+ x = x2
+ cc, p = solve_mec_nested_inner(th0, th1, x, fnl)
+ return p
+
+def plot(xys):
+ cmd = 'moveto'
+ for xy in xys:
+ print 306 + 200 * xy[0], 396 - 200 * xy[1], cmd
+ cmd = 'lineto'
+ print 'stroke'
+
+def mec_test():
+ th0, th1 = 0, pi / 4
+ fnl = mk_ths_fnl(th0, th1)
+ params = solve_mec_nested(th0, th1, fnl)
+ k, lam1, lam2 = params
+ xys, cost, x, y, th = run_mec_cos(k, lam1, lam2, 1000)
+ plot(xys)
+ print '% run_mec_cos:', cost, x, y, th
+ print '1 0 0 setrgbcolor'
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
+ print '%', xys
+ plot(xys)
+ print '% run_elastica:', cost, x, y, th
+ print 'showpage'
+ print '%', params
+
+def lenfig():
+ print '306 720 translate'
+ th0, th1 = pi / 2, pi / 2
+ for i in range(1, 10):
+ y = .1 * i + .003
+ fnl = mk_y_fnl(th0, th1, y)
+ params = solve_mec_3constr(fnl)
+ k, lam1, lam2 = params
+ print 'gsave 0.5 dup scale -306 -396 translate'
+ xys, cost, x, y, th = run_elastica(-2, 2, k, lam1, lam2, 100)
+ print 'gsave [2] 0 setdash'
+ plot(xys)
+ print 'grestore'
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2, 100)
+ plot(xys)
+ print 'grestore'
+ print '% y =', y, 'params =', params
+ if lam2 < 0:
+ mymaxk = k
+ else:
+ mymaxk = sqrt(k * k + 4 * lam2)
+ lam = abs(lam2) / (mymaxk * mymaxk)
+ print '-200 0 moveto /Symbol 12 selectfont (l) show'
+ print '/Times-Roman 12 selectfont ( = %.4g) show' % lam
+ print '0 -70 translate'
+ print 'showpage'
+
+def lenplot(figno, th0, th1):
+ result = []
+ if figno in (1, 2):
+ imax = 181
+ elif figno == 3:
+ imax = 191
+ for i in range(1, imax):
+ yy = .005 * i
+ if figno in (1, 2) and i == 127:
+ yy = 2 / pi
+ if figno == 3 and i == 165:
+ yy = .8270
+ fnl = mk_y_fnl(th0, th1, yy)
+ params = solve_mec_3constr(fnl)
+ k, lam1, lam2 = params
+ xys, cost, x0, y0, th = run_elastica(-.5, .5, k, lam1, lam2, 100)
+ if lam2 < 0:
+ mymaxk = k
+ else:
+ mymaxk = sqrt(k * k + 4 * lam2)
+ lam = abs(lam2) / (mymaxk * mymaxk)
+ result.append((yy, lam, cost))
+ return result
+
+def lengraph(figno):
+ if figno in (1, 2):
+ eps_prologue(15, 47, 585, 543)
+ th0, th1 = pi / 2, pi / 2
+ yscale = 900
+ ytic = .05
+ ymax = 10
+ elif figno == 3:
+ eps_prologue(15, 47, 585, 592)
+ th0, th1 = pi / 3, pi / 3
+ yscale = 500
+ ytic = .1
+ ymax = 10
+ x0 = 42
+ xscale = 7.5 * 72
+ y0 = 72
+ print '/Times-Roman 12 selectfont'
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ print '.5 setlinewidth'
+ print x0, y0, 'moveto', xscale, '0 rlineto 0', yscale * ytic * ymax, 'rlineto', -xscale, '0 rlineto closepath stroke'
+ for i in range(0, 11):
+ print x0 + .1 * i * xscale, y0, 'moveto 0 6 rlineto stroke'
+ print x0 + .1 * i * xscale, y0 + ytic * ymax * yscale, 'moveto 0 -6 rlineto stroke'
+ print x0 + .1 * i * xscale, y0 - 12, 'moveto'
+ print '(%.1g) dup stringwidth exch -.5 mul exch rmoveto show' % (.1 * i)
+ for i in range(0, 11):
+ print x0, y0 + ytic * i * yscale, 'moveto 6 0 rlineto stroke'
+ print x0 + xscale, y0 + ytic * i * yscale, 'moveto -6 0 rlineto stroke'
+ print x0 - 3, y0 + ytic * i * yscale - 3.5, 'moveto'
+ print '(%.2g) dup stringwidth exch neg exch rmoveto show' % (ytic * i)
+ print x0 + .25 * xscale, y0 - 24, 'moveto (chordlen / arclen) show'
+ print x0 - 14, y0 + ytic * ymax * yscale + 10, 'moveto /Symbol 12 selectfont (l) show'
+ if figno in (1, 2):
+ print x0 + 2 / pi * xscale, y0 - 18, 'moveto'
+ print '(2/p) dup stringwidth exch -.5 mul exch rmoveto show'
+ print 'gsave [3] 0 setdash'
+ print x0, y0 + .25 * yscale, 'moveto', xscale, '0 rlineto stroke'
+ if figno == 3:
+ print x0, y0 + .5 * yscale, 'moveto', xscale, '0 rlineto stroke'
+ xinterest = .81153
+ print x0 + xinterest * xscale, y0, 'moveto 0', yscale * .5, 'rlineto stroke'
+ print 'grestore'
+ print '.75 setlinewidth'
+ if 1:
+ if figno in (1, 2):
+ costscale = .01 * yscale
+ elif figno == 3:
+ costscale = .0187 * yscale
+ lenresult = lenplot(figno, th0, th1)
+ cmd = 'moveto'
+ for x, y, cost in lenresult:
+ print x0 + xscale * x, y0 + yscale * y, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ if figno in (2, 3):
+ cmd = 'moveto'
+ for x, y, cost in lenresult:
+ print x0 + xscale * x, y0 + costscale * cost, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ cmd = 'moveto'
+ for x, y, cost in lenresult:
+ print x0 + xscale * x, y0 + costscale * x * cost, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ print '/Times-Roman 12 selectfont'
+ if figno == 2:
+ xlm, ylm = .75, 7
+ xls, yls = .42, 15
+ elif figno == 3:
+ xlm, ylm = .6, 3
+ xls, yls = .37, 15
+ print x0 + xscale * xlm, y0 + costscale * ylm, 'moveto'
+ print '(MEC cost) show'
+ print x0 + xscale * xls, y0 + costscale * yls, 'moveto'
+ print '(SIMEC cost) show'
+ if figno == 1:
+ minis = [(.05, 5, -5),
+ (.26, -40, 10),
+ (.4569466, -5, -10),
+ (.55, 35, 12),
+
+ (.6026, -60, 45),
+ (.6046, -60, 15),
+ (.6066, -60, -15),
+
+ (.6213, -22, 10),
+ (.6366198, 15, 22),
+ (.66, 20, 10),
+ (.9, 0, -10)]
+ elif figno == 2:
+ minis = [(.4569466, -5, -10),
+ (.6366198, 15, 22)]
+ elif figno == 3:
+ minis = [(.741, 55, -10),
+ (.81153, -30, 20),
+ (.8270, 20, 30)]
+
+ for yy, xo, yo in minis:
+ fnl = mk_y_fnl(th0, th1, yy)
+ params = solve_mec_3constr(fnl)
+ k, lam1, lam2 = params
+ if lam2 < 0:
+ mymaxk = k
+ else:
+ mymaxk = sqrt(k * k + 4 * lam2)
+ lam = abs(lam2) / (mymaxk * mymaxk)
+ x = x0 + xscale * yy
+ y = y0 + yscale * lam
+ print 'gsave %g %g translate circle fill' % (x, y)
+ print '%g %g translate 0.15 dup scale' % (xo, yo)
+ print '-306 -396 translate'
+ print '2 setlinewidth'
+ if yy < .6 or yy > .61:
+ s = 2
+ elif yy == .6046:
+ s = 2.8
+ else:
+ s = 5
+ xys, cost, x, y, th = run_elastica(-s, s, k, lam1, lam2, 100)
+ print 'gsave [10] 0 setdash'
+ plot(xys)
+ print 'grestore 6 setlinewidth'
+ xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2, 100)
+ plot(xys)
+ print 'grestore'
+
+ print 'showpage'
+ eps_trailer()
+
+def draw_axes(x0, y0, xscale, yscale, xmax, ymax, nx, ny):
+ print '.5 setlinewidth'
+ print '/Times-Roman 12 selectfont'
+ print x0, y0, 'moveto', xscale * xmax, '0 rlineto 0', yscale * ymax, 'rlineto', -xscale * xmax, '0 rlineto closepath stroke'
+ xinc = (xmax * xscale * 1.) / nx
+ yinc = (ymax * yscale * 1.) / ny
+ for i in range(0, nx + 1):
+ if i > 0 and i < nx + 1:
+ print x0 + xinc * i, y0, 'moveto 0 6 rlineto stroke'
+ print x0 + xinc * i, y0 + ymax * yscale, 'moveto 0 -6 rlineto stroke'
+ print x0 + xinc * i, y0 - 12, 'moveto'
+ print '(%.2g) dup stringwidth exch -.5 mul exch rmoveto show' % ((i * xmax * 1.) / nx)
+ for i in range(0, ny + 1):
+ if i > 0 and i < ny + 1:
+ print x0, y0 + yinc * i, 'moveto 6 0 rlineto stroke'
+ print x0 + xmax * xscale, y0 + yinc * i, 'moveto -6 0 rlineto stroke'
+ print x0 - 3, y0 + yinc * i - 3.5, 'moveto'
+ print '(%.2g) dup stringwidth exch neg exch rmoveto show' % ((i * ymax * 1.) / ny)
+
+
+def mecchord():
+ x0 = 72
+ y0 = 72
+ thscale = 150
+ chscale = 400
+ print '.5 setlinewidth'
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ draw_axes(x0, y0, thscale, chscale, 3.2, 1, 16, 10)
+ print x0 + 1 * thscale, y0 - 28, 'moveto (angle) show'
+ print 'gsave', x0 - 32, y0 + .25 * chscale, 'translate'
+ print '90 rotate 0 0 moveto (chordlen / arclen) show'
+ print 'grestore'
+
+ cmd = 'moveto'
+ for i in range(0, 67):
+ s = i * .01
+ xys, cost, x, y, th = run_elastica(-s, s, 4, 0, -8, 100)
+ #plot(xys)
+ if s > 0:
+ ch = hypot(x, y) / (2 * s)
+ else:
+ ch = 1
+ print '%', s, x, y, th, ch
+ print x0 + thscale * th / 2, y0 + ch * chscale, cmd
+ cmd = 'lineto'
+ print 'stroke'
+
+ print 'gsave %g %g translate circle fill' % (x0 + thscale * th / 2, y0 + ch * chscale)
+ print '-30 15 translate 0.25 dup scale'
+ print '-306 -396 translate'
+ print '2 setlinewidth'
+ plot(xys)
+ print 'grestore'
+
+ print 'gsave [2] 0 setdash'
+ cmd = 'moveto'
+ for i in range(0, 151):
+ th = pi * i / 150.
+ if th > 0:
+ ch = sin(th) / th
+ else:
+ ch = 1
+ print x0 + thscale * th, y0 + ch * chscale, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ print 'grestore'
+
+ k0 = 4 * .4536 / (2 / pi)
+ s = pi / (2 * k0)
+ xys, cost, x, y, th = run_elastica(-s, s, k0, 0, 0, 100)
+ th = pi
+ ch = 2 / pi
+ print 'gsave %g %g translate circle fill' % (x0 + thscale * th / 2, y0 + ch * chscale)
+ print '30 15 translate 0.25 dup scale'
+ print '-306 -396 translate'
+ print '2 setlinewidth'
+ plot(xys)
+ print 'grestore'
+
+ print x0 + 1.25 * thscale, y0 + .55 * chscale, 'moveto (MEC) show'
+ print x0 + 2.3 * thscale, y0 + .35 * chscale, 'moveto (SIMEC) show'
+
+ print 'showpage'
+
+def trymec(sm, sp):
+ xys, thm, cost = run_elastica_half(abs(sm), 0, 1, 0, 10)
+ xm, ym, km = xys[-1]
+ if sm < 0:
+ xm = -xm
+ ym = -ym
+ thm = -thm
+ xys, thp, cost = run_elastica_half(abs(sp), 0, 1, 0, 10)
+ xp, yp, kp = xys[-1]
+ if sp < 0:
+ xp = -xp
+ yp = -yp
+ thp = -thp
+ chth = atan2(yp - ym, xp - xm)
+ #print xm, ym, xp, yp, chth, thm, thp
+ actual_th0 = chth - thm
+ actual_th1 = thp - chth
+ return actual_th0, actual_th1
+
+def findmec_old(th0, th1):
+ guess_ds = sqrt(6 * (th1 - th0))
+ guess_savg = 2 * (th1 + th0) / guess_ds
+ sm = .5 * (guess_savg - guess_ds)
+ sp = .5 * (guess_savg + guess_ds)
+ #sm, sp = th0, th1
+ for i in range(1):
+ actual_th0, actual_th1 = trymec(sm, sp)
+ guess_dth = 1./6 * (sp - sm) ** 2
+ guess_avth = .5 * (sp + sm) * (sp - sm)
+ guess_th0 = .5 * (guess_avth - guess_dth)
+ guess_th1 = .5 * (guess_avth + guess_dth)
+ print sm, sp, actual_th0, actual_th1, guess_th0, guess_th1
+
+def mecplots():
+ print '2 dup scale'
+ print '-153 -296 translate'
+ scale = 45
+ print '0.25 setlinewidth'
+ print 306 - scale * pi/2, 396, 'moveto', 306 + scale * pi/2, 396, 'lineto stroke'
+ print 306, 396 - scale * pi/2, 'moveto', 306, 396 + scale * pi/2, 'lineto stroke'
+ print '/ss .5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+
+ tic = .1
+ maj = 5
+ r0, r1 = 0, 81
+
+ for i in range(r0, r1, maj):
+ sm = i * tic
+ cmd = 'moveto'
+ for j in range(i, r1):
+ sp = j * tic + 1e-3
+ th0, th1 = trymec(sm, sp)
+ print '%', sm, sp, th0, th1
+ print 306 + scale * th0, 396 + scale * th1, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ for j in range(r0, r1, maj):
+ sp = j * tic + 1e-3
+ cmd = 'moveto'
+ for i in range(0, j + 1):
+ sm = i * tic
+ th0, th1 = trymec(sm, sp)
+ print '%', sm, sp, th0, th1
+ print 306 + scale * th0, 396 + scale * th1, cmd
+ cmd = 'lineto'
+ print 'stroke'
+
+ for i in range(r0, r1, maj):
+ sm = i * tic
+ for j in range(i, r1, maj):
+ sp = j * tic + 1e-3
+ th0, th1 = trymec(sm, sp)
+ print 'gsave'
+ print 306 + scale * th0, 596 + scale * th1, 'translate'
+ print 'circle fill'
+ uscale = 3
+ xys, thm, cost = run_elastica_half(abs(sm), 0, 1, 0, 100)
+ x0, y0 = xys[-1][0], xys[-1][1]
+ if sm < 0:
+ x0, y0 = -x0, -y0
+ xys, thm, cost = run_elastica_half(abs(sp), 0, 1, 0, 100)
+ x1, y1 = xys[-1][0], xys[-1][1]
+ if sp < 0:
+ x1, y1 = -x1, -y1
+ cmd = 'moveto'
+ for xy in xys:
+ print xy[0] * uscale, xy[1] * uscale, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ print '1 0 0 setrgbcolor'
+ print x0 * uscale, y0 * uscale, 'moveto'
+ print x1 * uscale, y1 * uscale, 'lineto stroke'
+ print 'grestore'
+
+ print 'showpage'
+
+def findmec(th0, th1):
+ delta = 1e-3
+ if th0 < 0 and th0 - th1 < .1:
+ sm = 5.
+ sp = 6.
+ else:
+ sm = 3.
+ sp = 5.
+ params = [sm, sp]
+ lasterr = 1e6
+ for i in range(25):
+ sm, sp = params
+ ath0, ath1 = trymec(sm, sp)
+ c1c, c2c = th0 - ath0, th1 - ath1
+
+ err = c1c * c1c + c2c * c2c
+ if 0:
+ print '%findmec', sm, sp, ath0, ath1, err
+ sys.stdout.flush()
+
+ if err < 1e-9:
+ return params
+ if err > lasterr:
+ return None
+ lasterr = err
+
+
+ dc1s = []
+ dc2s = []
+ for j in range(len(params)):
+ params1 = N.array(params)
+ params1[j] += delta
+ sm, sp = params1
+ ath0, ath1 = trymec(sm, sp)
+ c1p, c2p = th0 - ath0, th1 - ath1
+
+ params1 = N.array(params)
+ params1[j] -= delta
+ sm, sp = params1
+ ath0, ath1 = trymec(sm, sp)
+ c1m, c2m = th0 - ath0, th1 - ath1
+
+ dc1s.append((c1p - c1m) / (2 * delta))
+ dc2s.append((c2p - c2m) / (2 * delta))
+
+ jm = N.array([dc1s, dc2s])
+ ji = la.inverse(jm)
+ dp = N.dot(ji, [c1c, c2c])
+
+ if i < 4:
+ scale = .5
+ else:
+ scale = 1
+ params -= scale * dp
+ if params[0] < 0: params[0] = 0.
+ return params
+
+def mecrange(figtype):
+ scale = 130
+ eps_prologue(50, 110, 570, 630)
+ print -50, 0, 'translate'
+ print '0.5 setlinewidth'
+ thlmin, thlmax = -pi/2, 2.4
+ thrmin, thrmax = -2.2, pi / 2 + .2
+ print 306 + scale * thlmin, 396, 'moveto', 306 + scale * thlmax, 396, 'lineto stroke'
+ print 306, 396 + scale * thrmin, 'moveto', 306, 396 + scale * thrmax, 'lineto stroke'
+
+ print 'gsave [2] 0 setdash'
+ print 306, 396 + scale * pi / 2, 'moveto'
+ print 306 + scale * thlmax, 396 + scale * pi / 2, 'lineto stroke'
+ print 306 + scale * thlmin, 396 - scale * pi / 2, 'moveto'
+ print 306 + scale * thlmax, 396 - scale * pi / 2, 'lineto stroke'
+ print 306 + scale * pi / 2, 396 + scale * thrmin, 'moveto'
+ print 306 + scale * pi / 2, 396 + scale * thrmax, 'lineto stroke'
+ print 'grestore'
+
+ print 306 + 3, 396 + scale * thrmax - 10, 'moveto'
+ print '/Symbol 12 selectfont (q) show'
+ print 0, -2, 'rmoveto'
+ print '/Times-Italic 9 selectfont (right) show'
+
+ print 306 - 18, 396 + scale * pi / 2 - 4, 'moveto'
+ print '/Symbol 12 selectfont (p/2) show'
+ print 306 + scale * 2.2, 396 - scale * pi / 2 + 2, 'moveto'
+ print '/Symbol 12 selectfont (-p/2) show'
+
+ print 306 + scale * pi/2 + 2, 396 + scale * thrmax - 10, 'moveto'
+ print '/Symbol 12 selectfont (p/2) show'
+
+ print 306 + scale * 2.2, 396 + 6, 'moveto'
+ print '/Symbol 12 selectfont (q) show'
+ print 0, -2, 'rmoveto'
+ print '/Times-Italic 9 selectfont (left) show'
+
+ print '/ss 0.8 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ cmd = 'moveto'
+ for i in range(0, 201):
+ th = (i * .005 - .75 )* pi
+ rmin = 1.5
+ rmax = 2.5
+ for j in range(20):
+ r = (rmin + rmax) * .5
+ th0 = r * cos(th)
+ th1 = r * sin(th)
+ if findmec(th0, th1) == None:
+ rmax = r
+ else:
+ rmin = r
+ r = (rmin + rmax) * .5
+ th0 = r * cos(th)
+ th1 = r * sin(th)
+ print '%', r, th, th0, th1
+ print 306 + scale * th0, 396 + scale * th1, cmd
+ cmd = 'lineto'
+ sys.stdout.flush()
+ print 'stroke'
+ sys.stdout.flush()
+
+ for i in range(-11, 12):
+ for j in range(-11, i + 1):
+ th0, th1 = i * .196, j * .196
+ print '%', th0, th1
+ params = findmec(th0, th1)
+ if params != None:
+ sm, sp = params
+ print 'gsave'
+ print 306 + scale * th0, 396 + scale * th1, 'translate'
+ uscale = 22
+ k0, lam1, lam2 = justify_mec(sm, sp)
+ xys, cost, x, y, th = run_elastica(-.5, .5, k0, lam1, lam2)
+ cmdm = 'moveto'
+ dx = xys[-1][0] - xys[0][0]
+ dy = xys[-1][1] - xys[0][1]
+ ch = hypot(dx, dy)
+ chth = atan2(dy, dx)
+ if figtype == 'mecrange':
+ print 'circle fill'
+ s = uscale * sin(chth) / ch
+ c = uscale * cos(chth) / ch
+ h = -xys[0][0] * s + xys[0][1] * c
+ for xy in xys:
+ print xy[0] * c + xy[1] * s, h + xy[0] * s - xy[1] * c, cmdm
+ cmdm = 'lineto'
+ elif figtype == 'mecrangek':
+ ds = 1. / (len(xys) - 1)
+ sscale = 13. / ch
+ kscale = 3 * ch
+ print 'gsave .25 setlinewidth'
+ print sscale * -.5, 0, 'moveto', sscale, 0, 'rlineto stroke'
+ print 'grestore'
+ for l in range(len(xys)):
+ print sscale * (ds * l - 0.5), kscale * xys[l][2], cmdm
+ cmdm = 'lineto'
+ print 'stroke'
+ print 'grestore'
+ sys.stdout.flush()
+ print 'showpage'
+ eps_trailer()
+
+# given sm, sp in [0, 1, 0] mec, return params for -0.5..0.5 segment
+def justify_mec(sm, sp):
+ sc = (sm + sp) * 0.5
+ xys, thc, cost = run_elastica_half(sc, 0, 1, 0, 100)
+ print '% sc =', sc, 'thc =', thc
+ k0, lam1, lam2 = xys[-1][2], cos(thc), -sin(thc)
+ ds = sp - sm
+ k0 *= ds
+ lam1 *= ds * ds
+ lam2 *= ds * ds
+ return [k0, lam1, lam2]
+
+import sys
+figname = sys.argv[1]
+if len(figname) > 4 and figname[-4:] == '.pdf': figname = figname[:-4]
+if figname in ('lengraph1', 'lengraph2', 'lengraph3'):
+ lengraph(int(figname[-1]))
+ #mec_test()
+ #lenfig()
+elif figname == 'mecchord':
+ mecchord()
+elif figname in ('mecrange', 'mecrangek'):
+ mecrange(figname)
+elif figname == 'mecplots':
+ mecplots()
+elif figname == 'findmec':
+ findmec(-.1, -.1)
+ findmec(-.2, -.2)
+
diff --git a/third_party/spiro/curves/mvc.py b/third_party/spiro/curves/mvc.py
new file mode 100644
index 0000000..19ff927
--- /dev/null
+++ b/third_party/spiro/curves/mvc.py
@@ -0,0 +1,208 @@
+from math import *
+import array
+import sys
+import random
+
+def run_mvc(k, k1, k2, k3, C, n = 100, do_print = False):
+ cmd = 'moveto'
+ result = array.array('d')
+ cost = 0
+ th = 0
+ x = 0
+ y = 0
+ dt = 1.0 / n
+ for i in range(n):
+ k4 = -k * (k * k2 - .5 * k1 * k1 + C)
+
+ cost += dt * k1 * k1
+ x += dt * cos(th)
+ y += dt * sin(th)
+ th += dt * k
+
+ k += dt * k1
+ k1 += dt * k2
+ k2 += dt * k3
+ k3 += dt * k4
+ result.append(k)
+ if do_print: print 400 + 400 * x, 500 + 400 * y, cmd
+ cmd = 'lineto'
+ return result, cost, x, y, th
+
+def run_mec_cos(k, lam1, lam2, n = 100, do_print = False):
+ cmd = 'moveto'
+ result = array.array('d')
+ cost = 0
+ th = 0
+ x = 0
+ y = 0
+ dt = 1.0 / n
+ for i in range(n):
+ k1 = lam1 * cos(th) + lam2 * sin(th)
+
+ cost += dt * k * k
+ x += dt * cos(th)
+ y += dt * sin(th)
+ th += dt * k
+
+ k += dt * k1
+ result.append(k)
+ if do_print: print 400 + 400 * x, 500 + 400 * y, cmd
+ cmd = 'lineto'
+ return result, cost, x, y, th
+
+def descend(params, fnl):
+ delta = 1
+ for i in range(100):
+ best = fnl(params, i, True)
+ bestparams = params
+ for j in range(2 * len(params)):
+ ix = j / 2
+ sign = 1 - 2 * (ix & 1)
+ newparams = params[:]
+ newparams[ix] += delta * sign
+ new = fnl(newparams, i)
+ if (new < best):
+ bestparams = newparams
+ best = new
+ if (bestparams == params):
+ delta *= .5
+ print '%', params, delta
+ sys.stdout.flush()
+ params = bestparams
+ return bestparams
+
+def descend2(params, fnl):
+ delta = 20
+ for i in range(5):
+ best = fnl(params, i, True)
+ bestparams = params
+ for j in range(100000):
+ newparams = params[:]
+ for ix in range(len(params)):
+ newparams[ix] += delta * (2 * random.random() - 1)
+ new = fnl(newparams, i)
+ if (new < best):
+ bestparams = newparams
+ best = new
+ if (bestparams == params):
+ delta *= .5
+ params = bestparams
+ print '%', params, best, delta
+ sys.stdout.flush()
+ return bestparams
+
+def desc_eval(params, dfdp, fnl, i, x):
+ newparams = params[:]
+ for ix in range(len(params)):
+ newparams[ix] += x * dfdp[ix]
+ return fnl(newparams, i)
+
+def descend3(params, fnl):
+ dp = 1e-6
+ for i in range(1000):
+ base = fnl(params, i, True)
+ dfdp = []
+ for ix in range(len(params)):
+ newparams = params[:]
+ newparams[ix] += dp
+ new = fnl(newparams, i)
+ dfdp.append((new - base) / dp)
+ print '% dfdp = ', dfdp
+ xr = 0.
+ yr = base
+ xm = -1e-3
+ ym = desc_eval(params, dfdp, fnl, i, xm)
+ if ym > yr:
+ while ym > yr:
+ xl, yl = xm, ym
+ xm = .618034 * xl
+ ym = desc_eval(params, dfdp, fnl, i, xm)
+ else:
+ xl = 1.618034 * xm
+ yl = desc_eval(params, dfdp, fnl, i, xl)
+ while ym > yl:
+ xm, ym = xl, yl
+ xl = 1.618034 * xm
+ yl = desc_eval(params, dfdp, fnl, i, xl)
+
+ # We have initial bracket; ym < yl and ym < yr
+
+ x0, x3 = xl, xr
+ if abs(xr - xm) > abs(xm - xl):
+ x1, y1 = xm, ym
+ x2 = xm + .381966 * (xr - xm)
+ y2 = desc_eval(params, dfdp, fnl, i, x2)
+ else:
+ x2, y2 = xm, ym
+ x1 = xm + .381966 * (xl - xm)
+ y1 = desc_eval(params, dfdp, fnl, i, x1)
+ for j in range(30):
+ if y2 < y1:
+ x0, x1, x2 = x1, x2, x2 + .381966 * (x3 - x2)
+ y0, y1 = y1, y2
+ y2 = desc_eval(params, dfdp, fnl, i, x2)
+ else:
+ x1, x2, x3 = x1 + .381966 * (x0 - x1), x1, x2
+ y1 = desc_eval(params, dfdp, fnl, i, x1)
+ if y1 < y2:
+ xbest = x1
+ ybest = y1
+ else:
+ xbest = x2
+ ybest = y2
+ for ix in range(len(params)):
+ params[ix] += xbest * dfdp[ix]
+ print '%', params, xbest, ybest
+ sys.stdout.flush()
+ return params
+
+def mk_mvc_fnl(th0, th1):
+ def fnl(params, i, do_print = False):
+ k, k1, k2, k3, C = params
+ ks, cost, x, y, th = run_mvc(k, k1, k2, k3, C, 100)
+ cost *= hypot(y, x) ** 3
+ actual_th0 = atan2(y, x)
+ actual_th1 = th - actual_th0
+ if do_print: print '%', x, y, actual_th0, actual_th1, cost
+ err = (th0 - actual_th0) ** 2 + (th1 - actual_th1) ** 2
+ multiplier = 1000
+ return cost + err * multiplier
+ return fnl
+
+def mk_mec_fnl(th0, th1):
+ def fnl(params, i, do_print = False):
+ k, lam1, lam2 = params
+ ks, cost, x, y, th = run_mec_cos(k, lam1, lam2)
+ cost *= hypot(y, x)
+ actual_th0 = atan2(y, x)
+ actual_th1 = th - actual_th0
+ if do_print: print '%', x, y, actual_th0, actual_th1, cost
+ err = (th0 - actual_th0) ** 2 + (th1 - actual_th1) ** 2
+ multiplier = 10
+ return cost + err * multiplier
+ return fnl
+
+#ks, cost, x, y, th = run_mvc(0, 10, -10, 10, 200)
+#print '%', cost, x, y
+#print 'stroke showpage'
+
+def mvc_test():
+ fnl = mk_mvc_fnl(-pi, pi/4)
+ params = [0, 0, 0, 0, 0]
+ params = descend3(params, fnl)
+ k, k1, k2, k3, C = params
+ run_mvc(k, k1, k2, k3, C, 100, True)
+ print 'stroke showpage'
+ print '%', params
+
+def mec_test():
+ th0, th1 = pi/2, pi/2
+ fnl = mk_mec_fnl(th0, th1)
+ params = [0, 0, 0]
+ params = descend2(params, fnl)
+ k, lam1, lam2 = params
+ run_mec_cos(k, lam1, lam2, 1000, True)
+ print 'stroke showpage'
+ print '%', params
+
+mvc_test()
diff --git a/third_party/spiro/curves/numintsynth.py b/third_party/spiro/curves/numintsynth.py
new file mode 100644
index 0000000..9a51b66
--- /dev/null
+++ b/third_party/spiro/curves/numintsynth.py
@@ -0,0 +1,176 @@
+# Synthesize a procedure to numerically integrate the 3rd order poly spiral
+
+tex = False
+
+if tex:
+ mulsym = ' '
+else:
+ mulsym = ' * '
+
+class Poly:
+ def __init__(self, p0, coeffs):
+ self.p0 = p0
+ self.coeffs = coeffs
+ def eval(self, x):
+ y = x ** self.p0
+ z = 0
+ for c in coeffs:
+ z += y * c
+ y *= x
+ return z
+
+def add(poly0, poly1, nmax):
+ lp0 = len(poly0.coeffs)
+ lp1 = len(poly1.coeffs)
+ p0 = min(poly0.p0, poly1.p0)
+ n = min(max(poly0.p0 + lp0, poly1.p1 + lp1), nmax) - p0
+ if n <= 0: return Poly(0, [])
+ coeffs = []
+ for i in range(n):
+ c = 0
+ if i >= poly0.p0 - p0 and i < lp0 + poly0.p0 - p0:
+ c += poly0.coeffs[i + p0 - poly0.p0]
+ if i >= poly1.p0 - p0 and i < lp1 + poly1.p0 - p0:
+ c += poly1.coeffs[i + p0 - poly1.p0]
+ coeffs.append(c)
+ return Poly(p0, coeffs)
+
+def pr(str):
+ if tex:
+ print str, '\\\\'
+ else:
+ print '\t' + str + ';'
+
+def prd(str):
+ if tex:
+ print str, '\\\\'
+ else:
+ print '\tdouble ' + str + ';'
+
+def polymul(p0, p1, degree, basename, suppress_odd = False):
+ result = []
+ for i in range(min(degree, len(p0) + len(p1) - 1)):
+ terms = []
+ for j in range(i + 1):
+ if j < len(p0) and i - j < len(p1):
+ t0 = p0[j]
+ t1 = p1[i - j]
+ if t0 != None and t1 != None:
+ terms.append(t0 + mulsym + t1)
+ if terms == []:
+ result.append(None)
+ else:
+ var = basename % i
+ if (j % 2 == 0) or not suppress_odd:
+ prd(var + ' = ' + ' + '.join(terms))
+ result.append(var)
+ return result
+
+def polysquare(p0, degree, basename):
+ result = []
+ for i in range(min(degree, 2 * len(p0) - 1)):
+ terms = []
+ for j in range((i + 1)/ 2):
+ if i - j < len(p0):
+ t0 = p0[j]
+ t1 = p0[i - j]
+ if t0 != None and t1 != None:
+ terms.append(t0 + mulsym + t1)
+ if len(terms) >= 1:
+ if tex and len(terms) == 1:
+ terms = ['2 ' + terms[0]]
+ else:
+ terms = ['2' + mulsym + '(' + ' + '.join(terms) + ')']
+ if (i % 2) == 0:
+ t = p0[i / 2]
+ if t != None:
+ if tex:
+ terms.append(t + '^2')
+ else:
+ terms.append(t + mulsym + t)
+ if terms == []:
+ result.append(None)
+ else:
+ var = basename % i
+ prd(var + ' = ' + ' + '.join(terms))
+ result.append(var)
+ return result
+
+def mkspiro(degree):
+ if tex:
+ us = ['u = 1']
+ vs = ['v =']
+ else:
+ us = ['u = 1']
+ vs = ['v = 0']
+ if tex:
+ tp = [None, 't_{11}', 't_{12}', 't_{13}', 't_{14}']
+ else:
+ tp = [None, 't1_1', 't1_2', 't1_3', 't1_4']
+ if tex:
+ prd(tp[1] + ' = k_0')
+ prd(tp[2] + ' = \\frac{k_1}{2}')
+ prd(tp[3] + ' = \\frac{k_2}{6}')
+ prd(tp[4] + ' = \\frac{k_3}{24}')
+ else:
+ prd(tp[1] + ' = km0')
+ prd(tp[2] + ' = .5 * km1')
+ prd(tp[3] + ' = (1./6) * km2')
+ prd(tp[4] + ' = (1./24) * km3')
+ tlast = tp
+ coef = 1.
+ for i in range(1, degree - 1):
+ tmp = []
+ tcoef = coef
+ #print tlast
+ for j in range(len(tlast)):
+ c = tcoef / (j + 1)
+ if (j % 2) == 0 and tlast[j] != None:
+ if tex:
+ tmp.append('\\frac{%s}{%.0f}' % (tlast[j], 1./c))
+ else:
+ if c < 1e-9:
+ cstr = '%.16e' % c
+ else:
+ cstr = '(1./%d)' % int(.5 + (1./c))
+ tmp.append(cstr + ' * ' + tlast[j])
+ tcoef *= .5
+ if tmp != []:
+ sign = ('+', '-')[(i / 2) % 2]
+ var = ('u', 'v')[i % 2]
+ if tex:
+ if i == 1: pref = ''
+ else: pref = sign + ' '
+ str = pref + (' ' + sign + ' ').join(tmp)
+ else:
+ str = var + ' ' + sign + '= ' + ' + '.join(tmp)
+ if var == 'u': us.append(str)
+ else: vs.append(str)
+ if i < degree - 1:
+ if tex:
+ basename = 't_{%d%%d}' % (i + 1)
+ else:
+ basename = 't%d_%%d' % (i + 1)
+ if i == 1:
+ tnext = polysquare(tp, degree - 1, basename)
+ t2 = tnext
+ elif i == 3:
+ tnext = polysquare(t2l, degree - 1, basename)
+ elif (i % 2) == 0:
+ tnext = polymul(tlast, tp, degree - 1, basename, True)
+ else:
+ tnext = polymul(t2l, t2, degree - 1, basename)
+ t2l = tlast
+ tlast = tnext
+ coef /= (i + 1)
+ if tex:
+ pr(' '.join(us))
+ pr(' '.join(vs))
+ else:
+ for u in us:
+ pr(u)
+ for v in vs:
+ pr(v)
+
+if __name__ == '__main__':
+ mkspiro(12)
diff --git a/third_party/spiro/curves/offset.py b/third_party/spiro/curves/offset.py
new file mode 100644
index 0000000..c528aec
--- /dev/null
+++ b/third_party/spiro/curves/offset.py
@@ -0,0 +1,21 @@
+# offset curve of piecewise cornu curves
+
+from math import *
+
+import pcorn
+from clothoid import mod_2pi
+
+def seg_offset(seg, d):
+ th0 = seg.th(0)
+ th1 = seg.th(seg.arclen)
+ z0 = [seg.z0[0] + d * sin(th0), seg.z0[1] - d * cos(th0)]
+ z1 = [seg.z1[0] + d * sin(th1), seg.z1[1] - d * cos(th1)]
+ chth = atan2(z1[1] - z0[1], z1[0] - z0[0])
+ return [pcorn.Segment(z0, z1, mod_2pi(chth - th0), mod_2pi(th1 - chth))]
+
+
+def offset(curve, d):
+ segs = []
+ for seg in curve.segs:
+ segs.extend(seg_offset(seg, d))
+ return pcorn.Curve(segs)
diff --git a/third_party/spiro/curves/pcorn.py b/third_party/spiro/curves/pcorn.py
new file mode 100644
index 0000000..723a82f
--- /dev/null
+++ b/third_party/spiro/curves/pcorn.py
@@ -0,0 +1,160 @@
+# Utilities for piecewise cornu representation of curves
+
+from math import *
+
+import clothoid
+import cornu
+
+class Segment:
+ def __init__(self, z0, z1, th0, th1):
+ self.z0 = z0
+ self.z1 = z1
+ self.th0 = th0
+ self.th1 = th1
+ self.compute()
+ def __repr__(self):
+ return '[' + `self.z0` + `self.z1` + ' ' + `self.th0` + ' ' + `self.th1` + ']'
+ def compute(self):
+ dx = self.z1[0] - self.z0[0]
+ dy = self.z1[1] - self.z0[1]
+ chord = hypot(dy, dx)
+ chth = atan2(dy, dx)
+ k0, k1 = clothoid.solve_clothoid(self.th0, self.th1)
+ charc = clothoid.compute_chord(k0, k1)
+
+ self.chord = chord
+ self.chth = chth
+ self.k0, self.k1 = k0, k1
+ self.charc = charc
+ self.arclen = chord / charc
+ self.thmid = self.chth - self.th0 + 0.5 * self.k0 - 0.125 * self.k1
+
+ self.setup_xy_fresnel()
+
+ def setup_xy_fresnel(self):
+ k0, k1 = self.k0, self.k1
+ if k1 == 0: k1 = 1e-6 # hack
+ if k1 != 0:
+ sqrk1 = sqrt(2 * abs(k1))
+ t0 = (k0 - .5 * k1) / sqrk1
+ t1 = (k0 + .5 * k1) / sqrk1
+ (y0, x0) = cornu.eval_cornu(t0)
+ (y1, x1) = cornu.eval_cornu(t1)
+ chord_th = atan2(y1 - y0, x1 - x0)
+ chord = hypot(y1 - y0, x1 - x0)
+ scale = self.chord / chord
+ if k1 >= 0:
+ th = self.chth - chord_th
+ self.mxx = scale * cos(th)
+ self.myx = scale * sin(th)
+ self.mxy = -self.myx
+ self.myy = self.mxx
+ else:
+ th = self.chth + chord_th
+ self.mxx = scale * cos(th)
+ self.myx = scale * sin(th)
+ self.mxy = self.myx
+ self.myy = -self.mxx
+ # rotate -chord_th, flip top/bottom, rotate self.chth
+ self.x0 = self.z0[0] - (self.mxx * x0 + self.mxy * y0)
+ self.y0 = self.z0[1] - (self.myx * x0 + self.myy * y0)
+
+ def th(self, s):
+ u = s / self.arclen - 0.5
+ return self.thmid + (0.5 * self.k1 * u + self.k0) * u
+
+ def xy(self, s):
+ # using fresnel integrals; polynomial approx might be better
+ u = s / self.arclen - 0.5
+ k0, k1 = self.k0, self.k1
+ if k1 == 0: k1 = 1e-6 # hack
+ if k1 != 0:
+ sqrk1 = sqrt(2 * abs(k1))
+ t = (k0 + u * k1) / sqrk1
+ (y, x) = cornu.eval_cornu(t)
+ return [self.x0 + self.mxx * x + self.mxy * y,
+ self.y0 + self.myx * x + self.myy * y]
+
+ def find_extrema(self):
+ # find solutions of th(s) = 0 mod pi/2
+ # todo: find extra solutions when there's an inflection
+ th0 = self.thmid + 0.125 * self.k1 - 0.5 * self.k0
+ th1 = self.thmid + 0.125 * self.k1 + 0.5 * self.k0
+ twooverpi = 2 / pi
+ n0 = int(floor(th0 * twooverpi))
+ n1 = int(floor(th1 * twooverpi))
+ if th1 > th0: signum = 1
+ else: signum = -1
+ result = []
+ for i in range(n0, n1, signum):
+ th = pi/2 * (i + 0.5 * (signum + 1))
+ a = .5 * self.k1
+ b = self.k0
+ c = self.thmid - th
+ if a == 0:
+ u1 = -c/b
+ u2 = 1000
+ else:
+ sqrtdiscrim = sqrt(b * b - 4 * a * c)
+ u1 = (-b - sqrtdiscrim) / (2 * a)
+ u2 = (-b + sqrtdiscrim) / (2 * a)
+ if u1 >= -0.5 and u1 < 0.5:
+ result.append(self.arclen * (u1 + 0.5))
+ if u2 >= -0.5 and u2 < 0.5:
+ result.append(self.arclen * (u2 + 0.5))
+ return result
+
+class Curve:
+ def __init__(self, segs):
+ self.segs = segs
+ self.compute()
+ def compute(self):
+ arclen = 0
+ sstarts = []
+ for seg in self.segs:
+ sstarts.append(arclen)
+ arclen += seg.arclen
+
+ self.arclen = arclen
+ self.sstarts = sstarts
+ def th(self, s, deltas = False):
+ u = s / self.arclen
+ s = self.arclen * (u - floor(u))
+ if s == 0 and not deltas: s = self.arclen
+ i = 0
+ while i < len(self.segs) - 1:
+ # binary search would make a lot of sense here
+ snext = self.sstarts[i + 1]
+ if s < snext or (not deltas and s == snext):
+ break
+ i += 1
+ return self.segs[i].th(s - self.sstarts[i])
+ def xy(self, s):
+ u = s / self.arclen
+ s = self.arclen * (u - floor(u))
+ i = 0
+ while i < len(self.segs) - 1:
+ # binary search would make a lot of sense here
+ if s <= self.sstarts[i + 1]:
+ break
+ i += 1
+ return self.segs[i].xy(s - self.sstarts[i])
+ def find_extrema(self):
+ result = []
+ for i in range(len(self.segs)):
+ seg = self.segs[i]
+ for s in seg.find_extrema():
+ result.append(s + self.sstarts[i])
+ return result
+ def find_breaks(self):
+ result = []
+ for i in range(len(self.segs)):
+ pseg = self.segs[(i + len(self.segs) - 1) % len(self.segs)]
+ seg = self.segs[i]
+ th = clothoid.mod_2pi(pseg.chth + pseg.th1 - (seg.chth - seg.th0))
+ print '% pseg', pseg.chth + pseg.th1, 'seg', seg.chth - seg.th0
+ pisline = pseg.k0 == 0 and pseg.k1 == 0
+ sisline = seg.k0 == 0 and seg.k1 == 0
+ if fabs(th) > 1e-3 or (pisline and not sisline) or (sisline and not pisline):
+ result.append(self.sstarts[i])
+ return result
diff --git a/third_party/spiro/curves/plot_solve_clothoid.py b/third_party/spiro/curves/plot_solve_clothoid.py
new file mode 100644
index 0000000..c611069
--- /dev/null
+++ b/third_party/spiro/curves/plot_solve_clothoid.py
@@ -0,0 +1,164 @@
+import clothoid
+from math import *
+
+print '%!PS-Adobe'
+
+def integ_spiro(k0, k1, k2, k3, n = 4):
+ th1 = k0
+ th2 = .5 * k1
+ th3 = (1./6) * k2
+ th4 = (1./24) * k3
+ ds = 1. / n
+ ds2 = ds * ds
+ ds3 = ds2 * ds
+ s = .5 * ds - .5
+
+ k0 *= ds
+ k1 *= ds
+ k2 *= ds
+ k3 *= ds
+
+ x = 0
+ y = 0
+
+ for i in range(n):
+ if n == 1:
+ km0 = k0
+ km1 = k1 * ds
+ km2 = k2 * ds2
+ else:
+ km0 = (((1./6) * k3 * s + .5 * k2) * s + k1) * s + k0
+ km1 = ((.5 * k3 * s + k2) * s + k1) * ds
+ km2 = (k3 * s + k2) * ds2
+ km3 = k3 * ds3
+
+ t1_1 = km0
+ t1_2 = .5 * km1
+ t1_3 = (1./6) * km2
+ t1_4 = (1./24) * km3
+ t2_2 = t1_1 * t1_1
+ t2_3 = 2 * (t1_1 * t1_2)
+ t2_4 = 2 * (t1_1 * t1_3) + t1_2 * t1_2
+ t2_5 = 2 * (t1_1 * t1_4 + t1_2 * t1_3)
+ t2_6 = 2 * (t1_2 * t1_4) + t1_3 * t1_3
+ t2_7 = 2 * (t1_3 * t1_4)
+ t2_8 = t1_4 * t1_4
+ t3_4 = t2_2 * t1_2 + t2_3 * t1_1
+ t3_6 = t2_2 * t1_4 + t2_3 * t1_3 + t2_4 * t1_2 + t2_5 * t1_1
+ t3_8 = t2_4 * t1_4 + t2_5 * t1_3 + t2_6 * t1_2 + t2_7 * t1_1
+ t3_10 = t2_6 * t1_4 + t2_7 * t1_3 + t2_8 * t1_2
+ t4_4 = t2_2 * t2_2
+ t4_5 = 2 * (t2_2 * t2_3)
+ t4_6 = 2 * (t2_2 * t2_4) + t2_3 * t2_3
+ t4_7 = 2 * (t2_2 * t2_5 + t2_3 * t2_4)
+ t4_8 = 2 * (t2_2 * t2_6 + t2_3 * t2_5) + t2_4 * t2_4
+ t4_9 = 2 * (t2_2 * t2_7 + t2_3 * t2_6 + t2_4 * t2_5)
+ t4_10 = 2 * (t2_2 * t2_8 + t2_3 * t2_7 + t2_4 * t2_6) + t2_5 * t2_5
+ t5_6 = t4_4 * t1_2 + t4_5 * t1_1
+ t5_8 = t4_4 * t1_4 + t4_5 * t1_3 + t4_6 * t1_2 + t4_7 * t1_1
+ t5_10 = t4_6 * t1_4 + t4_7 * t1_3 + t4_8 * t1_2 + t4_9 * t1_1
+ t6_6 = t4_4 * t2_2
+ t6_7 = t4_4 * t2_3 + t4_5 * t2_2
+ t6_8 = t4_4 * t2_4 + t4_5 * t2_3 + t4_6 * t2_2
+ t6_9 = t4_4 * t2_5 + t4_5 * t2_4 + t4_6 * t2_3 + t4_7 * t2_2
+ t6_10 = t4_4 * t2_6 + t4_5 * t2_5 + t4_6 * t2_4 + t4_7 * t2_3 + t4_8 * t2_2
+ t7_8 = t6_6 * t1_2 + t6_7 * t1_1
+ t7_10 = t6_6 * t1_4 + t6_7 * t1_3 + t6_8 * t1_2 + t6_9 * t1_1
+ t8_8 = t6_6 * t2_2
+ t8_9 = t6_6 * t2_3 + t6_7 * t2_2
+ t8_10 = t6_6 * t2_4 + t6_7 * t2_3 + t6_8 * t2_2
+ t9_10 = t8_8 * t1_2 + t8_9 * t1_1
+ t10_10 = t8_8 * t2_2
+ u = 1
+ u -= (1./24) * t2_2 + (1./160) * t2_4 + (1./896) * t2_6 + (1./4608) * t2_8
+ u += (1./1920) * t4_4 + (1./10752) * t4_6 + (1./55296) * t4_8 + (1./270336) * t4_10
+ u -= (1./322560) * t6_6 + (1./1658880) * t6_8 + (1./8110080) * t6_10
+ u += (1./92897280) * t8_8 + (1./454164480) * t8_10
+ u -= 2.4464949595157930e-11 * t10_10
+ v = (1./12) * t1_2 + (1./80) * t1_4
+ v -= (1./480) * t3_4 + (1./2688) * t3_6 + (1./13824) * t3_8 + (1./67584) * t3_10
+ v += (1./53760) * t5_6 + (1./276480) * t5_8 + (1./1351680) * t5_10
+ v -= (1./11612160) * t7_8 + (1./56770560) * t7_10
+ v += 2.4464949595157932e-10 * t9_10
+ if n == 1:
+ x = u
+ y = v
+ else:
+ th = (((th4 * s + th3) * s + th2) * s + th1) * s
+ cth = cos(th)
+ sth = sin(th)
+
+ x += cth * u - sth * v
+ y += cth * v + sth * u
+ s += ds
+ return [x * ds, y * ds]
+
+count_iter = 0
+
+# Given th0 and th1 at endpoints (measured from chord), return k0
+# and k1 such that the clothoid k(s) = k0 + k1 s, evaluated from
+# s = -.5 to .5, has the tangents given
+def solve_clothoid(th0, th1, verbose = False):
+ global count_iter
+
+ k1_old = 0
+ e_old = th1 - th0
+ k0 = th0 + th1
+ k1 = 6 * (1 - ((.5 / pi) * k0) ** 3) * e_old
+
+ # secant method
+ for i in range(10):
+ x, y = integ_spiro(k0, k1, 0, 0)
+ e = (th1 - th0) + 2 * atan2(y, x) - .25 * k1
+ count_iter += 1
+ if verbose:
+ print k0, k1, e
+ if abs(e) < 1e-9: break
+ k1_old, e_old, k1 = k1, e, k1 + (k1_old - k1) * e / (e - e_old)
+
+ return k0, k1
+
+def plot_by_thp():
+ count = 0
+ for i in range(11):
+ thp = i * .1
+ print .5 + .05 * i, .5, .5, 'setrgbcolor'
+ print '.75 setlinewidth'
+ cmd = 'moveto'
+ for j in range(-40, 41):
+ thm = j * .02
+ k0, k1 = solve_clothoid(thp - thm, thp + thm, True)
+ count += 1
+ k1 = min(40, max(-40, k1))
+ print 306 + 75 * thm, 396 - 10 * k1, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ print '% count_iter = ', count_iter, 'for', count
+
+def plot_by_thm():
+ print '.75 setlinewidth'
+ print 36, 396 - 350, 'moveto'
+ print 0, 700, 'rlineto stroke'
+ for i in range(-10, 10):
+ if i == 0: wid = 636
+ else: wid = 5
+ print 36, 396 - 10 * i, 'moveto', wid, '0 rlineto stroke'
+ cmd = 'moveto'
+ thm = -.1
+ for i in range(41):
+ thp = i * .1
+ k0, k1 = solve_clothoid(thp - thm, thp + thm)
+ print 36 + 150 * thp, 396 - 100 * k1, cmd
+ cmd = 'lineto'
+ print 'stroke'
+ print '0 0 1 setrgbcolor'
+ cmd = 'moveto'
+ for i in range(41):
+ thp = i * .1
+ k1 = 12 * thm * cos(.5 * thp)
+ k1 = 12 * thm * (1 - (thp / pi) ** 3)
+ print 36 + 150 * thp, 396 - 100 * k1, cmd
+ cmd = 'lineto'
+ print 'stroke'
+
+plot_by_thp()
diff --git a/third_party/spiro/curves/poly3.py b/third_party/spiro/curves/poly3.py
new file mode 100644
index 0000000..daf6f87
--- /dev/null
+++ b/third_party/spiro/curves/poly3.py
@@ -0,0 +1,148 @@
+# Numerical techniques for solving 3rd order polynomial spline systems
+
+# The standard representation is the vector of derivatives at s=0,
+# with -.5 <= s <= 5.
+#
+# Thus, \kappa(s) = k0 + k1 s + 1/2 k2 s^2 + 1/6 k3 s^3
+
+from math import *
+
+def eval_cubic(a, b, c, d, x):
+ return ((d * x + c) * x + b) * x + a
+
+# integrate over s = [0, 1]
+def int_3spiro_poly(ks, n):
+ x, y = 0, 0
+ th = 0
+ ds = 1.0 / n
+ th1, th2, th3, th4 = ks[0], .5 * ks[1], (1./6) * ks[2], (1./24) * ks[3]
+ k0, k1, k2, k3 = ks[0] * ds, ks[1] * ds, ks[2] * ds, ks[3] * ds
+ s = 0
+ result = [(x, y)]
+ for i in range(n):
+ sm = s + 0.5 * ds
+ th = sm * eval_cubic(th1, th2, th3, th4, sm)
+ cth = cos(th)
+ sth = sin(th)
+
+ km0 = ((1./6 * k3 * sm + .5 * k2) * sm + k1) * sm + k0
+ km1 = ((.5 * k3 * sm + k2) * sm + k1) * ds
+ km2 = (k3 * sm + k2) * ds * ds
+ km3 = k3 * ds * ds * ds
+ #print km0, km1, km2, km3
+ u = 1 - km0 * km0 / 24
+ v = km1 / 24
+
+ u = 1 - km0 * km0 / 24 + (km0 ** 4 - 4 * km0 * km2 - 3 * km1 * km1) / 1920
+ v = km1 / 24 + (km3 - 6 * km0 * km0 * km1) / 1920
+
+ x += cth * u - sth * v
+ y += cth * v + sth * u
+ result.append((ds * x, ds * y))
+
+ s += ds
+
+ return result
+
+def integ_chord(k, n = 64):
+ ks = (k[0] * .5, k[1] * .25, k[2] * .125, k[3] * .0625)
+ xp, yp = int_3spiro_poly(ks, n)[-1]
+ ks = (k[0] * -.5, k[1] * .25, k[2] * -.125, k[3] * .0625)
+ xm, ym = int_3spiro_poly(ks, n)[-1]
+ dx, dy = .5 * (xp + xm), .5 * (yp + ym)
+ return hypot(dx, dy), atan2(dy, dx)
+
+# Return th0, th1, k0, k1 for given params
+def calc_thk(ks):
+ chord, ch_th = integ_chord(ks)
+ th0 = ch_th - (-.5 * ks[0] + .125 * ks[1] - 1./48 * ks[2] + 1./384 * ks[3])
+ th1 = (.5 * ks[0] + .125 * ks[1] + 1./48 * ks[2] + 1./384 * ks[3]) - ch_th
+ k0 = chord * (ks[0] - .5 * ks[1] + .125 * ks[2] - 1./48 * ks[3])
+ k1 = chord * (ks[0] + .5 * ks[1] + .125 * ks[2] + 1./48 * ks[3])
+ #print '%', (-.5 * ks[0] + .125 * ks[1] - 1./48 * ks[2] + 1./384 * ks[3]), (.5 * ks[0] + .125 * ks[1] + 1./48 * ks[2] + 1./384 * ks[3]), ch_th
+ return th0, th1, k0, k1
+
+def calc_k1k2(ks):
+ chord, ch_th = integ_chord(ks)
+ k1l = chord * chord * (ks[1] - .5 * ks[2] + .125 * ks[3])
+ k1r = chord * chord * (ks[1] + .5 * ks[2] + .125 * ks[3])
+ k2l = chord * chord * chord * (ks[2] - .5 * ks[3])
+ k2r = chord * chord * chord * (ks[2] + .5 * ks[3])
+ return k1l, k1r, k2l, k2r
+
+def plot(ks):
+ ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
+ pside = int_3spiro_poly(ksp, 64)
+ ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
+ mside = int_3spiro_poly(ksm, 64)
+ mside.reverse()
+ for i in range(len(mside)):
+ mside[i] = (-mside[i][0], -mside[i][1])
+ pts = mside + pside[1:]
+ cmd = "moveto"
+ for j in range(len(pts)):
+ x, y = pts[j]
+ print 306 + 300 * x, 400 + 300 * y, cmd
+ cmd = "lineto"
+ print "stroke"
+ x, y = pts[0]
+ print 306 + 300 * x, 400 + 300 * y, "moveto"
+ x, y = pts[-1]
+ print 306 + 300 * x, 400 + 300 * y, "lineto .5 setlinewidth stroke"
+ print "showpage"
+
+def solve_3spiro(th0, th1, k0, k1):
+ ks = [0, 0, 0, 0]
+ for i in range(5):
+ th0_a, th1_a, k0_a, k1_a = calc_thk(ks)
+ dth0 = th0 - th0_a
+ dth1 = th1 - th1_a
+ dk0 = k0 - k0_a
+ dk1 = k1 - k1_a
+ ks[0] += (dth0 + dth1) * 1.5 + (dk0 + dk1) * -.25
+ ks[1] += (dth1 - dth0) * 15 + (dk0 - dk1) * 1.5
+ ks[2] += (dth0 + dth1) * -12 + (dk0 + dk1) * 6
+ ks[3] += (dth0 - dth1) * 360 + (dk1 - dk0) * 60
+ #print '% ks =', ks
+ return ks
+
+def iter_spline(pts, ths, ks):
+ pass
+
+def solve_vee():
+ kss = []
+ for i in range(10):
+ kss.append([0, 0, 0, 0])
+ thl = [0] * len(kss)
+ thr = [0] * len(kss)
+ k0l = [0] * len(kss)
+ k0r = [0] * len(kss)
+ k1l = [0] * len(kss)
+ k1r = [0] * len(kss)
+ k2l = [0] * len(kss)
+ k2r = [0] * len(kss)
+ for i in range(10):
+ for j in range(len(kss)):
+ thl[j], thr[j], k0l[j], k0r[j] = calc_thk(kss[j])
+ k0l[j], k1r[j], k2l[j], k2r[j] = calc_k1k2(kss[j])
+ for j in range(len(kss) - 1):
+ dth = thl[j + 1] + thr[j]
+ if j == 5: dth += .1
+ dk0 = k0l[j + 1] - k0r[j]
+ dk1 = k1l[j + 1] - k1r[j]
+ dk2 = k2l[j + 1] - k2r[j]
+
+
+if __name__ == '__main__':
+ k0 = pi * 3
+ ks = [0, k0, -2 * k0, 0]
+ ks = [0, 0, 0, 0.01]
+ #plot(ks)
+ thk = calc_thk(ks)
+ print '%', thk
+
+ ks = solve_3spiro(0, 0, 0, 0.001)
+ print '% thk =', calc_thk(ks)
+ #plot(ks)
+ print '%', ks
+ print calc_k1k2(ks)
diff --git a/third_party/spiro/curves/polymat-bad.py b/third_party/spiro/curves/polymat-bad.py
new file mode 100644
index 0000000..493fcd8
--- /dev/null
+++ b/third_party/spiro/curves/polymat-bad.py
@@ -0,0 +1,64 @@
+from Numeric import *
+import LinearAlgebra as la
+import sys
+
+n = 15
+m = zeros(((n + 1) * 4, (n + 1) * 4), Float)
+for i in range(n):
+ m[4 * i + 2][4 * i + 0] = .5
+ m[4 * i + 2][4 * i + 1] = -1./12
+ m[4 * i + 2][4 * i + 2] = 1./48
+ m[4 * i + 2][4 * i + 3] = -1./480
+ m[4 * i + 2][4 * i + 4] = .5
+ m[4 * i + 2][4 * i + 5] = 1./12
+ m[4 * i + 2][4 * i + 6] = 1./48
+ m[4 * i + 2][4 * i + 7] = 1./480
+
+ m[4 * i + 3][4 * i + 0] = 1
+ m[4 * i + 3][4 * i + 1] = .5
+ m[4 * i + 3][4 * i + 2] = .125
+ m[4 * i + 3][4 * i + 3] = 1./48
+ m[4 * i + 3][4 * i + 4] = -1
+ m[4 * i + 3][4 * i + 5] = .5
+ m[4 * i + 3][4 * i + 6] = -.125
+ m[4 * i + 3][4 * i + 7] = 1./48
+
+ m[4 * i + 4][4 * i + 0] = 0
+ m[4 * i + 4][4 * i + 1] = 1
+ m[4 * i + 4][4 * i + 2] = .5
+ m[4 * i + 4][4 * i + 3] = .125
+ m[4 * i + 4][4 * i + 4] = 0
+ m[4 * i + 4][4 * i + 5] = -1
+ m[4 * i + 4][4 * i + 6] = .5
+ m[4 * i + 4][4 * i + 7] = -.125
+
+ m[4 * i + 5][4 * i + 0] = 0
+ m[4 * i + 5][4 * i + 1] = 0
+ m[4 * i + 5][4 * i + 2] = 1
+ m[4 * i + 5][4 * i + 3] = .5
+ m[4 * i + 5][4 * i + 4] = 0
+ m[4 * i + 5][4 * i + 5] = 0
+ m[4 * i + 5][4 * i + 6] = -1
+ m[4 * i + 5][4 * i + 7] = .5
+
+m[n * 4 + 2][2] = 1
+m[n * 4 + 3][3] = 1
+
+m[0][n * 4 + 2] = 1
+m[1][n * 4 + 3] = 1
+
+def printarr(m):
+ for j in range(n * 4 + 4):
+ for i in range(n * 4 + 4):
+ print '%6.1f' % m[j][i],
+ print ''
+
+sys.output_line_width = 160
+#print array2string(m, precision = 3)
+mi = la.inverse(m)
+#printarr(mi)
+print ''
+for j in range(n + 1):
+ for k in range(4):
+ print '%7.2f' % mi[j * 4 + k][(n / 2) * 4 + 2],
+ print ''
diff --git a/third_party/spiro/curves/polymat.py b/third_party/spiro/curves/polymat.py
new file mode 100644
index 0000000..a05e2bf
--- /dev/null
+++ b/third_party/spiro/curves/polymat.py
@@ -0,0 +1,399 @@
+import sys
+from math import *
+
+from Numeric import *
+import LinearAlgebra as la
+
+import poly3
+
+n = 15
+m = zeros(((n + 1) * 4, (n + 1) * 4), Float)
+for i in range(n):
+ m[4 * i + 2][4 * i + 0] = .5
+ m[4 * i + 2][4 * i + 1] = -1./12
+ m[4 * i + 2][4 * i + 2] = 1./48
+ m[4 * i + 2][4 * i + 3] = -1./480
+ m[4 * i + 2][4 * i + 4] = .5
+ m[4 * i + 2][4 * i + 5] = 1./12
+ m[4 * i + 2][4 * i + 6] = 1./48
+ m[4 * i + 2][4 * i + 7] = 1./480
+
+ m[4 * i + 3][4 * i + 0] = 1
+ m[4 * i + 3][4 * i + 1] = .5
+ m[4 * i + 3][4 * i + 2] = .125
+ m[4 * i + 3][4 * i + 3] = 1./48
+ m[4 * i + 3][4 * i + 4] = -1
+ m[4 * i + 3][4 * i + 5] = .5
+ m[4 * i + 3][4 * i + 6] = -.125
+ m[4 * i + 3][4 * i + 7] = 1./48
+
+ m[4 * i + 4][4 * i + 0] = 0
+ m[4 * i + 4][4 * i + 1] = 1
+ m[4 * i + 4][4 * i + 2] = .5
+ m[4 * i + 4][4 * i + 3] = .125
+ m[4 * i + 4][4 * i + 4] = 0
+ m[4 * i + 4][4 * i + 5] = -1
+ m[4 * i + 4][4 * i + 6] = .5
+ m[4 * i + 4][4 * i + 7] = -.125
+
+ m[4 * i + 5][4 * i + 0] = 0
+ m[4 * i + 5][4 * i + 1] = 0
+ m[4 * i + 5][4 * i + 2] = 1
+ m[4 * i + 5][4 * i + 3] = .5
+ m[4 * i + 5][4 * i + 4] = 0
+ m[4 * i + 5][4 * i + 5] = 0
+ m[4 * i + 5][4 * i + 6] = -1
+ m[4 * i + 5][4 * i + 7] = .5
+
+m[n * 4 + 2][2] = 1
+m[n * 4 + 3][3] = 1
+
+m[0][n * 4 + 2] = 1
+m[1][n * 4 + 3] = 1
+
+def printarr(m):
+ for j in range(n * 4 + 4):
+ for i in range(n * 4 + 4):
+ print '%6.1f' % m[j][i],
+ print ''
+
+sys.output_line_width = 160
+#print array2string(m, precision = 3)
+mi = la.inverse(m)
+#printarr(mi)
+
+# fit arc to pts (0, 0), (x, y), and (1, 0), return th tangent to
+# arc at (x, y)
+def fit_arc(x, y):
+ th = atan2(y - 2 * x * y, y * y + x - x * x)
+ return th
+
+def mod_2pi(th):
+ u = th / (2 * pi)
+ return 2 * pi * (u - floor(u + 0.5))
+
+def plot_path(path, th, k):
+ if path[0][2] == '{': closed = 0
+ else: closed = 1
+ j = 0
+ cmd = 'moveto'
+ for i in range(len(path) + closed - 1):
+ i1 = (i + 1) % len(path)
+ x0, y0, t0 = path[i]
+ x1, y1, t1 = path[i1]
+ j1 = (j + 1) % len(th)
+ th0 = th[j]
+ k0 = k[j]
+ th1 = th[j1]
+ k1 = k[j1]
+ chord_th = atan2(y1 - y0, x1 - x0)
+ chord_len = hypot(y1 - y0, x1 - x0)
+ ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
+ mod_2pi(th1 - chord_th),
+ k0 * chord_len, k1 * chord_len)
+ ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
+ pside = poly3.int_3spiro_poly(ksp, 64)
+ ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
+ mside = poly3.int_3spiro_poly(ksm, 64)
+ mside.reverse()
+ for i in range(len(mside)):
+ mside[i] = (-mside[i][0], -mside[i][1])
+ pts = mside + pside[1:]
+ rot = chord_th - atan2(pts[-1][1] - pts[0][1], pts[-1][0] - pts[0][0])
+ scale = hypot(x1 - x0, y1 - y0) / hypot(pts[-1][1] - pts[0][1], pts[-1][0] - pts[0][0])
+ u, v = scale * cos(rot), scale * sin(rot)
+ xt = x0 - u * pts[0][0] + v * pts[0][1]
+ yt = y0 - u * pts[0][1] - v * pts[0][0]
+ for x, y in pts:
+ print xt + u * x - v * y, yt + u * y + v * x, cmd
+ cmd = 'lineto'
+ if t1 == 'v':
+ j += 2
+ else:
+ j += 1
+ print 'stroke'
+ if 0:
+ j = 0
+ for i in range(len(path)):
+ x0, y0, t0 = path[i]
+ print 'gsave', x0, y0, 'translate'
+ print ' ', th[j] * 180 / pi, 'rotate'
+ print ' -50 0 moveto 50 0 lineto stroke'
+ print 'grestore'
+ j += 1
+
+def plot_ks(path, th, k):
+ if path[0][2] == '{': closed = 0
+ else: closed = 1
+ j = 0
+ cmd = 'moveto'
+ xo = 36
+ yo = 550
+ xscale = .5
+ yscale = 2000
+ x = xo
+ for i in range(len(path) + closed - 1):
+ i1 = (i + 1) % len(path)
+ x0, y0, t0 = path[i]
+ x1, y1, t1 = path[i1]
+ j1 = (j + 1) % len(th)
+ th0 = th[j]
+ k0 = k[j]
+ th1 = th[j1]
+ k1 = k[j1]
+ chord_th = atan2(y1 - y0, x1 - x0)
+ chord_len = hypot(y1 - y0, x1 - x0)
+ ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
+ mod_2pi(th1 - chord_th),
+ k0 * chord_len, k1 * chord_len)
+ ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
+ pside = poly3.int_3spiro_poly(ksp, 64)
+ ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
+ mside = poly3.int_3spiro_poly(ksm, 64)
+ print '%', x0, y0, k0, k1
+ print "% ks = ", ks
+ l = 2 * chord_len / hypot(pside[-1][0] + mside[-1][0], pside[-1][1] + mside[-1][1])
+ for i in range(100):
+ s = .01 * i - 0.5
+ kp = poly3.eval_cubic(ks[0], ks[1], .5 * ks[2], 1./6 * ks[3], s)
+ kp /= l
+ print x + xscale * l * (s + .5), yo + yscale * kp, cmd
+ cmd = 'lineto'
+ x += xscale * l
+ if t1 == 'v':
+ j += 2
+ else:
+ j += 1
+ print 'stroke'
+ print xo, yo, 'moveto', x, yo, 'lineto stroke'
+
+def make_error_vec(path, th, k):
+ if path[0][2] == '{': closed = 0
+ else: closed = 1
+ n = len(path)
+ v = zeros(n * 2, Float)
+ j = 0
+ for i in range(len(path) + closed - 1):
+ i1 = (i + 1) % len(path)
+ x0, y0, t0 = path[i]
+ x1, y1, t1 = path[i1]
+ j1 = (j + 1) % len(th)
+ th0 = th[j]
+ k0 = k[j]
+ th1 = th[j1]
+ k1 = k[j1]
+ chord_th = atan2(y1 - y0, x1 - x0)
+ chord_len = hypot(y1 - y0, x1 - x0)
+ ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
+ mod_2pi(th1 - chord_th),
+ k0 * chord_len, k1 * chord_len)
+ ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
+ pside = poly3.int_3spiro_poly(ksp, 64)
+ ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
+ mside = poly3.int_3spiro_poly(ksm, 64)
+ l = chord_len / hypot(pside[-1][0] + mside[-1][0], pside[-1][1] + mside[-1][1])
+ k1l = (ks[1] - .5 * ks[2] + .125 * ks[3]) / (l * l)
+ k1r = (ks[1] + .5 * ks[2] + .125 * ks[3]) / (l * l)
+ k2l = (ks[2] - .5 * ks[3]) / (l * l * l)
+ k2r = (ks[2] + .5 * ks[3]) / (l * l * l)
+
+ if t0 == 'o' or t0 == '[' or t0 == 'v':
+ v[2 * j] -= k1l
+ v[2 * j + 1] -= k2l
+ elif t0 == 'c':
+ v[2 * j + 1] += k2l
+
+ if t1 == 'o' or t1 == ']' or t1 == 'v':
+ v[2 * j1] += k1r
+ v[2 * j1 + 1] += k2r
+ elif t1 == 'c':
+ v[2 * j1] += k2r
+
+ print "% left k'", k1l, "k''", k2l, "right k'", k1r, "k''", k2r
+ if t1 == 'v':
+ j += 2
+ else:
+ j += 1
+ print '% error vector:'
+ for i in range(n):
+ print '% ', v[2 * i], v[2 * i + 1]
+ return v
+
+def add_k1l(m, row, col, col1, l, s):
+ l2 = l * l
+ m[row][2 * col] += s * 36 / l2
+ m[row][2 * col + 1] -= s * 9 / l
+ m[row][2 * col1] += s * 24 / l2
+ m[row][2 * col1 + 1] += s * 3 / l
+
+def add_k1r(m, row, col, col1, l, s):
+ l2 = l * l
+ m[row][2 * col] += s * 24 / l2
+ m[row][2 * col + 1] -= s * 3 / l
+ m[row][2 * col1] += s * 36 / l2
+ m[row][2 * col1 + 1] += s * 9 / l
+
+def add_k2l(m, row, col, col1, l, s):
+ l2 = l * l
+ l3 = l2 * l
+ m[row][2 * col] -= s * 192 / l3
+ m[row][2 * col + 1] += s * 36 / l2
+ m[row][2 * col1] -= s * 168 / l3
+ m[row][2 * col1 + 1] -= s * 24 / l2
+
+def add_k2r(m, row, col, col1, l, s):
+ l2 = l * l
+ l3 = l2 * l
+ m[row][2 * col] += s * 168 / l3
+ m[row][2 * col + 1] -= s * 24 / l2
+ m[row][2 * col1] += s * 192 / l3
+ m[row][2 * col1 + 1] += s * 36 / l2
+
+def make_matrix(path, th, k):
+ if path[0][2] == '{': closed = 0
+ else: closed = 1
+ n = len(path)
+ m = zeros((n * 2, n * 2), Float)
+ j = 0
+ for i in range(len(path) + closed - 1):
+ i1 = (i + 1) % len(path)
+ x0, y0, t0 = path[i]
+ x1, y1, t1 = path[i1]
+ j1 = (j + 1) % len(th)
+ th0 = th[j]
+ k0 = k[j]
+ th1 = th[j1]
+ k1 = k[j1]
+ chord_th = atan2(y1 - y0, x1 - x0)
+ chord_len = hypot(y1 - y0, x1 - x0)
+ ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
+ mod_2pi(th1 - chord_th),
+ k0 * chord_len, k1 * chord_len)
+ ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
+ pside = poly3.int_3spiro_poly(ksp, 64)
+ ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
+ mside = poly3.int_3spiro_poly(ksm, 64)
+ l = chord_len / hypot(pside[-1][0] + mside[-1][0], pside[-1][1] + mside[-1][1])
+
+ if t0 == 'o' or t0 == '[' or t0 == 'v':
+ add_k1l(m, 2 * j, j, j1, l, -1)
+ add_k2l(m, 2 * j + 1, j, j1, l, -1)
+ elif t0 == 'c':
+ add_k2l(m, 2 * j + 1, j, j1, l, 1)
+
+ if t1 == 'o' or t1 == ']' or t1 == 'v':
+ add_k1r(m, 2 * j1, j, j1, l, 1)
+ add_k2r(m, 2 * j1 + 1, j, j1, l, 1)
+ elif t1 == 'c':
+ add_k2r(m, 2 * j1, j, j1, l, 1)
+
+ if t1 == 'v':
+ j += 2
+ else:
+ j += 1
+ return m
+
+def solve(path):
+ if path[0][2] == '{': closed = 0
+ else: closed = 1
+ dxs = []
+ dys = []
+ chords = []
+ for i in range(len(path) - 1):
+ dxs.append(path[i + 1][0] - path[i][0])
+ dys.append(path[i + 1][1] - path[i][1])
+ chords.append(hypot(dxs[-1], dys[-1]))
+ nominal_th = []
+ nominal_k = []
+ if not closed:
+ nominal_th.append(atan2(dys[0], dxs[0]))
+ nominal_k.append(0)
+ for i in range(1 - closed, len(path) - 1 + closed):
+ x0, y0, t0 = path[(i + len(path) - 1) % len(path)]
+ x1, y1, t1 = path[i]
+ x2, y2, t2 = path[(i + 1) % len(path)]
+ dx = float(x2 - x0)
+ dy = float(y2 - y0)
+ ir2 = dx * dx + dy * dy
+ x = ((x1 - x0) * dx + (y1 - y0) * dy) / ir2
+ y = ((y1 - y0) * dx - (x1 - x0) * dy) / ir2
+ th = fit_arc(x, y) + atan2(dy, dx)
+ bend_angle = mod_2pi(atan2(y2 - y1, x2 - x1) - atan2(y1 - y0, x1 - x0))
+ k = 2 * bend_angle/(hypot(y2 - y1, x2 - x1) + hypot(y1 - y0, x1 - x0))
+ print '% bend angle', bend_angle, 'k', k
+ if t1 == ']':
+ th = atan2(y1 - y0, x1 - x0)
+ k = 0
+ elif t1 == '[':
+ th = atan2(y2 - y1, x2 - x1)
+ k = 0
+ nominal_th.append(th)
+ nominal_k.append(k)
+ if not closed:
+ nominal_th.append(atan2(dys[-1], dxs[-1]))
+ nominal_k.append(0)
+ print '%', nominal_th
+ print '0 0 1 setrgbcolor .5 setlinewidth'
+ plot_path(path, nominal_th, nominal_k)
+ plot_ks(path, nominal_th, nominal_k)
+ th = nominal_th[:]
+ k = nominal_k[:]
+ n = 8
+ for i in range(n):
+ ev = make_error_vec(path, th, k)
+ m = make_matrix(path, th, k)
+ #print m
+ #print 'inverse:'
+ #print la.inverse(m)
+ v = dot(la.inverse(m), ev)
+ #print v
+ for j in range(len(path)):
+ th[j] += 1. * v[2 * j]
+ k[j] -= 1. * .5 * v[2 * j + 1]
+ if i == n - 1:
+ print '0 0 0 setrgbcolor 1 setlinewidth'
+ elif i == 0:
+ print '1 0 0 setrgbcolor'
+ elif i == 1:
+ print '0 0.5 0 setrgbcolor'
+ elif i == 2:
+ print '0.3 0.3 0.3 setrgbcolor'
+ plot_path(path, th, k)
+ plot_ks(path, th, k)
+ print '% th:', th
+ print '% k:', k
+
+path = [(100, 100, 'o'), (200, 250, 'o'), (350, 225, 'o'),
+ (450, 350, 'c'), (450, 200, 'o'), (300, 100, 'o')]
+if 0:
+ path = [(100, 480, '['), (100, 300, ']'), (300, 100, 'o'),
+ (500, 300, '['), (500, 480, ']'), (480, 500, '['),
+ (120, 500, ']')]
+
+
+path = [(100, 480, ']'), (100, 120, '['),
+ (120, 100, ']'), (480, 100, '['),
+ (500, 120, ']'), (500, 480, '['),
+ (480, 500, ']'), (120, 500, '[')]
+
+path = [(100, 120, '['), (120, 100, ']'),
+ (140, 100, 'o'), (160, 100, 'o'), (180, 100, 'o'), (200, 100, 'o'),
+ (250, 250, 'o'),
+ (100, 200, 'o'), (100, 180, 'o'), (100, 160, 'o'), (100, 140, 'o')]
+
+path = [(100, 350, 'o'), (225, 350, 'o'), (350, 450, 'o'),
+ (450, 400, 'o'), (315, 205, 'o'), (300, 200, 'o'),
+ (285, 205, 'o')]
+
+if 0:
+ path = []
+ path.append((350, 600, 'c'))
+ path.append((50, 450, 'c'))
+ for i in range(10):
+ path.append((50 + i * 30, 300 - i * 5, 'c'))
+ for i in range(11):
+ path.append((350 + i * 30, 250 + i * 5, 'c'))
+ path.append((650, 450, 'c'))
+
+solve(path)
+print 'showpage'
diff --git a/third_party/spiro/curves/tocubic.py b/third_party/spiro/curves/tocubic.py
new file mode 100644
index 0000000..d70b9a2
--- /dev/null
+++ b/third_party/spiro/curves/tocubic.py
@@ -0,0 +1,461 @@
+# Some code to convert arbitrary curves to high quality cubics.
+
+# Some conventions: points are (x, y) pairs. Cubic Bezier segments are
+# lists of four points.
+
+import sys
+
+from math import *
+
+import pcorn
+
+def pt_wsum(points, wts):
+ x, y = 0, 0
+ for i in range(len(points)):
+ x += points[i][0] * wts[i]
+ y += points[i][1] * wts[i]
+ return x, y
+
+# Very basic spline primitives
+def bz_eval(bz, t):
+ degree = len(bz) - 1
+ mt = 1 - t
+ if degree == 3:
+ return pt_wsum(bz, [mt * mt * mt, 3 * mt * mt * t, 3 * mt * t * t, t * t * t])
+ elif degree == 2:
+ return pt_wsum(bz, [mt * mt, 2 * mt * t, t * t])
+ elif degree == 1:
+ return pt_wsum(bz, [mt, t])
+
+def bz_deriv(bz):
+ degree = len(bz) - 1
+ return [(degree * (bz[i + 1][0] - bz[i][0]), degree * (bz[i + 1][1] - bz[i][1])) for i in range(degree)]
+
+def bz_arclength(bz, n = 10):
+ # We're just going to integrate |z'| over the parameter [0..1].
+ # The integration algorithm here is eqn 4.1.14 from NRC2, and is
+ # chosen for simplicity. Likely adaptive and/or higher-order
+ # algorithms would be better, but this should be good enough.
+ # Convergence should be quartic in n.
+ wtarr = (3./8, 7./6, 23./24)
+ dt = 1./n
+ s = 0
+ dbz = bz_deriv(bz)
+ for i in range(0, n + 1):
+ if i < 3:
+ wt = wtarr[i]
+ elif i > n - 3:
+ wt = wtarr[n - i]
+ else:
+ wt = 1.
+ dx, dy = bz_eval(dbz, i * dt)
+ ds = hypot(dx, dy)
+ s += wt * ds
+ return s * dt
+
+# One step of 4th-order Runge-Kutta numerical integration - update y in place
+def rk4(y, dydx, x, h, derivs):
+ hh = h * .5
+ h6 = h * (1./6)
+ xh = x + hh
+ yt = []
+ for i in range(len(y)):
+ yt.append(y[i] + hh * dydx[i])
+ dyt = derivs(xh, yt)
+ for i in range(len(y)):
+ yt[i] = y[i] + hh * dyt[i]
+ dym = derivs(xh, yt)
+ for i in range(len(y)):
+ yt[i] = y[i] + h * dym[i]
+ dym[i] += dyt[i]
+ dyt = derivs(x + h, yt)
+ for i in range(len(y)):
+ y[i] += h6 * (dydx[i] + dyt[i] + 2 * dym[i])
+
+def bz_arclength_rk4(bz, n = 10):
+ dbz = bz_deriv(bz)
+ def arclength_deriv(x, ys):
+ dx, dy = bz_eval(dbz, x)
+ return [hypot(dx, dy)]
+ dt = 1./n
+ t = 0
+ ys = [0]
+ for i in range(n):
+ dydx = arclength_deriv(t, ys)
+ rk4(ys, dydx, t, dt, arclength_deriv)
+ t += dt
+ return ys[0]
+
+# z0 and z1 are start and end points, resp.
+# th0 and th1 are the initial and final tangents, measured in the
+# direction of the curve.
+# aab is a/(a + b), where a and b are the lengths of the bezier "arms"
+def fit_cubic_arclen(z0, z1, arclen, th0, th1, aab):
+ chord = hypot(z1[0] - z0[0], z1[1] - z0[1])
+ cth0, sth0 = cos(th0), sin(th0)
+ cth1, sth1 = -cos(th1), -sin(th1)
+ armlen = .66667 * chord
+ darmlen = 1e-6 * armlen
+ for i in range(10):
+ a = armlen * aab
+ b = armlen - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ actual_s = bz_arclength_rk4(bz)
+ if (abs(arclen - actual_s) < 1e-12):
+ break
+ a = (armlen + darmlen) * aab
+ b = (armlen + darmlen) - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ actual_s2 = bz_arclength_rk4(bz)
+ ds = (actual_s2 - actual_s) / darmlen
+ #print '% armlen = ', armlen
+ if ds == 0:
+ break
+ armlen += (arclen - actual_s) / ds
+ a = armlen * aab
+ b = armlen - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ return bz
+
+def mod_2pi(th):
+ u = th / (2 * pi)
+ return 2 * pi * (u - floor(u + 0.5))
+
+def measure_bz(bz, arclen, th_fn, n = 1000):
+ dt = 1./n
+ dbz = bz_deriv(bz)
+ s = 0
+ score = 0
+ for i in range(n):
+ dx, dy = bz_eval(dbz, (i + .5) * dt)
+ ds = dt * hypot(dx, dy)
+ s += ds
+ score += ds * (mod_2pi(atan2(dy, dx) - th_fn(s)) ** 2)
+ return score
+
+def measure_bz_rk4(bz, arclen, th_fn, n = 10):
+ dbz = bz_deriv(bz)
+ def measure_derivs(x, ys):
+ dx, dy = bz_eval(dbz, x)
+ ds = hypot(dx, dy)
+ s = ys[0]
+ dscore = ds * (mod_2pi(atan2(dy, dx) - th_fn(s)) ** 2)
+ return [ds, dscore]
+ dt = 1./n
+ t = 0
+ ys = [0, 0]
+ for i in range(n):
+ dydx = measure_derivs(t, ys)
+ rk4(ys, dydx, t, dt, measure_derivs)
+ t += dt
+ return ys[1]
+
+# th_fn() is a function that takes an arclength from the start point, and
+# returns an angle - thus th_fn(0) and th_fn(arclen) are the initial and
+# final tangents.
+# z0, z1, and arclen are as fit_cubic_arclen
+def fit_cubic(z0, z1, arclen, th_fn, fast = 1):
+ chord = hypot(z1[0] - z0[0], z1[1] - z0[1])
+ if (arclen < 1.000001 * chord):
+ return [z0, z1], 0
+ th0 = th_fn(0)
+ th1 = th_fn(arclen)
+ imax = 4
+ jmax = 10
+ aabmin = 0
+ aabmax = 1.
+ if fast:
+ imax = 1
+ jmax = 0
+ for i in range(imax):
+ for j in range(jmax + 1):
+ if jmax == 0:
+ aab = 0.5 * (aabmin + aabmax)
+ else:
+ aab = aabmin + (aabmax - aabmin) * j / jmax
+ bz = fit_cubic_arclen(z0, z1, arclen, th0, th1, aab)
+ score = measure_bz_rk4(bz, arclen, th_fn)
+ print '% aab =', aab, 'score =', score
+ sys.stdout.flush()
+ if j == 0 or score < best_score:
+ best_score = score
+ best_aab = aab
+ best_bz = bz
+ daab = .06 * (aabmax - aabmin)
+ aabmin = max(0, best_aab - daab)
+ aabmax = min(1, best_aab + daab)
+ print '%--- best_aab =', best_aab
+ return best_bz, best_score
+
+def plot_prolog():
+ print '%!PS'
+ print '/m { moveto } bind def'
+ print '/l { lineto } bind def'
+ print '/c { curveto } bind def'
+ print '/z { closepath } bind def'
+
+def plot_bz(bz, z0, scale, do_moveto = True):
+ x0, y0 = z0
+ if do_moveto:
+ print bz[0][0] * scale + x0, bz[0][1] * scale + y0, 'm'
+ if len(bz) == 4:
+ x1, y1 = bz[1][0] * scale + x0, bz[1][1] * scale + y0
+ x2, y2 = bz[2][0] * scale + x0, bz[2][1] * scale + y0
+ x3, y3 = bz[3][0] * scale + x0, bz[3][1] * scale + y0
+ print x1, y1, x2, y2, x3, y3, 'c'
+ elif len(bz) == 2:
+ print bz[1][0] * scale + x0, bz[1][1] * scale + y0, 'l'
+
+def test_bz_arclength():
+ bz = [(0, 0), (.5, 0), (1, 0.5), (1, 1)]
+ ans = bz_arclength_rk4(bz, 2048)
+ last = 1
+ lastrk = 1
+ for i in range(3, 11):
+ n = 1 << i
+ err = bz_arclength(bz, n) - ans
+ err_rk = bz_arclength_rk4(bz, n) - ans
+ print n, err, last / err, err_rk, lastrk / err_rk
+ last = err
+ lastrk = err_rk
+
+def test_fit_cubic_arclen():
+ th = pi / 4
+ arclen = th / sin(th)
+ bz = fit_cubic_arclen((0, 0), (1, 0), arclen, th, th, .5)
+ print '%', bz
+ plot_bz(bz, (100, 400), 500)
+ print 'stroke'
+ print 'showpage'
+
+# -- cornu fitting
+
+import cornu
+
+def cornu_to_cubic(t0, t1):
+ def th_fn(s):
+ return (s + t0) ** 2
+ y0, x0 = cornu.eval_cornu(t0)
+ y1, x1 = cornu.eval_cornu(t1)
+ bz, score = fit_cubic((x0, y0), (x1, y1), t1 - t0, th_fn, 0)
+ return bz, score
+
+def test_draw_cornu():
+ plot_prolog()
+ thresh = 1e-6
+ print '/ss 1.5 def'
+ print '/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def'
+ s0 = 0
+ imax = 200
+ x0, y0, scale = 36, 100, 500
+ bzs = []
+ for i in range(1, imax):
+ s = sqrt(i * .1)
+ bz, score = cornu_to_cubic(s0, s)
+ if score > (s - s0) * thresh or i == imax - 1:
+ plot_bz(bz, (x0, y0), scale, s0 == 0)
+ bzs.append(bz)
+ s0 = s
+ print 'stroke'
+ for i in range(len(bzs)):
+ bz = bzs[i]
+ bx0, by0 = x0 + bz[0][0] * scale, y0 + bz[0][1] * scale
+ bx1, by1 = x0 + bz[1][0] * scale, y0 + bz[1][1] * scale
+ bx2, by2 = x0 + bz[2][0] * scale, y0 + bz[2][1] * scale
+ bx3, by3 = x0 + bz[3][0] * scale, y0 + bz[3][1] * scale
+ print 'gsave 0 0 1 setrgbcolor .5 setlinewidth'
+ print bx0, by0, 'moveto', bx1, by1, 'lineto stroke'
+ print bx2, by2, 'moveto', bx3, by3, 'lineto stroke'
+ print 'grestore'
+ print 'gsave', bx0, by0, 'translate circle fill grestore'
+ print 'gsave', bx1, by1, 'translate .5 dup scale circle fill grestore'
+ print 'gsave', bx2, by2, 'translate .5 dup scale circle fill grestore'
+ print 'gsave', bx3, by3, 'translate circle fill grestore'
+
+# -- fitting of piecewise cornu curves
+
+def pcorn_segment_to_bzs_optim_inner(curve, s0, s1, thresh, nmax = None):
+ result = []
+ if s0 == s1: return [], 0
+ while s0 < s1:
+ def th_fn_inner(s):
+ if s > s1: s = s1
+ return curve.th(s0 + s, s == 0)
+ z0 = curve.xy(s0)
+ z1 = curve.xy(s1)
+ bz, score = fit_cubic(z0, z1, s1 - s0, th_fn_inner, 0)
+ if score < thresh or nmax != None and len(result) == nmax - 1:
+ result.append(bz)
+ break
+ r = s1
+ l = s0 + .001 * (s1 - s0)
+ for i in range(10):
+ smid = 0.5 * (l + r)
+ zmid = curve.xy(smid)
+ bz, score = fit_cubic(z0, zmid, smid - s0, th_fn_inner, 0)
+ if score > thresh:
+ r = smid
+ else:
+ l = smid
+ print '% s0=', s0, 'smid=', smid, 'actual score =', score
+ result.append(bz)
+ s0 = smid
+ print '% last actual score=', score
+ return result, score
+
+def pcorn_segment_to_bzs_optim(curve, s0, s1, thresh, optim):
+ result, score = pcorn_segment_to_bzs_optim_inner(curve, s0, s1, thresh)
+ bresult, bscore = result, score
+ if len(result) > 1 and optim > 2:
+ nmax = len(result)
+ gamma = 1./6
+ l = score
+ r = thresh
+ for i in range(5):
+ tmid = (0.5 * (l ** gamma + r ** gamma)) ** (1/gamma)
+ result, score = pcorn_segment_to_bzs_optim_inner(curve, s0, s1, tmid, nmax)
+ if score < tmid:
+ l = max(l, score)
+ r = tmid
+ else:
+ l = tmid
+ r = min(r, score)
+ if max(score, tmid) < bscore:
+ bresult, bscore = result, max(score, tmid)
+ return result
+
+def pcorn_segment_to_bzs(curve, s0, s1, optim = 0, thresh = 1e-3):
+ if optim >= 2:
+ return pcorn_segment_to_bzs_optim(curve, s0, s1, thresh, optim)
+ z0 = curve.xy(s0)
+ z1 = curve.xy(s1)
+ fast = (optim == 0)
+ def th_fn(s):
+ return curve.th(s0 + s, s == 0)
+ bz, score = fit_cubic(z0, z1, s1 - s0, th_fn, fast)
+ if score < thresh:
+ return [bz]
+ else:
+ smid = 0.5 * (s0 + s1)
+ result = pcorn_segment_to_bzs(curve, s0, smid, optim, thresh)
+ result.extend(pcorn_segment_to_bzs(curve, smid, s1, optim, thresh))
+ return result
+
+def pcorn_curve_to_bzs(curve, optim = 3, thresh = 1e-3):
+ result = []
+ extrema = curve.find_extrema()
+ extrema.extend(curve.find_breaks())
+ extrema.sort()
+ print '%', extrema
+ for i in range(len(extrema)):
+ s0 = extrema[i]
+ if i == len(extrema) - 1:
+ s1 = extrema[0] + curve.arclen
+ else:
+ s1 = extrema[i + 1]
+ result.extend(pcorn_segment_to_bzs(curve, s0, s1, optim, thresh))
+ return result
+
+import struct
+
+def fit_cubic_arclen_forplot(z0, z1, arclen, th0, th1, aab):
+ chord = hypot(z1[0] - z0[0], z1[1] - z0[1])
+ cth0, sth0 = cos(th0), sin(th0)
+ cth1, sth1 = -cos(th1), -sin(th1)
+ armlen = .66667 * chord
+ darmlen = 1e-6 * armlen
+ for i in range(10):
+ a = armlen * aab
+ b = armlen - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ actual_s = bz_arclength_rk4(bz)
+ if (abs(arclen - actual_s) < 1e-12):
+ break
+ a = (armlen + darmlen) * aab
+ b = (armlen + darmlen) - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ actual_s2 = bz_arclength_rk4(bz)
+ ds = (actual_s2 - actual_s) / darmlen
+ #print '% armlen = ', armlen
+ armlen += (arclen - actual_s) / ds
+ a = armlen * aab
+ b = armlen - a
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ return bz, a, b
+
+def plot_errors_2d(t0, t1, as_ppm):
+ xs = 1024
+ ys = 1024
+ if as_ppm:
+ print 'P6'
+ print xs, ys
+ print 255
+ def th_fn(s):
+ return (s + t0) ** 2
+ y0, x0 = cornu.eval_cornu(t0)
+ y1, x1 = cornu.eval_cornu(t1)
+ z0 = (x0, y0)
+ z1 = (x1, y1)
+ chord = hypot(y1 - y0, x1 - x0)
+ arclen = t1 - t0
+ th0 = th_fn(0)
+ th1 = th_fn(arclen)
+ cth0, sth0 = cos(th0), sin(th0)
+ cth1, sth1 = -cos(th1), -sin(th1)
+
+ for y in range(ys):
+ b = .8 * chord * (ys - y - 1) / ys
+ for x in range(xs):
+ a = .8 * chord * x / xs
+ bz = [z0, (z0[0] + cth0 * a, z0[1] + sth0 * a),
+ (z1[0] + cth1 * b, z1[1] + sth1 * b), z1]
+ s_bz = bz_arclength(bz, 10)
+ def th_fn_scaled(s):
+ return (s * arclen / s_bz + t0) ** 2
+ score = measure_bz_rk4(bz, arclen, th_fn_scaled, 10)
+ if as_ppm:
+ ls = -log(score)
+ color_th = ls
+ darkstep = 0
+ if s_bz > arclen:
+ g0 = 128 - darkstep
+ else:
+ g0 = 128 + darkstep
+ sc = 127 - darkstep
+ rr = g0 + sc * cos(color_th)
+ gg = g0 + sc * cos(color_th + 2 * pi / 3)
+ bb = g0 + sc * cos(color_th - 2 * pi / 3)
+ sys.stdout.write(struct.pack('3B', rr, gg, bb))
+ else:
+ print a, b, score
+ if not as_ppm:
+ print
+
+def plot_arclen(t0, t1):
+ def th_fn(s):
+ return (s + t0) ** 2
+ y0, x0 = cornu.eval_cornu(t0)
+ y1, x1 = cornu.eval_cornu(t1)
+ z0 = (x0, y0)
+ z1 = (x1, y1)
+ chord = hypot(y1 - y0, x1 - x0)
+ arclen = t1 - t0
+ th0 = th_fn(0)
+ th1 = th_fn(arclen)
+ for i in range(101):
+ aab = i * .01
+ bz, a, b = fit_cubic_arclen_forplot(z0, z1, arclen, th0, th1, aab)
+ print a, b
+
+if __name__ == '__main__':
+ #test_bz_arclength()
+ test_draw_cornu()
+ #run_one_cornu_seg()
+ #plot_errors_2d(.5, 1.0, False)
+ #plot_arclen(.5, 1.0)