import math, cmath

def rootWrapper(a,b,c,d):

if a:

# Monics formula see http://en.wikipedia.org/wiki/Cubic_function#Monic_formula_of_roots

a,b,c = (b/a, c/a, d/a)

m = 2.0*a**3 - 9.0*a*b + 27.0*c

k = a**2 - 3.0*b

n = m**2 - 4.0*k**3

w1 = -.5 + .5*cmath.sqrt(-3.0)

w2 = -.5 - .5*cmath.sqrt(-3.0)

if n < 0:

m1 = pow(complex((m+cmath.sqrt(n))/2),1./3)

n1 = pow(complex((m-cmath.sqrt(n))/2),1./3)

else:

if m+math.sqrt(n) < 0:

m1 = -pow(-(m+math.sqrt(n))/2,1./3)

else:

m1 = pow((m+math.sqrt(n))/2,1./3)

if m-math.sqrt(n) < 0:

n1 = -pow(-(m-math.sqrt(n))/2,1./3)

else:

n1 = pow((m-math.sqrt(n))/2,1./3)

x1 = -1./3 * (a + m1 + n1)

x2 = -1./3 * (a + w1*m1 + w2*n1)

x3 = -1./3 * (a + w2*m1 + w1*n1)

return (x1,x2,x3)

elif b:

det=c**2.0-4.0*b*d

if det:

return (-c+cmath.sqrt(det))/(2.0*b),(-c-cmath.sqrt(det))/(2.0*b)

else:

return -c/(2.0*b),

elif c:

return 1.0*(-d/c),

return ()

def bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3))):

#parametric bezier

x0=bx0

y0=by0

cx=3*(bx1-x0)

bx=3*(bx2-bx1)-cx

ax=bx3-x0-cx-bx

cy=3*(by1-y0)

by=3*(by2-by1)-cy

ay=by3-y0-cy-by

return ax,ay,bx,by,cx,cy,x0,y0

#ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

def linebezierintersect(((lx1,ly1),(lx2,ly2)),((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3))):

#parametric line

dd=lx1

cc=lx2-lx1

bb=ly1

aa=ly2-ly1

if aa:

coef1=cc/aa

coef2=1

else:

coef1=1

coef2=aa/cc

ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

#cubic intersection coefficients

a=coef1*ay-coef2*ax

b=coef1*by-coef2*bx

c=coef1*cy-coef2*cx

d=coef1*(y0-bb)-coef2*(x0-dd)

roots = rootWrapper(a,b,c,d)

retval = []

for i in roots:

if type(i) is complex and i.imag==0:

i = i.real

if type(i) is not complex and 0<=i<=1:

retval.append(bezierpointatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),i))

return retval

def bezierpointatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),t):

ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

x=ax*(t**3)+bx*(t**2)+cx*t+x0

y=ay*(t**3)+by*(t**2)+cy*t+y0

return x,y

def bezierslopeatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),t):

ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

dx=3*ax*(t**2)+2*bx*t+cx

dy=3*ay*(t**2)+2*by*t+cy

return dx,dy

def beziertatslope(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),(dy,dx)):

ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

#quadratic coefficents of slope formula

if dx:

slope = 1.0*(dy/dx)

a=3*ay-3*ax*slope

b=2*by-2*bx*slope

c=cy-cx*slope

elif dy:

slope = 1.0*(dx/dy)

a=3*ax-3*ay*slope

b=2*bx-2*by*slope

c=cx-cy*slope

else:

return []

roots = rootWrapper(0,a,b,c)

retval = []

for i in roots:

if type(i) is complex and i.imag==0:

i = i.real

if type(i) is not complex and 0<=i<=1:

retval.append(i)

return retval

def tpoint((x1,y1),(x2,y2),t):

return x1+t*(x2-x1),y1+t*(y2-y1)

def beziersplitatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),t):

m1=tpoint((bx0,by0),(bx1,by1),t)

