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