|
| 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(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 | + |
| 73 | +def 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 | + |
| 105 | +def 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 | + |
| 112 | +def 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 | + |
| 119 | +def 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 | + |
| 146 | +def 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) |
| 150 | +def 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 | +''' |
| 162 | +Approximating the arc length of a bezier curve |
| 163 | +according to <http://www.cit.gu.edu.au/~anthony/info/graphics/bezier.curves> |
| 164 | +
|
| 165 | +if: |
| 166 | + L1 = |P0 P1| +|P1 P2| +|P2 P3| |
| 167 | + L0 = |P0 P3| |
| 168 | +then: |
| 169 | + L = 1/2*L0 + 1/2*L1 |
| 170 | + ERR = L1-L0 |
| 171 | +ERR approaches 0 as the number of subdivisions (m) increases |
| 172 | + 2^-4m |
| 173 | +
|
| 174 | +Reference: |
| 175 | +Jens Gravesen <gravesen@mat.dth.dk> |
| 176 | +"Adaptive subdivision and the length of Bezier curves" |
| 177 | +mat-report no. 1992-10, Mathematical Institute, The Technical |
| 178 | +University of Denmark. |
| 179 | +''' |
| 180 | +def 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)) |
| 184 | +def 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) |
| 195 | +def 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 |
| 201 | +balfax,balfbx,balfcx,balfay,balfby,balfcy = 0,0,0,0,0,0 |
| 202 | +def balf(t): |
| 203 | + retval = (balfax*(t**2) + balfbx*t + balfcx)**2 + (balfay*(t**2) + balfby*t + balfcy)**2 |
| 204 | + return math.sqrt(retval) |
| 205 | + |
| 206 | +def 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 | + |
| 229 | +def 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 | + |
| 236 | +def 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 |
| 257 | +bezierlength = bezierlengthSimpson |
| 258 | + |
| 259 | +if __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 encoding=utf-8 textwidth=99 |
0 commit comments