291 lines
9.2 KiB
Python
Executable File
291 lines
9.2 KiB
Python
Executable File
#!/usr/bin/env python
|
|
'''
|
|
Copyright (C) 2010 Nick Drobchenko, nick@cnc-club.ru
|
|
Copyright (C) 2005 Aaron Spike, aaron@ekips.org
|
|
|
|
This program is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
'''
|
|
|
|
from __future__ import absolute_import
|
|
from __future__ import print_function
|
|
import math, cmath
|
|
|
|
def rootWrapper(a,b,c,d):
|
|
if a:
|
|
# Monics formula see http://en.wikipedia.org/wiki/Cubic_function#Monic_formula_of_roots
|
|
a,b,c = (b/a, c/a, d/a)
|
|
m = 2.0*a**3 - 9.0*a*b + 27.0*c
|
|
k = a**2 - 3.0*b
|
|
n = m**2 - 4.0*k**3
|
|
w1 = -.5 + .5*cmath.sqrt(-3.0)
|
|
w2 = -.5 - .5*cmath.sqrt(-3.0)
|
|
if n < 0:
|
|
m1 = pow(complex((m+cmath.sqrt(n))/2),1./3)
|
|
n1 = pow(complex((m-cmath.sqrt(n))/2),1./3)
|
|
else:
|
|
if m+math.sqrt(n) < 0:
|
|
m1 = -pow(-(m+math.sqrt(n))/2,1./3)
|
|
else:
|
|
m1 = pow((m+math.sqrt(n))/2,1./3)
|
|
if m-math.sqrt(n) < 0:
|
|
n1 = -pow(-(m-math.sqrt(n))/2,1./3)
|
|
else:
|
|
n1 = pow((m-math.sqrt(n))/2,1./3)
|
|
x1 = -1./3 * (a + m1 + n1)
|
|
x2 = -1./3 * (a + w1*m1 + w2*n1)
|
|
x3 = -1./3 * (a + w2*m1 + w1*n1)
|
|
return (x1,x2,x3)
|
|
elif b:
|
|
det=c**2.0-4.0*b*d
|
|
if det:
|
|
return (-c+cmath.sqrt(det))/(2.0*b),(-c-cmath.sqrt(det))/(2.0*b)
|
|
else:
|
|
return -c/(2.0*b),
|
|
elif c:
|
|
return 1.0*(-d/c),
|
|
return ()
|
|
|
|
def bezierparameterize(xxx_todo_changeme):
|
|
#parametric bezier
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme
|
|
x0=bx0
|
|
y0=by0
|
|
cx=3*(bx1-x0)
|
|
bx=3*(bx2-bx1)-cx
|
|
ax=bx3-x0-cx-bx
|
|
cy=3*(by1-y0)
|
|
by=3*(by2-by1)-cy
|
|
ay=by3-y0-cy-by
|
|
|
|
return ax,ay,bx,by,cx,cy,x0,y0
|
|
#ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
|
|
def linebezierintersect(xxx_todo_changeme1, xxx_todo_changeme2):
|
|
#parametric line
|
|
((lx1,ly1),(lx2,ly2)) = xxx_todo_changeme1
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme2
|
|
dd=lx1
|
|
cc=lx2-lx1
|
|
bb=ly1
|
|
aa=ly2-ly1
|
|
|
|
if aa:
|
|
coef1=cc/aa
|
|
coef2=1
|
|
else:
|
|
coef1=1
|
|
coef2=aa/cc
|
|
|
|
ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
#cubic intersection coefficients
|
|
a=coef1*ay-coef2*ax
|
|
b=coef1*by-coef2*bx
|
|
c=coef1*cy-coef2*cx
|
|
d=coef1*(y0-bb)-coef2*(x0-dd)
|
|
|
|
roots = rootWrapper(a,b,c,d)
|
|
retval = []
|
|
for i in roots:
|
|
if type(i) is complex and i.imag==0:
|
|
i = i.real
|
|
if type(i) is not complex and 0<=i<=1:
|
|
retval.append(bezierpointatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),i))
|
|
return retval
|
|
|
|
def bezierpointatt(xxx_todo_changeme3,t):
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme3
|
|
ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
x=ax*(t**3)+bx*(t**2)+cx*t+x0
|
|
y=ay*(t**3)+by*(t**2)+cy*t+y0
|
|
return x,y
|
|
|
|
def bezierslopeatt(xxx_todo_changeme4,t):
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme4
|
|
ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
dx=3*ax*(t**2)+2*bx*t+cx
|
|
dy=3*ay*(t**2)+2*by*t+cy
|
|
return dx,dy
|
|
|
|
def beziertatslope(xxx_todo_changeme5, xxx_todo_changeme6):
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme5
|
|
(dy,dx) = xxx_todo_changeme6
|
|
ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
#quadratic coefficents of slope formula
|
|
if dx:
|
|
slope = 1.0*(dy/dx)
|
|
a=3*ay-3*ax*slope
|
|
b=2*by-2*bx*slope
|
|
c=cy-cx*slope
|
|
elif dy:
|
|
slope = 1.0*(dx/dy)
|
|
a=3*ax-3*ay*slope
|
|
b=2*bx-2*by*slope
|
|
c=cx-cy*slope
|
|
else:
|
|
return []
|
|
|
|
roots = rootWrapper(0,a,b,c)
|
|
retval = []
|
|
for i in roots:
|
|
if type(i) is complex and i.imag==0:
|
|
i = i.real
|
|
if type(i) is not complex and 0<=i<=1:
|
|
retval.append(i)
|
|
return retval
|
|
|
|
def tpoint(xxx_todo_changeme7, xxx_todo_changeme8,t):
|
|
(x1,y1) = xxx_todo_changeme7
|
|
(x2,y2) = xxx_todo_changeme8
|
|
return x1+t*(x2-x1),y1+t*(y2-y1)
|
|
def beziersplitatt(xxx_todo_changeme9,t):
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme9
|
|
m1=tpoint((bx0,by0),(bx1,by1),t)
|
|
m2=tpoint((bx1,by1),(bx2,by2),t)
|
|
m3=tpoint((bx2,by2),(bx3,by3),t)
|
|
m4=tpoint(m1,m2,t)
|
|
m5=tpoint(m2,m3,t)
|
|
m=tpoint(m4,m5,t)
|
|
|
|
return ((bx0,by0),m1,m4,m),(m,m5,m3,(bx3,by3))
|
|
|
|
'''
|
|
Approximating the arc length of a bezier curve
|
|
according to <http://www.cit.gu.edu.au/~anthony/info/graphics/bezier.curves>
|
|
|
|
if:
|
|
L1 = |P0 P1| +|P1 P2| +|P2 P3|
|
|
L0 = |P0 P3|
|
|
then:
|
|
L = 1/2*L0 + 1/2*L1
|
|
ERR = L1-L0
|
|
ERR approaches 0 as the number of subdivisions (m) increases
|
|
2^-4m
|
|
|
|
Reference:
|
|
Jens Gravesen <gravesen@mat.dth.dk>
|
|
"Adaptive subdivision and the length of Bezier curves"
|
|
mat-report no. 1992-10, Mathematical Institute, The Technical
|
|
University of Denmark.
|
|
'''
|
|
def pointdistance(xxx_todo_changeme10, xxx_todo_changeme11):
|
|
(x1,y1) = xxx_todo_changeme10
|
|
(x2,y2) = xxx_todo_changeme11
|
|
return math.sqrt(((x2 - x1) ** 2) + ((y2 - y1) ** 2))
|
|
def Gravesen_addifclose(b, len, error = 0.001):
|
|
box = 0
|
|
for i in range(1,4):
|
|
box += pointdistance(b[i-1], b[i])
|
|
chord = pointdistance(b[0], b[3])
|
|
if (box - chord) > error:
|
|
first, second = beziersplitatt(b, 0.5)
|
|
Gravesen_addifclose(first, len, error)
|
|
Gravesen_addifclose(second, len, error)
|
|
else:
|
|
len[0] += (box / 2.0) + (chord / 2.0)
|
|
def bezierlengthGravesen(b, error = 0.001):
|
|
len = [0]
|
|
Gravesen_addifclose(b, len, error)
|
|
return len[0]
|
|
|
|
# balf = Bezier Arc Length Function
|
|
balfax,balfbx,balfcx,balfay,balfby,balfcy = 0,0,0,0,0,0
|
|
def balf(t):
|
|
retval = (balfax*(t**2) + balfbx*t + balfcx)**2 + (balfay*(t**2) + balfby*t + balfcy)**2
|
|
return math.sqrt(retval)
|
|
|
|
def Simpson(f, a, b, n_limit, tolerance):
|
|
n = 2
|
|
multiplier = (b - a)/6.0
|
|
endsum = f(a) + f(b)
|
|
interval = (b - a)/2.0
|
|
asum = 0.0
|
|
bsum = f(a + interval)
|
|
est1 = multiplier * (endsum + (2.0 * asum) + (4.0 * bsum))
|
|
est0 = 2.0 * est1
|
|
#print multiplier, endsum, interval, asum, bsum, est1, est0
|
|
while n < n_limit and abs(est1 - est0) > tolerance:
|
|
n *= 2
|
|
multiplier /= 2.0
|
|
interval /= 2.0
|
|
asum += bsum
|
|
bsum = 0.0
|
|
est0 = est1
|
|
for i in range(1, n, 2):
|
|
bsum += f(a + (i * interval))
|
|
est1 = multiplier * (endsum + (2.0 * asum) + (4.0 * bsum))
|
|
#print multiplier, endsum, interval, asum, bsum, est1, est0
|
|
return est1
|
|
|
|
def bezierlengthSimpson(xxx_todo_changeme12, tolerance = 0.001):
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme12
|
|
global balfax,balfbx,balfcx,balfay,balfby,balfcy
|
|
ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
balfax,balfbx,balfcx,balfay,balfby,balfcy = 3*ax,2*bx,cx,3*ay,2*by,cy
|
|
return Simpson(balf, 0.0, 1.0, 4096, tolerance)
|
|
|
|
def beziertatlength(xxx_todo_changeme13, l = 0.5, tolerance = 0.001):
|
|
((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)) = xxx_todo_changeme13
|
|
global balfax,balfbx,balfcx,balfay,balfby,balfcy
|
|
ax,ay,bx,by,cx,cy,x0,y0=bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)))
|
|
balfax,balfbx,balfcx,balfay,balfby,balfcy = 3*ax,2*bx,cx,3*ay,2*by,cy
|
|
t = 1.0
|
|
tdiv = t
|
|
curlen = Simpson(balf, 0.0, t, 4096, tolerance)
|
|
targetlen = l * curlen
|
|
diff = curlen - targetlen
|
|
while abs(diff) > tolerance:
|
|
tdiv /= 2.0
|
|
if diff < 0:
|
|
t += tdiv
|
|
else:
|
|
t -= tdiv
|
|
curlen = Simpson(balf, 0.0, t, 4096, tolerance)
|
|
diff = curlen - targetlen
|
|
return t
|
|
|
|
#default bezier length method
|
|
bezierlength = bezierlengthSimpson
|
|
|
|
if __name__ == '__main__':
|
|
# import timing
|
|
#print linebezierintersect(((,),(,)),((,),(,),(,),(,)))
|
|
#print linebezierintersect(((0,1),(0,-1)),((-1,0),(-.5,0),(.5,0),(1,0)))
|
|
tol = 0.00000001
|
|
curves = [((0,0),(1,5),(4,5),(5,5)),
|
|
((0,0),(0,0),(5,0),(10,0)),
|
|
((0,0),(0,0),(5,1),(10,0)),
|
|
((-10,0),(0,0),(10,0),(10,10)),
|
|
((15,10),(0,0),(10,0),(-5,10))]
|
|
'''
|
|
for curve in curves:
|
|
timing.start()
|
|
g = bezierlengthGravesen(curve,tol)
|
|
timing.finish()
|
|
gt = timing.micro()
|
|
|
|
timing.start()
|
|
s = bezierlengthSimpson(curve,tol)
|
|
timing.finish()
|
|
st = timing.micro()
|
|
|
|
print g, gt
|
|
print s, st
|
|
'''
|
|
for curve in curves:
|
|
print(beziertatlength(curve,0.5))
|
|
|
|
|
|
# vim: expandtab shiftwidth=4 tabstop=8 softtabstop=4 encoding=utf-8 textwidth=99
|