1#!/usr/local/bin/python3.8
2'''
3Copyright (C) 2010 Nick Drobchenko, nick@cnc-club.ru
4Copyright (C) 2005 Aaron Spike, aaron@ekips.org
5
6This program is free software; you can redistribute it and/or modify
7it under the terms of the GNU General Public License as published by
8the Free Software Foundation; either version 2 of the License, or
9(at your option) any later version.
10
11This program is distributed in the hope that it will be useful,
12but WITHOUT ANY WARRANTY; without even the implied warranty of
13MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14GNU General Public License for more details.
15
16You should have received a copy of the GNU General Public License
17along with this program; if not, write to the Free Software
18Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
19'''
20
21import math, cmath
22
23def rootWrapper(a,b,c,d):
24    if a:
25        # Monics formula see http://en.wikipedia.org/wiki/Cubic_function#Monic_formula_of_roots
26        a,b,c = (b/a, c/a, d/a)
27        m = 2.0*a**3 - 9.0*a*b + 27.0*c
28        k = a**2 - 3.0*b
29        n = m**2 - 4.0*k**3
30        w1 = -.5 + .5*cmath.sqrt(-3.0)
31        w2 = -.5 - .5*cmath.sqrt(-3.0)
32        if n < 0:
33            m1 = pow(complex((m+cmath.sqrt(n))/2),1./3)
34            n1 = pow(complex((m-cmath.sqrt(n))/2),1./3)
35        else:
36            if m+math.sqrt(n) < 0:
37                m1 = -pow(-(m+math.sqrt(n))/2,1./3)
38            else:
39                m1 = pow((m+math.sqrt(n))/2,1./3)
40            if m-math.sqrt(n) < 0:
41                n1 = -pow(-(m-math.sqrt(n))/2,1./3)
42            else:
43                n1 = pow((m-math.sqrt(n))/2,1./3)
44        x1 = -1./3 * (a + m1 + n1)
45        x2 = -1./3 * (a + w1*m1 + w2*n1)
46        x3 = -1./3 * (a + w2*m1 + w1*n1)
47        return (x1,x2,x3)
48    elif b:
49        det=c**2.0-4.0*b*d
50        if det:
51            return (-c+cmath.sqrt(det))/(2.0*b),(-c-cmath.sqrt(det))/(2.0*b)
52        else:
53            return -c/(2.0*b),
54    elif c:
55        return 1.0*(-d/c),
56    return ()
57
58def bezierparameterize(xxx_todo_changeme):
59    #parametric bezier
60    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme
61    x0=bx0
62    y0=by0
63    cx=3*(bx1-x0)
64    bx=3*(bx2-bx1)-cx
65    ax=bx3-x0-cx-bx
66    cy=3*(by1-y0)
67    by=3*(by2-by1)-cy
68    ay=by3-y0-cy-by
69
70    return ax,ay,bx,by,cx,cy,x0,y0
71    #ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
72
73def linebezierintersect(xxx_todo_changeme1, xxx_todo_changeme2):
74    #parametric line
75    ((lx1,ly1),(lx2,ly2)) = xxx_todo_changeme1
76    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme2
77    dd=lx1
78    cc=lx2-lx1
79    bb=ly1
80    aa=ly2-ly1
81
82    if aa:
83        coef1=cc/aa
84        coef2=1
85    else:
86        coef1=1
87        coef2=aa/cc
88
89    ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
90    #cubic intersection coefficients
91    a=coef1*ay-coef2*ax
92    b=coef1*by-coef2*bx
93    c=coef1*cy-coef2*cx
94    d=coef1*(y0-bb)-coef2*(x0-dd)
95
96    roots = rootWrapper(a,b,c,d)
97    retval = []
98    for i in roots:
99        if type(i) is complex and i.imag==0:
100            i = i.real
101        if type(i) is not complex and 0<=i<=1:
102            retval.append(bezierpointatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),i))
103    return retval
104
105def bezierpointatt(xxx_todo_changeme3,t):
106    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme3
107    ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
108    x=ax*(t**3)+bx*(t**2)+cx*t+x0
109    y=ay*(t**3)+by*(t**2)+cy*t+y0
110    return x,y
111
112def bezierslopeatt(xxx_todo_changeme4,t):
113    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme4
114    ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
115    dx=3*ax*(t**2)+2*bx*t+cx
116    dy=3*ay*(t**2)+2*by*t+cy
117    return dx,dy
118
119def beziertatslope(xxx_todo_changeme5, xxx_todo_changeme6):
120    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme5
121    (dy,dx) = xxx_todo_changeme6
122    ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
123    #quadratic coefficents of slope formula
124    if dx:
125        slope = 1.0*(dy/dx)
126        a=3*ay-3*ax*slope
127        b=2*by-2*bx*slope
128        c=cy-cx*slope
129    elif dy:
130        slope = 1.0*(dx/dy)
131        a=3*ax-3*ay*slope
132        b=2*bx-2*by*slope
133        c=cx-cy*slope
134    else:
135        return []
136
137    roots = rootWrapper(0,a,b,c)
138    retval = []
139    for i in roots:
140        if type(i) is complex and i.imag==0:
141            i = i.real
142        if type(i) is not complex and 0<=i<=1:
143            retval.append(i)
144    return retval
145
146def tpoint(xxx_todo_changeme7, xxx_todo_changeme8,t):
147    (x1,y1) = xxx_todo_changeme7
148    (x2,y2) = xxx_todo_changeme8
149    return x1+t*(x2-x1),y1+t*(y2-y1)
150def beziersplitatt(xxx_todo_changeme9,t):
151    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme9
152    m1=tpoint((bx0,by0),(bx1,by1),t)
153    m2=tpoint((bx1,by1),(bx2,by2),t)
154    m3=tpoint((bx2,by2),(bx3,by3),t)
155    m4=tpoint(m1,m2,t)
156    m5=tpoint(m2,m3,t)
157    m=tpoint(m4,m5,t)
158
159    return ((bx0,by0),m1,m4,m),(m,m5,m3,(bx3,by3))
160
161'''
162Approximating the arc length of a bezier curve
163according to <http://www.cit.gu.edu.au/~anthony/info/graphics/bezier.curves>
164
165if:
166    L1 = |P0 P1| +|P1 P2| +|P2 P3|
167    L0 = |P0 P3|
168then:
169    L = 1/2*L0 + 1/2*L1
170    ERR = L1-L0
171ERR approaches 0 as the number of subdivisions (m) increases
172    2^-4m
173
174Reference:
175Jens Gravesen <gravesen@mat.dth.dk>
176"Adaptive subdivision and the length of Bezier curves"
177mat-report no. 1992-10, Mathematical Institute, The Technical
178University of Denmark.
179'''
180def pointdistance(xxx_todo_changeme10, xxx_todo_changeme11):
181    (x1,y1) = xxx_todo_changeme10
182    (x2,y2) = xxx_todo_changeme11
183    return math.sqrt(((x2 - x1) ** 2) + ((y2 - y1) ** 2))
184def Gravesen_addifclose(b, len, error = 0.001):
185    box = 0
186    for i in range(1,4):
187        box += pointdistance(b[i-1], b[i])
188    chord = pointdistance(b[0], b[3])
189    if (box - chord) > error:
190        first, second = beziersplitatt(b, 0.5)
191        Gravesen_addifclose(first, len, error)
192        Gravesen_addifclose(second, len, error)
193    else:
194        len[0] += (box / 2.0) + (chord / 2.0)
195def bezierlengthGravesen(b, error = 0.001):
196    len = [0]
197    Gravesen_addifclose(b, len, error)
198    return len[0]
199
200# balf = Bezier Arc Length Function
201balfax,balfbx,balfcx,balfay,balfby,balfcy = 0,0,0,0,0,0
202def balf(t):
203    retval = (balfax*(t**2) + balfbx*t + balfcx)**2 + (balfay*(t**2) + balfby*t + balfcy)**2
204    return math.sqrt(retval)
205
206def Simpson(f, a, b, n_limit, tolerance):
207    n = 2
208    multiplier = (b - a)/6.0
209    endsum = f(a) + f(b)
210    interval = (b - a)/2.0
211    asum = 0.0
212    bsum = f(a + interval)
213    est1 = multiplier * (endsum + (2.0 * asum) + (4.0 * bsum))
214    est0 = 2.0 * est1
215    #print multiplier, endsum, interval, asum, bsum, est1, est0
216    while n < n_limit and abs(est1 - est0) > tolerance:
217        n *= 2
218        multiplier /= 2.0
219        interval /= 2.0
220        asum += bsum
221        bsum = 0.0
222        est0 = est1
223        for i in range(1, n, 2):
224            bsum += f(a + (i * interval))
225            est1 = multiplier * (endsum + (2.0 * asum) + (4.0 * bsum))
226    #print multiplier, endsum, interval, asum, bsum, est1, est0
227    return est1
228
229def bezierlengthSimpson(xxx_todo_changeme12, tolerance = 0.001):
230    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme12
231    global balfax,balfbx,balfcx,balfay,balfby,balfcy
232    ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
233    balfax,balfbx,balfcx,balfay,balfby,balfcy = 3*ax,2*bx,cx,3*ay,2*by,cy
234    return Simpson(balf, 0.0, 1.0, 4096, tolerance)
235
236def beziertatlength(xxx_todo_changeme13, l = 0.5, tolerance = 0.001):
237    ((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme13
238    global balfax,balfbx,balfcx,balfay,balfby,balfcy
239    ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
240    balfax,balfbx,balfcx,balfay,balfby,balfcy = 3*ax,2*bx,cx,3*ay,2*by,cy
241    t = 1.0
242    tdiv = t
243    curlen = Simpson(balf, 0.0, t, 4096, tolerance)
244    targetlen = l * curlen
245    diff = curlen - targetlen
246    while abs(diff) > tolerance:
247        tdiv /= 2.0
248        if diff < 0:
249            t += tdiv
250        else:
251            t -= tdiv
252        curlen = Simpson(balf, 0.0, t, 4096, tolerance)
253        diff = curlen - targetlen
254    return t
255
256#default bezier length method
257bezierlength = bezierlengthSimpson
258
259if __name__ == '__main__':
260    import timing
261    #print linebezierintersect(((,),(,)),((,),(,),(,),(,)))
262    #print linebezierintersect(((0,1),(0,-1)),((-1,0),(-.5,0),(.5,0),(1,0)))
263    tol = 0.00000001
264    curves = [((0,0),(1,5),(4,5),(5,5)),
265            ((0,0),(0,0),(5,0),(10,0)),
266            ((0,0),(0,0),(5,1),(10,0)),
267            ((-10,0),(0,0),(10,0),(10,10)),
268            ((15,10),(0,0),(10,0),(-5,10))]
269    '''
270    for curve in curves:
271        timing.start()
272        g = bezierlengthGravesen(curve,tol)
273        timing.finish()
274        gt = timing.micro()
275
276        timing.start()
277        s = bezierlengthSimpson(curve,tol)
278        timing.finish()
279        st = timing.micro()
280
281        print g, gt
282        print s, st
283    '''
284    for curve in curves:
285        print(beziertatlength(curve,0.5))
286
287
288# vim: expandtab shiftwidth=4 tabstop=8 softtabstop=4 fileencoding=utf-8 textwidth=99
289