Add Spiro version 0.01.

1 files changed, 399 insertions, 0 deletions

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'