share/extensions/bezmisc.py

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