git.haldean.org plotter / 144cef7
arc support Haldean Brown 4 years ago
4 changed file(s) with 144 addition(s) and 32 deletion(s). Raw diff Collapse all Expand all
0 willh@hiro.9100:1524679355
11
22 (in-package :so3-cnc)
33
4 ; a more useful constant
5 (setq tau (* 2 pi))
6
47 (defun vec+ (v1 v2) (cons (+ (car v1) (car v2)) (+ (cdr v1) (cdr v2))))
58 (defun vec- (v1 v2) (cons (- (car v1) (car v2)) (- (cdr v1) (cdr v2))))
69 (defun vec* (s v) (cons (* s (car v)) (* s (cdr v))))
10 (defun dot (v1 v2) (+ (* (car v1) (car v2)) (* (cdr v1) (cdr v2))))
11 (defun norm (v) (sqrt (dot v v)))
712
813 (defun eval-bezier (theta pts)
914 (if (= 1 (length pts))
1621 (eval-bezier theta (list p0 p1 p2 p3)))
1722 (defun eval-bezier-quadratic (theta p0 p1 p2)
1823 (eval-bezier theta (list p0 p1 p2)))
24
25 (defun angle-between (v0 v1)
26 (* (if (< 0 (- (* (car v0) (cdr v1)) (* (cdr v0) (car v1)))) 1.0d0 -1.0d0)
27 (acos (/ (dot v0 v1) (* (norm v0) (norm v1))))
28 ))
00 (load "package.lisp")
1 (load "bezier.lisp")
12 (in-package :so3-cnc)
23
34 (ql:quickload :alexandria)
56 (ql:quickload :jsown)
67 (ql:quickload :parse-float)
78 (ql:quickload :s-http-client)
8
9 ; a more useful constant
10 (setq tau (* 2 pi))
119
1210 ; width of the work area, in stepper steps
1311 (setf *width* 0)
1212 (make-array '(3 3)
1313 :initial-contents (list (list (first vs) (second vs) (third vs))
1414 (list (fourth vs) (fifth vs) (sixth vs))
15 '(0 0 1)))))
16 (defun point (xy) (clem:array->matrix
17 (make-array '(3 1)
18 :initial-contents (list (list (car xy))
19 (list (cdr xy))
20 '(1)))))
21
22 (defun vec (xy) (clem:array->matrix
23 (make-array '(3 1)
24 :initial-contents (list (list (car xy))
25 (list (cdr xy))
26 '(0)))))
27
28 (defun clem-to-list (v) (list (clem:val v 0 0) (clem:val v 1 0)))
15 '(0 0 1)))
16 :matrix-class 'clem:double-float-matrix))
17
18 (defun vec3 (xy z)
19 (clem:array->matrix
20 (make-array '(3 1) :initial-contents (list (list (car xy)) (list (cdr xy)) (list z)))
21 :matrix-class 'clem:double-float-matrix))
22
23 (defun point (xy) (vec3 xy 1))
24 (defun vec (xy) (vec3 xy 0))
25
26 (defun clem-to-list (v) (cons (clem:val v 0 0) (clem:val v 1 0)))
2927
3028 (defun make-matrix (a b c d e f) (reshape-to-2x3 (list a b c d e f)))
3129 (defun make-translate (x &optional (y 0)) (reshape-to-2x3 (list 1 0 x 0 1 y)))
3230 (defun make-scale (x &optional (y nil)) (reshape-to-2x3 (list x 0 0 0 (if (null y) x y) 0)))
3331 (defun make-rotate (a &optional (x 0) (y 0))
3432 (clem:m* (make-translate x y)
35 (reshape-to-2x3 (list (cos a) (- (sin a)) (sin a) (cos a) 0 0))
33 (reshape-to-2x3 (list (cos a) (- (sin a)) 0 (sin a) (cos a) 0))
3634 (make-translate (- x) (- y))
3735 ))
3836 (defun make-skew-x (a) (reshape-to-2x3 (list 1 (tan a) 0 0 1 0)))
5957 (gcode '())
6058 )
6159
62 (defun to-abs-xy (svgm mode pairs)
60 (defun pair-to-abs-xy (svgm p) (vec+ p (svgm-current svgm)))
61 (defun pairs-to-abs-xy (svgm mode pairs)
6362 (let* ((cur-xy (svgm-current svgm))
6463 (cur-x (car cur-xy))
6564 (cur-y (cdr cur-xy)))
6968 ))
7069 ))
7170
71 (defun apply-xform (xform p &key (pfunc #'point))
72 (clem-to-list (clem:mat-mult xform (funcall pfunc p))))
73 (defun make-xformer (xform &key (pfunc #'point)) (lambda (p) (apply-xform xform p :pfunc pfunc)))
7274 (defun apply-xforms (xform s)
7375 (let* ((mode (first s))
7476 (args (rest s))
7577 (pairs (loop for i from 0 to (- (length args) 1) by 2
76 collect (cons (nth i args) (nth (+ i 1) args)))))
77 (cons mode
78 (reduce #'append
79 (mapcar (lambda (p) (clem-to-list (clem:mat-mult xform (point p)))) pairs)))))
78 collect (cons (nth i args) (nth (1+ i) args)))))
79 (cons mode (reduce (lambda (l p) (append l (list (car p) (cdr p))))
80 (mapcar (make-xformer xform) pairs)
81 :initial-value '()))))
82
83 (defun point-add (a b)
84 (point (cons (+ (clem:val a 0 0) (clem:val b 0 0)) (+ (clem:val a 1 0) (clem:val b 1 0)))))
85 (defun point-scale (a s)
86 (point (cons (* (clem:val a 0 0) s) (* (clem:val a 1 0) s))))
87
88 (defun arc-center-xy (a xy1p)
89 "Finds and returns (cx, cy), is-clockwise, theta1, theta2 - theta1, in that
90 order, as specified by the SVG arc implementation notes"
91 (let* ((rx (abs (first a)))
92 (ry (abs (second a)))
93 (rx2 (expt rx 2))
94 (ry2 (expt ry 2))
95 (phi (* (/ pi 180) (third a)))
96 (large-arc? (not (= (fourth a) 0.0)))
97 (cw? (not (= (fifth a) 0.0)))
98 (xy1 (point xy1p))
99 (xy2 (point (cons (sixth a) (seventh a))))
100 (rphi (make-rotate (- phi)))
101 (xy1-prime (clem:mat-mult rphi (point-scale (point-add xy1 (point-scale xy2 -1)) 0.5)))
102 (x-prime (clem:val xy1-prime 0 0))
103 (y-prime (clem:val xy1-prime 1 0))
104 (Lambda (+ (/ (expt x-prime 2) rx2) (/ (expt y-prime 2) ry2))))
105 (if (> Lambda 1.0d0)
106 (let ((scale (+ (sqrt Lambda) 0.0001)))
107 (arc-center-xy (append (list (* scale rx) (* scale ry)) (subseq a 2)) xy1p))
108 (let* ((cxy-rc (* (if (equal large-arc? cw?) -1.0 1.0)
109 (sqrt (/ (- (* rx2 ry2) (* rx2 (expt y-prime 2)) (* ry2 (expt x-prime 2)))
110 (+ (* rx2 (expt y-prime 2)) (* ry2 (expt x-prime 2)))))))
111 (cxy-r (if (complexp cxy-rc) (break) cxy-rc))
112 (cxy-prime (point (cons (/ (* cxy-r rx y-prime) ry)
113 (/ (* -1 cxy-r ry x-prime) rx))))
114 (cxy (point-add
115 (clem:mat-mult (clem:transpose rphi) cxy-prime)
116 (point-scale (clem:mat-add xy1 xy2) 0.5)))
117 (cx-prime (clem:val cxy-prime 0 0))
118 (cy-prime (clem:val cxy-prime 1 0))
119 (theta1 (angle-between
120 '(1 . 0)
121 (cons (/ (- x-prime cx-prime) rx)
122 (/ (- y-prime cy-prime) ry))))
123 (dtheta-unwhitened (angle-between
124 (cons (/ (- x-prime cx-prime) rx)
125 (/ (- y-prime cy-prime) ry))
126 (cons (/ (- 0 x-prime cx-prime) rx)
127 (/ (- 0 y-prime cy-prime) ry))))
128 (dtheta (cond
129 ((and cw? (> dtheta-unwhitened 0)) (- dtheta-unwhitened tau))
130 ((and (not cw?) (< dtheta-unwhitened 0)) (+ dtheta-unwhitened tau))
131 (t dtheta-unwhitened)))
132 )
133 (values (cons (clem:val cxy 0 0) (clem:val cxy 1 0))
134 cw?
135 theta1
136 dtheta
137 )
138 ))))
80139
81140 (defun to-abs (s xform svgm)
82 (macrolet ((pairs () `(apply-xforms xform (to-abs-xy svgm (char-upcase (first s)) (rest s)))))
141 (macrolet ((pairs () `(apply-xforms xform (pairs-to-abs-xy svgm (char-upcase (first s)) (rest s)))))
83142 (alexandria:switch ((first s))
84143 (#\m (pairs))
85144 (#\l (pairs))
87146 (#\s (pairs))
88147 (#\q (pairs))
89148 (#\t (pairs))
90 (#\h (to-abs-xy svgm #\L (list (second s) 0)))
91 (#\v (to-abs-xy svgm #\L (list 0 (second s))))
92 (#\a (error "relative arc moves not supported yet"))
93 (#\A s)
149 (#\h (apply-xforms xform (pairs-to-abs-xy svgm #\L (list (second s) 0))))
150 (#\v (apply-xforms xform (pairs-to-abs-xy svgm #\L (list 0 (second s)))))
151 (#\H (apply-xforms xform (list #\L (second s) 0)))
152 (#\V (apply-xforms xform (list #\L 0 (second s))))
153 (#\a (let* ((args (rest s))
154 (xy (pair-to-abs-xy svgm (cons (sixth args) (seventh args)))))
155 ; send the abs version through to-abs again to be transformed
156 (to-abs (append '(#\A) (subseq s 1 6) (list (car xy) (cdr xy))) xform svgm)))
157 (#\A (let* ((args (rest s))
158 (rx (first args))
159 (ry (second args))
160 (x (sixth args))
161 (y (seventh args))
162 )
163 (if (> (abs (- rx ry)) 0.001)
164 ; if it's not a perfect circle, we convert it to lines. Ellipses aren't
165 ; supported by G-Code, so instead of trying to bake the xform into a special
166 ; format to pass on, we just interpolate
167 (error "ellipses not supported yet")
168 ; otherwise we leave it as a circle so we can use the G-Code arc commands
169 ; Since it's a circle, we can set phi to zero (because there is no major/minor axis)
170 (let ((xformed-xy (apply-xform xform (cons x y))))
171 (list #\A rx ry 0.0d0 (fourth args) (fifth args) (car xformed-xy) (cdr xformed-xy)))
172 )))
94173 (#\z '(#\Z)) ; z and Z are the same, but we upcase it to make conditionals simpler later
95174 (otherwise (apply-xforms xform s))
96175 )))
101180 (defun push-polyline (svgm xform pts)
102181 (reduce (lambda (svgm p) (push-stanza svgm (list #\L (car p) (cdr p)) xform)) pts :initial-value svgm))
103182
104 (setf *theta-steps* '(0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0.40 0.45 0.50 0.55 0.60 0.65 0.70 0.75 0.80 0.85 0.90 0.95 1.00))
183 (defun linspace (a b step)
184 (let* ((steps (abs (ceiling (/ (- b a) step))))
185 (s (/ (- b a) steps)))
186 (loop for i from 0 to steps collect (+ a (* s i)))))
105187
106188 (defun push-stanza (svgm sraw xform)
107189 "Creates and returns a new svg machine that includes the result of interpreting the given stanza"
140222 (#\C (let* ((c1 (cons (first args) (second args)))
141223 (c2 (cons (third args) (fourth args)))
142224 (p2 (cons (fifth args) (sixth args)))
143 (svg-poly (push-polyline svgm xform (loop for theta in *theta-steps*
225 (svg-poly (push-polyline svgm xform (loop for theta in (linspace 0 1 0.05)
144226 collect (eval-bezier-cubic theta cur-xy c1 c2 p2)))))
145227 (make-svg-machine
146228 :current (svgm-current svg-poly) :last-start (svgm-last-start svg-poly)
154236 ; quadratic beziers
155237 (#\Q (let* ((c (cons (first args) (second args)))
156238 (p2 (cons (fifth args) (sixth args)))
157 (svg-poly (push-polyline svgm xform (loop for theta in *theta-steps*
239 (svg-poly (push-polyline svgm xform (loop for theta in (linspace 0 1 0.05)
158240 collect (eval-bezier-quadratic theta cur-xy c p2)))))
159241 (make-svg-machine
160242 :current (svgm-current svg-poly) :last-start (svgm-last-start svg-poly)
166248 (push-stanza svgm (append (list #\Q (car c) (cdr c)) args) xform)))
167249
168250 ; arcs
251 (#\A
252 (multiple-value-bind (center-xy cw?) (arc-center-xy args cur-xy)
253 (let ((xy (cons (sixth args) (seventh args)))
254 (ij (vec- center-xy cur-xy)))
255 (make-svg-machine
256 :current xy :last-start ls
257 :gcode (append gcode (list (list (cons :G (if cw? 2 3))
258 (cons :I (car ij))
259 (cons :J (cdr ij))
260 (cons :X (car xy))
261 (cons :Y (cdr xy)))))
262 ))))
263
169264 (otherwise (error "unsupported mode ~A" mode))
170265 )))
171266
213308 (when (> (length d) 0)
214309 (multiple-value-bind (st end) (cl-ppcre:scan "^[a-zA-Z][^a-zA-Z]*" d)
215310 (cons
216 (cons (char d 0) (load-path-args (subseq d (+ st 1) end)))
311 (cons (char d 0) (load-path-args (subseq d (1+ st) end)))
217312 (load-path-stanzas (subseq d end)))
218313 )))
219314
245340 (let* ((svgtxt (alexandria:read-file-into-string fname))
246341 (svgdata (xmls:parse svgtxt)))
247342 (svgm-gcode (load-svg-from-xml svgdata (make-svg-machine) nil))))
343
344 (defun svgm-emit-gcode (svgm)
345 (labels ((gcode-number (n) (if (< (abs (- (round n) n)) 0.001)
346 (format nil "~D" (round n))
347 (format nil "~,3F" n)))
348 (emit-assignment (reg) (format nil "~A~A" (car reg) (gcode-number (cdr reg))))
349 (emit-line (line) (format nil "~{~A~^ ~}" (mapcar #'emit-assignment line))))
350 (format nil "~{~A~^~%~}" (mapcar #'emit-line (svgm-gcode svgm)))))