share/extensions/bezmisc.py

   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 fileencoding=utf-8 textwidth=99