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