summaryrefslogtreecommitdiff log msg author committer range
path: root/third_party/spiro/curves/polymat.py
diff options
 context: 12345678910152025303540 space: includeignore mode: unifiedssdiffstat only
Diffstat (limited to 'third_party/spiro/curves/polymat.py')
-rw-r--r--third_party/spiro/curves/polymat.py399
1 files changed, 399 insertions, 0 deletions
 diff --git a/third_party/spiro/curves/polymat.py b/third_party/spiro/curves/polymat.pynew file mode 100644index 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'