~vaifrax/inkscape/bugfix170049

#!/usr/bin/env python
'''
Copyright (C) 2005 Aaron Spike, aaron@ekips.org

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
'''

import math, cmath

def rootWrapper(a,b,c,d):
    if a:
	#TODO: find a new cubic solver and put it here
      	#return solveCubicMonic(b/a,c/a,d/a)
        return ()
    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(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))

'''
Approximating the arc length of a bezier curve
according to <http://www.cit.gu.edu.au/~anthony/info/graphics/bezier.curves>

if:
    L1 = |P0 P1| +|P1 P2| +|P2 P3| 
    L0 = |P0 P3|
then: 
    L = 1/2*L0 + 1/2*L1
    ERR = L1-L0
ERR approaches 0 as the number of subdivisions (m) increases
    2^-4m

Reference:
Jens Gravesen <gravesen@mat.dth.dk>
"Adaptive subdivision and the length of Bezier curves"
mat-report no. 1992-10, Mathematical Institute, The Technical
University of Denmark. 
'''
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]

# balf = Bezier Arc Length Function
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

#default bezier length method
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:
		timing.start()
		g = bezierlengthGravesen(curve,tol)
		timing.finish()
		gt = timing.micro()

		timing.start()
		s = bezierlengthSimpson(curve,tol)
		timing.finish()
		st = timing.micro()

		print g, gt
		print s, st
	'''
	for curve in curves:
		print beziertatlength(curve,0.5)