1 files changed, 461 insertions, 0 deletions
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)