(load "package.lisp")
(load "bezier.lisp")
(in-package :so3-cnc)
(ql:quickload :alexandria)
(ql:quickload :clem)
(ql:quickload :cl-ppcre)
(ql:quickload :parse-float)
(ql:quickload :xmls)
(defparameter gcode-preamble '(((:G . 1) (:X . 0) (:Y . 0) (:F . 100))
))
(defparameter gcode-postamble '(((:G . 0) (:Z . 1))
((:G . 0) (:X . 0) (:Y . 0))))
(defun reshape-to-2x3 (vs)
(clem:array->matrix
(make-array '(3 3)
:initial-contents (list (list (first vs) (second vs) (third vs))
(list (fourth vs) (fifth vs) (sixth vs))
'(0 0 1)))
:matrix-class 'clem:double-float-matrix))
(defun vec3 (xy z)
(clem:array->matrix
(make-array '(3 1) :initial-contents (list (list (car xy)) (list (cdr xy)) (list z)))
:matrix-class 'clem:double-float-matrix))
(defun point (xy) (vec3 xy 1))
(defun vec (xy) (vec3 xy 0))
(defun clem-to-list (v) (cons (clem:val v 0 0) (clem:val v 1 0)))
(defun make-matrix (a b c d e f) (reshape-to-2x3 (list a b c d e f)))
(defun make-translate (x &optional (y 0)) (reshape-to-2x3 (list 1 0 x 0 1 y)))
(defun make-identity () (reshape-to-2x3 (list 1 0 0 0 1 0)))
(defun make-scale (x &optional (y nil)) (reshape-to-2x3 (list x 0 0 0 (if (null y) x y) 0)))
(defun make-rotate (a &optional (x 0) (y 0))
(clem:m* (make-translate x y)
(reshape-to-2x3 (list (cos a) (- (sin a)) 0 (sin a) (cos a) 0))
(make-translate (- x) (- y))
))
(defun make-skew-x (a) (reshape-to-2x3 (list 1 (tan a) 0 0 1 0)))
(defun make-skew-y (a) (reshape-to-2x3 (list 1 0 0 (tan a) 1 0)))
(defconstant point-to-mm-xform (make-scale (/ 25.4 72)))
(defun parse-transform (xform-str)
(cl-ppcre:register-groups-bind (fun argstr) ("([a-zA-Z]+)\\(([^)]*)\\)" xform-str)
(apply
(alexandria:switch (fun :test #'string-equal)
("matrix" #'make-matrix)
("translate" #'make-translate)
("scale" #'make-scale)
("rotate" #'make-rotate)
("skewX" #'make-skew-x)
("skewY" #'make-skew-y)
)
(mapcar #'parse-float:parse-float (cl-ppcre:split "[\\s,]+" argstr))
)))
(defstruct (svg-machine (:conc-name svgm-))
(current '(0 . 0)) ; current position
(rel-base '(0 . 0)) ; the position we base relative moves off of, updated at the end of each stanza
(last-start nil) ; last path start position
(last-ctrl-point nil) ; the last control point used in any curve operation, for S paths
(gcode gcode-preamble) ; transformed, complete gcode
(gcode-unxf nil) ; untransformed gcode that is being generated
)
(defun point-add (a b)
(point (cons (+ (clem:val a 0 0) (clem:val b 0 0)) (+ (clem:val a 1 0) (clem:val b 1 0)))))
(defun point-scale (a s)
(point (cons (* (clem:val a 0 0) s) (* (clem:val a 1 0) s))))
(defun arc-center-xy (a xy1p)
"Finds and returns (cx, cy), is-clockwise, theta1, theta2 - theta1, in that
order, as specified by the SVG arc implementation notes"
(let* ((rx (abs (first a)))
(ry (abs (second a)))
(rx2 (expt rx 2))
(ry2 (expt ry 2))
(phi (* (/ pi 180) (third a)))
(large-arc? (not (= (fourth a) 0.0)))
(cw? (not (= (fifth a) 0.0)))
(xy1 (point xy1p))
(xy2 (point (cons (sixth a) (seventh a))))
(rphi (make-rotate (- phi)))
(xy1-prime (clem:mat-mult rphi (point-scale (point-add xy1 (point-scale xy2 -1)) 0.5)))
(x-prime (clem:val xy1-prime 0 0))
(y-prime (clem:val xy1-prime 1 0))
(Lambda (+ (/ (expt x-prime 2) rx2) (/ (expt y-prime 2) ry2))))
(cond
((and (= x-prime 0) (= y-prime 0)) (error "arcs cannot start and end at same point"))
((> Lambda 1.0d0)
(let ((scale (+ (sqrt Lambda) 0.0001)))
(when (> scale 2)
(error "poorly formed arc, maybe? ~A ~A ~A" a xy1p scale))
(arc-center-xy (append (list (* scale rx) (* scale ry)) (subseq a 2)) xy1p)))
(t (let* ((cxy-rc (* (if (equal large-arc? cw?) -1.0 1.0)
(sqrt (/ (- (* rx2 ry2) (* rx2 (expt y-prime 2)) (* ry2 (expt x-prime 2)))
(+ (* rx2 (expt y-prime 2)) (* ry2 (expt x-prime 2)))))))
(cxy-r (if (complexp cxy-rc) (break) cxy-rc))
(cxy-prime (point (cons (/ (* cxy-r rx y-prime) ry)
(/ (* -1 cxy-r ry x-prime) rx))))
(cxy (point-add
(clem:mat-mult (clem:transpose rphi) cxy-prime)
(point-scale (clem:mat-add xy1 xy2) 0.5)))
(cx-prime (clem:val cxy-prime 0 0))
(cy-prime (clem:val cxy-prime 1 0))
(theta1 (angle-between
'(1 . 0)
(cons (/ (- x-prime cx-prime) rx)
(/ (- y-prime cy-prime) ry))))
(dtheta-unwhitened (angle-between
(cons (/ (- x-prime cx-prime) rx)
(/ (- y-prime cy-prime) ry))
(cons (/ (- 0 x-prime cx-prime) rx)
(/ (- 0 y-prime cy-prime) ry))))
(dtheta (cond
((and cw? (> dtheta-unwhitened 0)) (- dtheta-unwhitened tau))
((and (not cw?) (< dtheta-unwhitened 0)) (+ dtheta-unwhitened tau))
(t dtheta-unwhitened)))
)
(values (cons (clem:val cxy 0 0) (clem:val cxy 1 0))
cw?
theta1
dtheta
)
)))))
(defun splat-pairs (head pairs)
(cond
((consp head) (cons (car head) (splat-pairs (cdr head) pairs)))
((null pairs) (list head))
((null head) (splat-pairs (car pairs) (cdr pairs)))
(t (cons head (splat-pairs (car pairs) (cdr pairs))))))
(defun build-pairs (args)
(loop for i from 0 to (1- (length args)) by 2
collect (cons (nth i args) (nth (1+ i) args))))
(defun pair-to-abs-xy (svgm p) (vec+ p (svgm-current svgm)))
(defun pairs-to-abs-xy (svgm pairs)
(mapcar (lambda (p) (pair-to-abs-xy svgm p)) pairs))
(defun apply-xform (xform p &key (pfunc #'point))
(clem-to-list (clem:mat-mult xform (funcall pfunc p))))
(defun update-assoc (alist &rest update)
(labels
((update-assoc-1 (alist k v)
(cond
((null alist) (cons (cons k v) nil))
((eq (caar alist) k) (cons (cons k v) (cdr alist)))
(t (cons (car alist) (update-assoc-1 (cdr alist) k v)))
)))
(reduce (lambda (al up) (update-assoc-1 al (car up) (cdr up)))
update :initial-value alist)))
(defun xform-gcode-xy (xform line)
(let ((x (cdr (assoc :X line)))
(y (cdr (assoc :Y line))))
(cond
((and (null x) (null y)) line)
((or (null x) (null y)) (error "must specify either both x and y or neither, got " line))
(t (let ((xformed (apply-xform xform (cons x y))))
(update-assoc line (cons :X (car xformed)) (cons :Y (cdr xformed))))))))
(defun xform-gcode-arc (xform line)
(let ((x (cdr (assoc :X line)))
(y (cdr (assoc :Y line)))
(i (cdr (assoc :I line)))
(j (cdr (assoc :J line))))
(let ((xfxy (apply-xform xform (cons x y)))
(xfij (apply-xform xform (cons i j) :pfunc #'vec)))
(update-assoc line
(cons :X (car xfxy))
(cons :Y (cdr xfxy))
(cons :I (car xfij))
(cons :J (cdr xfij))
))))
(defun xform-gcode (xform lines)
(mapcar (lambda (line)
(let ((mode (cdr (assoc :G line))))
(alexandria:switch (mode)
(0 (xform-gcode-xy xform line))
(1 (xform-gcode-xy xform line))
(2 (xform-gcode-arc xform line))
(3 (xform-gcode-arc xform line))
(otherwise line)
)))
lines))
(defun interpolate-ellipse (arc-args svgm)
(labels
((eval-ellipse (theta rx ry) (cons (* rx (cos theta)) (* ry (sin theta)))))
(multiple-value-bind (cxy cw? theta1 dtheta) (arc-center-xy arc-args (svgm-current svgm))
(loop for theta in (linspace theta1 (+ theta1 dtheta) (/ tau 60))
collect
(let ((xy (vec+ (eval-ellipse theta (first arc-args) (second arc-args)) cxy)))
(list #\L (car xy) (cdr xy))))
)))
(defun to-abs (s svgm)
(labels ((from-pairs (mode ps)
(list (splat-pairs mode (pairs-to-abs-xy svgm ps))))
(auto-pairs () (from-pairs (char-upcase (first s)) (build-pairs (rest s))))
)
(alexandria:switch ((first s))
(#\m (auto-pairs))
(#\l (auto-pairs))
(#\c (auto-pairs))
(#\s (auto-pairs))
(#\q (auto-pairs))
(#\t (auto-pairs))
(#\h (from-pairs #\L (list (cons (second s) 0))))
(#\H (list (list #\L (second s) (cdr (svgm-current svgm)))))
(#\v (from-pairs #\L (list (cons 0 (second s)))))
(#\V (list (list #\L (car (svgm-current svgm)) (second s))))
(#\a (let* ((args (rest s))
(xy (pair-to-abs-xy svgm (cons (sixth args) (seventh args)))))
; send the abs version through to-abs again to be transformed
(to-abs (append '(#\A) (subseq s 1 6) (list (car xy) (cdr xy))) svgm)))
(#\A (let* ((args (rest s))
(rx (first args))
(ry (second args))
(x (sixth args))
(y (seventh args))
)
(if (> (abs (- rx ry)) 0.001)
; if it's not a perfect circle, we convert it to lines. Ellipses aren't
; supported by G-Code, so instead of trying to bake the xform into a special
; format to pass on, we just interpolate
(interpolate-ellipse args svgm)
; otherwise we leave it as a circle so we can use the G-Code arc commands
; Since it's a circle, we can set phi to zero (because there is no major/minor axis)
(list (list #\A rx ry 0.0d0 (fourth args) (fifth args) x y))
)))
(#\z '((#\Z))) ; z and Z are the same, but we upcase it to make conditionals simpler later
(otherwise (list s))
)))
(defun gcode-goto (xy &key (mode 1))
(list (cons :G mode) (cons :X (car xy)) (cons :Y (cdr xy))))
(defun push-polyline (svgm pts)
(reduce (lambda (svgm p) (push-stanza svgm (list #\L (car p) (cdr p))))
pts :initial-value svgm))
(defun linspace (a b step)
(let* ((steps (abs (ceiling (/ (- b a) step))))
(s (/ (- b a) steps)))
(loop for i from 0 to steps collect (+ a (* s i)))))
(defun push-stanza (svgm s)
"Creates and returns a new svg machine that includes the result of interpreting the given stanza. Stanza must be 'final', meaning that it's already in absolute coordinates."
(let* ((mode (first s))
(args (rest s))
(cur-xy (svgm-current svgm))
(ls (svgm-last-start svgm))
(gcode (svgm-gcode svgm))
(gunxf (svgm-gcode-unxf svgm))
(rb (svgm-rel-base svgm))
)
(alexandria:switch (mode)
; path start/end
(#\M (let ((p (cons (first args) (second args))))
(make-svg-machine
:current p :last-start p :rel-base rb
:gcode-unxf (append gunxf (list '((:G . 0) (:Z . 1)) (gcode-goto p :mode 0) '((:G . 1) (:Z . -1))))
:gcode gcode
)))
(#\Z (progn
(when (null ls)
(error "got Z (close path) when there's no current path"))
(make-svg-machine
:current ls :last-start ls :rel-base rb
:gcode-unxf (append gunxf (list (gcode-goto ls))) :gcode gcode
)))
; lines
(#\L (let ((p (cons (first args) (second args))))
(make-svg-machine
:current p :last-start (if (null ls) cur-xy ls) :rel-base rb
:gcode-unxf (append gunxf (list (gcode-goto p))) :gcode gcode
)))
; cubic beziers
(#\C (let* ((c1 (cons (first args) (second args)))
(c2 (cons (third args) (fourth args)))
(p2 (cons (fifth args) (sixth args)))
(svg-poly (push-polyline svgm (linterp-bezier-cubic cur-xy c1 c2 p2))))
(make-svg-machine
:current (svgm-current svg-poly) :last-start (svgm-last-start svg-poly)
:gcode (svgm-gcode svg-poly) :gcode-unxf (svgm-gcode-unxf svg-poly)
:last-ctrl-point c2 :rel-base rb
)))
(#\S (let* ((lcp? (svgm-last-ctrl-point svgm))
(lcp (if (null lcp?) cur-xy lcp?))
(c1 (vec+ cur-xy (vec- cur-xy lcp))))
(push-stanza svgm (append (list #\C (car c1) (cdr c1)) args))))
; quadratic beziers
(#\Q (let* ((c (cons (first args) (second args)))
(p2 (cons (third args) (fourth args)))
(svg-poly (push-polyline svgm (linterp-bezier-quadratic cur-xy c p2))))
(make-svg-machine
:current (svgm-current svg-poly) :last-start (svgm-last-start svg-poly)
:gcode (svgm-gcode svg-poly) :gcode-unxf (svgm-gcode-unxf svg-poly)
:last-ctrl-point c :rel-base rb
)))
(#\T (let* ((lcp? (svgm-last-ctrl-point svgm))
(lcp (if (null lcp?) cur-xy lcp?))
(c (vec+ cur-xy (vec- cur-xy lcp))))
(push-stanza svgm (append (list #\Q (car c) (cdr c)) args))))
; arcs
(#\A
(multiple-value-bind (center-xy cw?) (arc-center-xy args cur-xy)
(let ((xy (cons (sixth args) (seventh args)))
(ij (vec- center-xy cur-xy)))
(make-svg-machine
:current xy :last-start ls :rel-base rb
:gcode gcode
:gcode-unxf (append gunxf (list (list (cons :G (if cw? 3 2))
(cons :I (car ij))
(cons :J (cdr ij))
(cons :X (car xy))
(cons :Y (cdr xy)))))
))))
; rel-base update
(:end-stanza (make-svg-machine :current cur-xy :last-start ls :rel-base cur-xy
:gcode gcode :gcode-unxf gunxf))
(otherwise (error "unsupported mode ~A" mode))
)))
(defun run-stanzas (stanzas svgm)
(reduce
(lambda (isvgm s) (reduce (lambda (insvgm sabs) (push-stanza insvgm sabs))
(to-abs s isvgm)
:initial-value isvgm))
stanzas :initial-value svgm))
(defun normalize-stanza (s)
"Takes a stanza of the form (MODE ARG0 ARG1 ARG2 ... ARGN) and returns a list of stanzas
in which each stanza contains only one move with a mode modifier."
(let* ((mode (car s))
(target-len (alexandria:switch ((char-downcase mode))
(#\m 2) (#\z 0) (#\l 2) (#\h 1) (#\v 1) (#\c 6)
(#\s 4) (#\q 4) (#\t 2) (#\a 7)))
(args (cdr s))
(args-len (length args))
; some modes switch after the first set of arguments; this captures
; that.
(mode2 (cond
((equal mode #\m) #\l)
((equal mode #\M) #\L)
(t mode)
))
)
(if (= target-len 0)
(if (= 0 args-len)
(list (list mode))
(error "bad number of arguments for zero-length mode ~A: ~A" mode args-len))
(progn
(unless (= 0 (mod args-len target-len))
(error "bad number of arguments in mode ~A: ~A" mode args-len))
(labels ((split (a is-first)
(when (> (length a) 0)
(cons (cons (if is-first mode mode2) (subseq a 0 target-len))
(split (subseq a target-len) nil)))))
(append (split args t) '((:end-stanza)))
)))))
(defun load-path-args (stz)
(let ((stz-clean (string-trim '(#\Space #\Newline #\Tab #\Linefeed #\Return #\,) stz)))
(when (> (length stz-clean) 0)
(multiple-value-bind (st end) (cl-ppcre:scan "[\\-]?[0-9]*(\\.[0-9]+|[0-9]*)" stz-clean)
(cons (parse-float:parse-float (subseq stz-clean st end))
(load-path-args (subseq stz-clean end)))))))
(defun load-path-stanzas (d)
(when (> (length d) 0)
(multiple-value-bind (st end) (cl-ppcre:scan "^[a-zA-Z][^a-zA-Z]*" d)
(cons
(cons (char d 0) (load-path-args (subseq d (1+ st) end)))
(load-path-stanzas (subseq d end)))
)))
(defun load-path-data (d)
(reduce #'append (mapcar #'normalize-stanza (load-path-stanzas d))))
(defun update-struct (struct &rest bindings)
(loop
with copy = (copy-structure struct)
for (slot value) on bindings by #'cddr
do (setf (slot-value copy slot) value)
finally (return copy)))
(defun xform-svgm (svgm xform)
"Transforms gcode-unxf and appends it onto the gcode list"
(update-struct svgm
'gcode-unxf nil
'gcode (append (svgm-gcode svgm)
(xform-gcode xform (svgm-gcode-unxf svgm)))))
(defun a (attr data)
(let ((attr-str (cadr (assoc attr (cadr data) :test #'string-equal))))
(if attr-str (parse-float:parse-float attr-str) 0)))
(defun load-unxf-path (svgm data)
(run-stanzas (load-path-data (cadr (assoc "d" (cadr data) :test #'string-equal))) svgm))
(defun load-unxf-line (svgm d)
(run-stanzas (list (list #\M (a "x1" d) (a "y1" d)) (list #\L (a "x2" d) (a "y2" d))) svgm))
(defun load-unxf-circle (svgm d)
(let* ((cx (a "cx" d)) (cy (a "cy" d)) (r (a "r" d))
(xx (+ cx r)) (yy cy))
(update-struct
svgm
'gcode-unxf (list
(list '(:G . 0) '(:Z . 1))
(list '(:G . 0) (cons :X xx) (cons :Y yy))
(list '(:G . 1) '(:Z . -1))
(list '(:G . 2) (cons :X xx) (cons :Y yy) (cons :I (- r)) '(:J . 0))
(list '(:G . 0) '(:Z . 1))
))))
(defun load-unxf-polygon (svgm d)
(let ((points (load-path-args (cadr (assoc "points" (cadr d) :test #'string-equal)))))
(run-stanzas (append (list (list #\M (first points) (second points)))
(normalize-stanza (cons #\L (cddr points))))
svgm)
))
(defun load-unxf-rect (svgm d)
(let ((x (a "x" d))
(y (a "y" d))
(w (a "width" d))
(h (a "height" d)))
(run-stanzas (list (list #\M x y)
(list #\l w 0)
(list #\l 0 h)
(list #\l (- w) 0)
'(#\Z))
svgm)))
(defun load-unxf-ellipse (svgm d)
(let* ((rx (a "rx" d))
(ry (a "ry" d))
(sx (+ (a "cx" d) rx))
(mx (- (a "cx" d) rx))
(sy (a "cy" d)))
(run-stanzas (list (list #\M sx sy)
(list #\A rx ry 0 0 0 mx sy)
(list #\A rx ry 0 0 0 sx sy)
)
svgm)
))
(defun load-unxf-gcode (svgm data)
(funcall (alexandria:switch ((caar data) :test #'string-equal)
("path" #'load-unxf-path)
("line" #'load-unxf-line)
("circle" #'load-unxf-circle)
("ellipse" #'load-unxf-ellipse)
("polygon" #'load-unxf-polygon)
("rect" #'load-unxf-rect)
(otherwise (lambda (svgm &rest _) svgm))
)
svgm data))
(defun print-xform-stack (xforms)
(format t "[~%")
(mapcar (lambda (x) (clem:print-matrix x) (format t "~%")) xforms)
(format t "]~%")
xforms)
(defun load-svg-from-xml (data svgm xform-stack)
(if (listp data)
(let* ((tag (car data))
(attributes (cadr data))
(xform-attr (assoc "transform" attributes :test #'string-equal))
(new-xform-stack (if (null xform-attr)
xform-stack
(cons (parse-transform (cadr xform-attr)) xform-stack)))
(xform (reduce #'clem:mat-mult (reverse new-xform-stack)
:initial-value (clem:identity-matrix 3)))
(children (cddr data))
(svgm-unxf (load-unxf-gcode svgm data))
(svgm-new (xform-svgm svgm-unxf xform))
)
(reduce (lambda (asvgm child)
(load-svg-from-xml child asvgm new-xform-stack))
children
:initial-value svgm-new)
)
svgm))
(defun svg-load (svgdata)
(load-svg-from-xml svgdata (make-svg-machine) (list point-to-mm-xform)))
(defun svg-to-gcode (fname gcode-out)
(with-open-file (outf gcode-out :direction :output :if-exists :supersede)
(let* ((svgtxt (alexandria:read-file-into-string fname))
(svgdata (xmls:parse svgtxt)))
(svgm-emit-gcode outf (svg-load svgdata)))))
(defun svgm-emit-gcode (outstream svgm)
(labels ((gcode-number (n) (if (< (abs (- (round n) n)) 0.001)
(format nil "~D" (round n))
(format nil "~,2F" n)))
(emit-assignment (reg) (format nil "~A~A" (car reg) (gcode-number (cdr reg))))
(emit-line (line) (format nil "~{~A~^ ~}" (mapcar #'emit-assignment line))))
(format outstream "~{~A~^~%~}" (mapcar #'emit-line (append (svgm-gcode svgm) gcode-postamble)))))