tip/printrun-src/printrun/svg2gcode/bezmisc.py

#!/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
'''

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(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3))):
    #parametric bezier
    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(((lx1,ly1),(lx2,ly2)),((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3))):
    #parametric line
    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(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),t):
    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(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),t):
    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(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),(dy,dx)):
    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((x1,y1),(x2,y2),t):
    return x1+t*(x2-x1),y1+t*(y2-y1)
def beziersplitatt(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)),t):
    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((x1,y1),(x2,y2)):
    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 xrange(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(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)), tolerance = 0.001):
    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(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3)), l = 0.5, tolerance = 0.001):
    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