diff options
Diffstat (limited to 'third_party/spiro/curves/fromcubic.py')
| -rw-r--r-- | third_party/spiro/curves/fromcubic.py | 208 |
1 files changed, 208 insertions, 0 deletions
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) |