diff options
Diffstat (limited to 'third_party/spiro/curves')
| -rw-r--r-- | third_party/spiro/curves/band.py | 85 | ||||
| -rw-r--r-- | third_party/spiro/curves/bezfigs.py | 183 | ||||
| -rw-r--r-- | third_party/spiro/curves/bigmat.py | 662 | ||||
| -rw-r--r-- | third_party/spiro/curves/cloth_off.py | 2 | ||||
| -rw-r--r-- | third_party/spiro/curves/clothoid.py | 66 | ||||
| -rw-r--r-- | third_party/spiro/curves/cornu.py | 140 | ||||
| -rw-r--r-- | third_party/spiro/curves/euler-elastica.py | 29 | ||||
| -rw-r--r-- | third_party/spiro/curves/fromcubic.py | 208 | ||||
| -rw-r--r-- | third_party/spiro/curves/mecsolve.py | 859 | ||||
| -rw-r--r-- | third_party/spiro/curves/mvc.py | 208 | ||||
| -rw-r--r-- | third_party/spiro/curves/numintsynth.py | 176 | ||||
| -rw-r--r-- | third_party/spiro/curves/offset.py | 21 | ||||
| -rw-r--r-- | third_party/spiro/curves/pcorn.py | 160 | ||||
| -rw-r--r-- | third_party/spiro/curves/plot_solve_clothoid.py | 164 | ||||
| -rw-r--r-- | third_party/spiro/curves/poly3.py | 148 | ||||
| -rw-r--r-- | third_party/spiro/curves/polymat-bad.py | 64 | ||||
| -rw-r--r-- | third_party/spiro/curves/polymat.py | 399 | ||||
| -rw-r--r-- | third_party/spiro/curves/tocubic.py | 461 |
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) |