#!/usr/bin/python # planar spline to arc conversion # Copyright 2007 Jeff Epler # # 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 class P: def __init__(self, x, y): self.x, self.y = x, y def __add__(self, other): return P(self.x + other.x, self.y + other.y) def __sub__(self, other): return P(self.x - other.x, self.y - other.y) def __mul__(self, other): if isinstance(other, P): return self.x * other.x + self.y * other.y return P(self.x * other, self.y * other) __rmul__ = __mul__ def __div__(self, other): return P(self.x / other, self.y / other) def mag(self): return math.hypot(self.x, self.y) def unit(self): h = self.mag() if h: return self / h else: return P(0,0) def dot(self, other): return self.x * other.x + self.y * other.y def rot(self, theta): c = math.cos(theta) s = math.sin(theta) return P(self.x * c - self.y * s, self.x * s + self.y * c) def angle(self): return math.atan2(self.y, self.x) def __repr__(self): return "<%.2f %.2f>" % (self.x, self.y) def biarc(P0, TS, P4, TE, r): TS = TS.unit() TE = TE.unit() v = P0 - P4 c = v*v b = 2*v*(r*TS+TE) a = 2*r*(TS*TE-1) if a == 0: raise ValueError, (a,b,c) discr = b*b-4*a*c if discr < 0: raise ValueError, (a,b,c,discr) disq = discr**.5 beta1 = (-b - disq) / 2 / a beta2 = (-b + disq) / 2 / a #print (beta1, beta2) if beta1 > 0 and beta2 > 0: raise ValueError, (a,b,c,disq,beta1,beta2) beta = max(beta1, beta2) if beta < 0: raise ValueError, (a,b,c,disq,beta) alpha = beta * r ab = alpha + beta P1 = P0 + alpha * TS P3 = P4 - beta * TE P2 = (beta / ab) * P1 + (alpha / ab) * P3 #print "(%f %f %f %f)" % (a, b, c, discr) #print "(%f %f %f %f)" % (P2.x, P2.y, alpha, beta) return P1, P2, P3, alpha, beta small = 1e-10 def unit(a,b): h = math.hypot(a, b) if h: return a/h, b/h else: return a, b def arc(x1, y1, x2, y2, dx, dy): """\ r(x1,y1, x2,y2, dx,dy) -> dx, dy Given the initial location (x1,y1) the desired location (x2,y2), and the direction of travel at the initial location (dx,dy), find the line or arc satisfying these conditions and output a line of g-code to perform that arc In the case that the target point is exactly in the opposite direction of the initial direction of motion, the tangency condition cannot be maintained. In this case, the points are joined with a line that violates the tangency condition. Returns the direction of travel at the final location """ dx, dy = unit(dx, dy) #print "(r %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f)" % (x1, y1, x2, y2, dx, dy), x = x2-x1 y = y2-y1 den = 2*(y*dx - x*dy) if abs(den) < small: # Line print "G1 X%.4f Y%.4f" % (x2, y2) dx = x2-x1; dy = y2-y1 u = math.hypot(dx, dy) return unit(x2-x1, y2-y1) r = -(x**2 + y**2) / den i = dy * r j = -dx * r cx = x1 + i cy = y1 + j if r < 0: # Counterclockwise print "G3 I%.4f J%.4f X%.4f Y%.4f" % (i, j, x2, y2) return unit(-(y2-cy), x2-cx) else: # Clockwise print "G2 I%.4f J%.4f X%.4f Y%.4f" % (i, j, x2, y2) return unit(y2-cy, -(x2-cx)) def giarc(PS, TS, PE, TE, r=1): TS = TS.unit() TE = TE.unit() P1, P2, P3, alpha, beta = biarc(PS, TS, PE, TE, r) if 0: print "G0 X%.4f Y%.4f" % (P1.x, P1.y) print "G0 X%.4f Y%.4f" % (P2.x, P2.y) print "G4 P0" print "G0 X%.4f Y%.4f" % (P3.x, P3.y) print "G0 X%.4f Y%.4f" % (PE.x, PE.y) print "G0 X%.4f Y%.4f" % (PS.x, PS.y) arc(PS.x, PS.y, P2.x, P2.y, TS.x, TS.y) arc(P2.x, P2.y, PE.x, PE.y, P3.x-P2.x, P3.y-P2.y) def spline(P0, P1, P2, P3, n=5): def _spline(t): return P0*(1-t)**3 + 3*P1*t*(1-t)**2 + 3*P2*t**2*(1-t) + P3*t**3 def _tan(t): return Q0*(1-t)**2 + 2*Q1*t*(1-t) + Q2*t**2 Q0 = 3*(P1-P0) Q1 = 3*(P2-P1) Q2 = 3*(P3-P2) PS = P0 TS = P1 - P0 for i in range(0, n): t1 = (i + 1) * 1. / n PE = _spline(t1) TE = _tan(t1) giarc(PS, TS, PE, TE) PS = PE TS = TE if __name__ == '__main__': print "G0X0Y0Z0" print "F100" spline(P(0,0), P(.25,1), P(1,1), P(2,0), 2) print "G0X0Y0Z0" spline(P(0,0), P(.25,1), P(1,1), P(2,0), 4) print "G0X0Y0Z0" spline(P(0,0), P(.25,1), P(1,1), P(2,0), 8) print "M2"