m2=tpoint((bx1,by1),(bx2,by2),t)

m3=tpoint((bx2,by2),(bx3,by3),t)

m4=tpoint(m1,m2,t)

m5=tpoint(m2,m3,t)

m=tpoint(m4,m5,t)

return ((bx0,by0),m1,m4,m),(m,m5,m3,(bx3,by3))

def pointdistance((x1,y1),(x2,y2)):

return math.sqrt(((x2 - x1) ** 2) + ((y2 - y1) ** 2))

def Gravesen_addifclose(b, len, error = 0.001):

box = 0

for i in range(1,4):

box += pointdistance(b[i-1], b[i])

chord = pointdistance(b[0], b[3])

if (box - chord) > error:

first, second = beziersplitatt(b, 0.5)

Gravesen_addifclose(first, len, error)

Gravesen_addifclose(second, len, error)

else:

len[0] += (box / 2.0) + (chord / 2.0)

def bezierlengthGravesen(b, error = 0.001):

len = [0]

Gravesen_addifclose(b, len, error)

return len[0]

balfax,balfbx,balfcx,balfay,balfby,balfcy = 0,0,0,0,0,0

def balf(t):

retval = (balfax*(t**2) + balfbx*t + balfcx)**2 + (balfay*(t**2) + balfby*t + balfcy)**2

return math.sqrt(retval)

def Simpson(f, a, b, n_limit, tolerance):

n = 2

multiplier = (b - a)/6.0

endsum = f(a) + f(b)

interval = (b - a)/2.0

asum = 0.0

bsum = f(a + interval)

est1 = multiplier * (endsum + (2.0 * asum) + (4.0 * bsum))

est0 = 2.0 * est1

#print multiplier, endsum, interval, asum, bsum, est1, est0

while n < n_limit and abs(est1 - est0) > tolerance:

n *= 2

multiplier /= 2.0

interval /= 2.0

asum += bsum

bsum = 0.0

est0 = est1

for i in xrange(1, n, 2):

bsum += f(a + (i * interval))

est1 = multiplier * (endsum + (2.0 * asum) + (4.0 * bsum))

#print multiplier, endsum, interval, asum, bsum, est1, est0

return est1

def bezierlengthSimpson(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)), tolerance = 0.001):

global balfax,balfbx,balfcx,balfay,balfby,balfcy

ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

balfax,balfbx,balfcx,balfay,balfby,balfcy = 3*ax,2*bx,cx,3*ay,2*by,cy

return Simpson(balf, 0.0, 1.0, 4096, tolerance)

def beziertatlength(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)), l = 0.5, tolerance = 0.001):

global balfax,balfbx,balfcx,balfay,balfby,balfcy

ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))

balfax,balfbx,balfcx,balfay,balfby,balfcy = 3*ax,2*bx,cx,3*ay,2*by,cy

t = 1.0

tdiv = t

curlen = Simpson(balf, 0.0, t, 4096, tolerance)

targetlen = l * curlen

diff = curlen - targetlen

while abs(diff) > tolerance:

tdiv /= 2.0

if diff < 0:

t += tdiv

else:

t -= tdiv

curlen = Simpson(balf, 0.0, t, 4096, tolerance)

diff = curlen - targetlen

return t

bezierlength = bezierlengthSimpson

if __name__ == '__main__':

import timing

#print linebezierintersect(((,),(,)),((,),(,),(,),(,)))

#print linebezierintersect(((0,1),(0,-1)),((-1,0),(-.5,0),(.5,0),(1,0)))

tol = 0.00000001

curves = [((0,0),(1,5),(4,5),(5,5)),

((0,0),(0,0),(5,0),(10,0)),

((0,0),(0,0),(5,1),(10,0)),

((-10,0),(0,0),(10,0),(10,10)),

((15,10),(0,0),(10,0),(-5,10))]

'''

for curve in curves:

print beziertatlength(curve,0.5)