font-Roboto/third_party/spiro/curves/plot_solve_clothoid.py

import clothoid
from math import *

print '%!PS-Adobe'

def integ_spiro(k0, k1, k2, k3, n = 4):
    th1 = k0
    th2 = .5 * k1
    th3 = (1./6) * k2
    th4 = (1./24) * k3
    ds = 1. / n
    ds2 = ds * ds
    ds3 = ds2 * ds
    s = .5 * ds - .5

    k0 *= ds
    k1 *= ds
    k2 *= ds
    k3 *= ds

    x = 0
    y = 0

    for i in range(n):
	if n == 1:
	    km0 = k0
	    km1 = k1 * ds
	    km2 = k2 * ds2
        else:
	    km0 = (((1./6) * k3 * s + .5 * k2) * s + k1) * s + k0
	    km1 = ((.5 * k3 * s + k2) * s + k1) * ds
	    km2 = (k3 * s + k2) * ds2
	km3 = k3 * ds3

	t1_1 = km0
	t1_2 = .5 * km1
	t1_3 = (1./6) * km2
	t1_4 = (1./24) * km3
	t2_2 = t1_1 * t1_1
	t2_3 = 2 * (t1_1 * t1_2)
	t2_4 = 2 * (t1_1 * t1_3) + t1_2 * t1_2
	t2_5 = 2 * (t1_1 * t1_4 + t1_2 * t1_3)
	t2_6 = 2 * (t1_2 * t1_4) + t1_3 * t1_3
	t2_7 = 2 * (t1_3 * t1_4)
	t2_8 = t1_4 * t1_4
	t3_4 = t2_2 * t1_2 + t2_3 * t1_1
	t3_6 = t2_2 * t1_4 + t2_3 * t1_3 + t2_4 * t1_2 + t2_5 * t1_1
	t3_8 = t2_4 * t1_4 + t2_5 * t1_3 + t2_6 * t1_2 + t2_7 * t1_1
	t3_10 = t2_6 * t1_4 + t2_7 * t1_3 + t2_8 * t1_2
	t4_4 = t2_2 * t2_2
	t4_5 = 2 * (t2_2 * t2_3)
	t4_6 = 2 * (t2_2 * t2_4) + t2_3 * t2_3
	t4_7 = 2 * (t2_2 * t2_5 + t2_3 * t2_4)
	t4_8 = 2 * (t2_2 * t2_6 + t2_3 * t2_5) + t2_4 * t2_4
	t4_9 = 2 * (t2_2 * t2_7 + t2_3 * t2_6 + t2_4 * t2_5)
	t4_10 = 2 * (t2_2 * t2_8 + t2_3 * t2_7 + t2_4 * t2_6) + t2_5 * t2_5
	t5_6 = t4_4 * t1_2 + t4_5 * t1_1
	t5_8 = t4_4 * t1_4 + t4_5 * t1_3 + t4_6 * t1_2 + t4_7 * t1_1
	t5_10 = t4_6 * t1_4 + t4_7 * t1_3 + t4_8 * t1_2 + t4_9 * t1_1
	t6_6 = t4_4 * t2_2
	t6_7 = t4_4 * t2_3 + t4_5 * t2_2
	t6_8 = t4_4 * t2_4 + t4_5 * t2_3 + t4_6 * t2_2
	t6_9 = t4_4 * t2_5 + t4_5 * t2_4 + t4_6 * t2_3 + t4_7 * t2_2
	t6_10 = t4_4 * t2_6 + t4_5 * t2_5 + t4_6 * t2_4 + t4_7 * t2_3 + t4_8 * t2_2
	t7_8 = t6_6 * t1_2 + t6_7 * t1_1
	t7_10 = t6_6 * t1_4 + t6_7 * t1_3 + t6_8 * t1_2 + t6_9 * t1_1
	t8_8 = t6_6 * t2_2
	t8_9 = t6_6 * t2_3 + t6_7 * t2_2
	t8_10 = t6_6 * t2_4 + t6_7 * t2_3 + t6_8 * t2_2
	t9_10 = t8_8 * t1_2 + t8_9 * t1_1
	t10_10 = t8_8 * t2_2
        u = 1
        u -= (1./24) * t2_2 + (1./160) * t2_4 + (1./896) * t2_6 + (1./4608) * t2_8
        u += (1./1920) * t4_4 + (1./10752) * t4_6 + (1./55296) * t4_8 + (1./270336) * t4_10
        u -= (1./322560) * t6_6 + (1./1658880) * t6_8 + (1./8110080) * t6_10
        u += (1./92897280) * t8_8 + (1./454164480) * t8_10
        u -= 2.4464949595157930e-11 * t10_10
        v = (1./12) * t1_2 + (1./80) * t1_4
        v -= (1./480) * t3_4 + (1./2688) * t3_6 + (1./13824) * t3_8 + (1./67584) * t3_10
        v += (1./53760) * t5_6 + (1./276480) * t5_8 + (1./1351680) * t5_10
        v -= (1./11612160) * t7_8 + (1./56770560) * t7_10
        v += 2.4464949595157932e-10 * t9_10
	if n == 1:
	    x = u
	    y = v
        else:
	    th = (((th4 * s + th3) * s + th2) * s + th1) * s
	    cth = cos(th)
	    sth = sin(th)

	    x += cth * u - sth * v
	    y += cth * v + sth * u
	    s += ds
    return [x * ds, y * ds]

