(load "package.lisp")
(in-package :so3-cnc)
; a more useful constant
(setq tau (* 2 pi))
(defun vec+ (v1 v2) (cons (+ (car v1) (car v2)) (+ (cdr v1) (cdr v2))))
(defun vec- (v1 v2) (cons (- (car v1) (car v2)) (- (cdr v1) (cdr v2))))
(defun vec* (s v) (cons (* s (car v)) (* s (cdr v))))
(defun vec= (v1 v2) (and (= (car v1) (car v2)) (= (cdr v1) (cdr v2))))
(defun dot (v1 v2) (+ (* (car v1) (car v2)) (* (cdr v1) (cdr v2))))
(defun norm (v) (sqrt (dot v v)))
(defun normalize (v) (vec* (/ 1 (norm v)) v))
(defun eval-bezier (theta pts)
(if (= 1 (length pts))
(first pts)
(eval-bezier theta (loop for x in pts
for y in (rest pts)
collect (vec+ (vec* (- 1 theta) x) (vec* theta y))))))
(defun eval-bezier-cubic (theta p0 p1 p2 p3)
(eval-bezier theta (list p0 p1 p2 p3)))
(defun eval-bezier-quadratic (theta p0 p1 p2)
(eval-bezier theta (list p0 p1 p2)))
(defun linspace (a b step)
(let ((steps (abs (ceiling (/ (- b a) step)))))
(divspace a b steps)))
(defun divspace (a b steps)
(let* ((s (/ (- b a) steps)))
(loop for i from 0 to steps collect (+ a (* s i)))))
(defun linterp-bezier (pts &optional (points-per-mm 0.55))
(let ((npts (ceiling
(* points-per-mm
(reduce
(lambda (sum point-pair)
(+ sum (sqrt (+ (expt (- (caar point-pair) (caadr point-pair)) 2)
(expt (- (cdar point-pair) (cdadr point-pair)) 2)))))
(mapcar #'list pts (rest pts))
:initial-value 0)))))
(loop for theta in (divspace 0 1 npts) collect (eval-bezier theta pts))))
(defun linterp-bezier-cubic (p0 p1 p2 p3)
(linterp-bezier (list p0 p1 p2 p3)))
(defun linterp-bezier-quadratic (p0 p1 p2)
(linterp-bezier (list p0 p1 p2)))
(defun angle-between (v0 v1)
(* (if (< 0 (- (* (car v0) (cdr v1)) (* (cdr v0) (car v1)))) 1.0d0 -1.0d0)
(acos (/ (dot v0 v1) (* (norm v0) (norm v1))))
))