# 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)