count_iter = 0

# Given th0 and th1 at endpoints (measured from chord), return k0
# and k1 such that the clothoid k(s) = k0 + k1 s, evaluated from
# s = -.5 to .5, has the tangents given
def solve_clothoid(th0, th1, verbose = False):
    global count_iter

    k1_old = 0
    e_old = th1 - th0
    k0 = th0 + th1
    k1 = 6 * (1 - ((.5 / pi) * k0) ** 3) * e_old

    # secant method
    for i in range(10):
	x, y = integ_spiro(k0, k1, 0, 0)
        e = (th1 - th0) + 2 * atan2(y, x) - .25 * k1
	count_iter += 1
        if verbose:
            print k0, k1, e
        if abs(e) < 1e-9: break
        k1_old, e_old, k1 = k1, e, k1 + (k1_old - k1) * e / (e - e_old)

    return k0, k1

def plot_by_thp():
    count = 0
    for i in range(11):
	thp = i * .1
	print .5 + .05 * i, .5, .5, 'setrgbcolor'
	print '.75 setlinewidth'
	cmd = 'moveto'
	for j in range(-40, 41):
	    thm = j * .02
	    k0, k1 = solve_clothoid(thp - thm, thp + thm, True)
	    count += 1
	    k1 = min(40, max(-40, k1))
	    print 306 + 75 * thm, 396 - 10 * k1, cmd
	    cmd = 'lineto'
	print 'stroke'
	print '% count_iter = ', count_iter, 'for', count

def plot_by_thm():
    print '.75 setlinewidth'
    print 36, 396 - 350, 'moveto'
    print 0, 700, 'rlineto stroke'
    for i in range(-10, 10):
	if i == 0: wid = 636
	else: wid = 5
	print 36, 396 - 10 * i, 'moveto', wid, '0 rlineto stroke'
    cmd = 'moveto'
    thm = -.1
    for i in range(41):
	thp = i * .1
	k0, k1 = solve_clothoid(thp - thm, thp + thm)
	print 36 + 150 * thp, 396 - 100 * k1, cmd
	cmd = 'lineto'
    print 'stroke'
    print '0 0 1 setrgbcolor'
    cmd = 'moveto'
    for i in range(41):
	thp = i * .1
	k1 = 12 * thm * cos(.5 * thp)
	k1 = 12 * thm * (1 - (thp / pi) ** 3)
	print 36 + 150 * thp, 396 - 100 * k1, cmd
	cmd = 'lineto'
    print 'stroke'

plot_by_thp()