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