Mercurial > hg-public > lasercut-scripts / file comparison
comparison: printrun-src/printrun/svg2gcode/bezmisc.py
printrun-src/printrun/svg2gcode/bezmisc.py
- changeset 16
- 36d478bde840
equal
deleted
inserted
replaced
| 15:0bbb006204fc | 16:36d478bde840 |
|---|---|
| 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 |
