root/trunk/thirdparty/cl-vectors-0.1.3/paths.lisp

;;;; cl-vectors -- Rasterizer and paths manipulation library
;;;; Copyright (C) 2007  Frédéric Jolliton <frederic@jolliton.com>
;;;; 
;;;; This library is free software; you can redistribute it and/or
;;;; modify it under the terms of the Lisp Lesser GNU Public License
;;;; (http://opensource.franz.com/preamble.html), known as the LLGPL.
;;;; 
;;;; This library 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 Lisp
;;;; Lesser GNU Public License for more details.

;;;; This file provides facilities to create and manipulate vectorial paths.
;;;;
;;;; Changelogs:
;;;;
;;;; 2007-02-20: first release

#+nil(error "This file assume that #+NIL is never defined.")

(in-package #:net.tuxee.paths)

(defvar *bezier-distance-tolerance* 0.5
  "The default distance tolerance used when rendering Bezier
curves.")

(defvar *bezier-angle-tolerance* 0.05
  "The default angle tolerance (in radian) used when rendering
Bezier curves")

(defvar *arc-length-tolerance* 1.0
  "The maximum length of segment describing an arc.")

(defvar *miter-limit* 4.0
  "Miter limit before reverting to bevel joint. Must be >=1.0.")

;;;--[ Math utilities ]------------------------------------------------------

;;; http://mathworld.wolfram.com/Line-LineIntersection.html
(defun line-intersection (x1 y1 x2 y2
                          x3 y3 x4 y4)
  "Compute the intersection between 2 lines (x1,y1)-(x2,y2)
and (x3,y3)-(x4,y4). Return the coordinates of the intersection
points as 2 values. If the 2 lines are colinears, return NIL."
  (flet ((det (a b c d)
           (- (* a d)
              (* b c))))
    (let* ((dx1 (- x2 x1))
           (dy1 (- y2 y1))
           (dx2 (- x4 x3))
           (dy2 (- y4 y3))
           (d (det dx2 dy2 dx1 dy1)))
      (unless (zerop d)
        (let ((a (det x1 y1 x2 y2))
              (b (det x3 y3 x4 y4)))
          (values (/ (det a dx1 b dx2) d)
                  (/ (det a dy1 b dy2) d)))))))

(defun line-intersection/delta (x1 y1 dx1 dy1
                                x2 y2 dx2 dy2)
  "Compute the intersection between the line by (x1,y1) and
direction (dx1,dy1) and the line by (x2,y2) and
direction (dx2,dy2). Return the coordinates of the intersection
points as 2 values. If the 2 lines are colinears, return NIL."
  (flet ((det (a b c d)
           (- (* a d)
              (* b c))))
    (let ((d (det dx2 dy2 dx1 dy1)))
      (unless (zerop d)
        (let ((a (det x1 y1 (+ x1 dx1) (+ y1 dy1)))
              (b (det x2 y2 (+ x2 dx2) (+ y2 dy2))))
          (values (/ (det a dx1 b dx2) d)
                  (/ (det a dy1 b dy2) d)))))))

(defun normalize (x y &optional (length 1.0))
  "Normalize the vector (X,Y) such that its length is LENGTH (or
1.0 if unspecified.) Return the component of the resulting vector
as 2 values. Return NIL if the input vector had a null length."
  (if (zerop length)
      (values 0.0 0.0)
      (let ((norm (/ (sqrt (+ (* x x) (* y y))) length)))
        (unless (zerop norm)
          (values (/ x norm) (/ y norm))))))

(defun line-normal (x1 y1 x2 y2)
  "Normalize the vector (X2-X1,Y2-Y1). See NORMALIZE."
  (normalize (- x2 x1) (- y2 y1)))

;;;--[ Points ]--------------------------------------------------------------

;;; Points are supposed to be immutable

(declaim (inline make-point point-x point-y))
(defun make-point (x y) (cons x y))
(defun point-x (point) (car point))
(defun point-y (point) (cdr point))

;;; Utility functions for points

(defun p+ (p1 p2)
  (make-point (+ (point-x p1) (point-x p2))
              (+ (point-y p1) (point-y p2))))

(defun p- (p1 p2)
  (make-point (- (point-x p1) (point-x p2))
              (- (point-y p1) (point-y p2))))

(defun p* (point scale &optional (scale-y scale))
  (make-point (* (point-x point) scale)
              (* (point-y point) scale-y)))

(defun point-rotate (point angle)
  "Rotate POINT by ANGLE radian around the origin."
  (let ((x (point-x point))
        (y (point-y point)))
    (make-point (- (* x (cos angle)) (* y (sin angle)))
                (+ (* y (cos angle)) (* x (sin angle))))))

(defun point-angle (point)
  "Compute the angle of POINT relatively to the X axis."
  (atan (point-y point) (point-x point)))

(defun point-norm (point)
  "Compute the distance of POINT from origin."
  (sqrt (+ (expt (point-x point) 2)
           (expt (point-y point) 2))))

;; (point-norm (p- p2 p1))
(defun point-distance (p1 p2)
  "Compute the distance between P1 and P2."
  (sqrt (+ (expt (- (point-x p2) (point-x p1)) 2)
           (expt (- (point-y p2) (point-y p1)) 2))))

;; (p* (p+ p1 p2) 0.5)
(defun point-middle (p1 p2)
  "Compute the point between P1 and P2."
  (make-point (/ (+ (point-x p1) (point-x p2)) 2.0)
              (/ (+ (point-y p1) (point-y p2)) 2.0)))

;;;--[ Paths ]---------------------------------------------------------------

(defstruct path
  (type :open-polyline :type (member :open-polyline :closed-polyline :polygon))
  (knots (make-array 0 :adjustable t :fill-pointer 0))
  (interpolations (make-array 0 :adjustable t :fill-pointer 0)))

(defun create-path (type)
  "Create a new path of the given type. The type must be one of
the following keyword:

:open-polyline -- An open polyline path,
:closed-polyline -- A closed polyline path,
:polygon -- Like :closed-polyline, but implicitly filled."
  (assert (member type '(:open-polyline :closed-polyline :polygon)))
  (make-path :type type))

(defun path-clear (path)
  "Clear the path such that it is empty."
  (setf (fill-pointer (path-knots path)) 0
        (fill-pointer (path-interpolations path)) 0))

(defun path-reset (path knot)
  "Reset the path such that it is a single knot."
  (path-clear path)
  (vector-push-extend knot (path-knots path))
  (vector-push-extend (make-straight-line) (path-interpolations path)))

(defun path-extend (path interpolation knot)
  "Extend the path to KNOT, with INTERPOLATION."
  (vector-push-extend interpolation (path-interpolations path))
  (vector-push-extend knot (path-knots path)))

(defun path-concatenate (path interpolation other-path)
  "Append OTHER-PATH to PATH, joined by INTERPOLATION."
  (let ((interpolations (path-interpolations other-path))
        (knots (path-knots other-path)))
    (loop for i below (length knots)
       do (path-extend path
                       (interpolation-clone (if (and (zerop i) interpolation)
                                                interpolation
                                                (aref interpolations i)))
                       (aref knots i)))))

(defun path-replace (path other-path)
  "Replace PATH with contents of OTHER-PATH."
  (path-clear path)
  (path-concatenate path nil other-path))

(defun path-size (path)
  "Return the number of knots on the path."
  (length (path-knots path)))

(defun path-last-knot (path)
  "Return the last knot of the path. Return NIL if the path is
empty."
  (let ((knots (path-knots path)))
    (when (plusp (length knots))
      (aref knots (1- (length knots))))))

;;; Iterators

(defgeneric path-iterator-reset (iterator)
  (:documentation "Reset the iterator before the first knot."))

(defgeneric path-iterator-next (iterator)
  (:documentation "Move the iterator to the next knot, and return
3 values: INTERPOLATION, KNOT and END-P. INTERPOLATION is the
interpolation between the previous knot and the current one. For
the first iteration, INTERPOLATION is usually the implicit
straight line between the last knot and the first knot. KNOT and
INTERPOLATION are null if the path is empty. END-P is true if the
knot is the last on the path or if the path is empty."))

(defun path-from-iterator (iterator type)
  "Construct a new path from the given iterator."
  (let ((path (create-path type)))
    (loop
       (multiple-value-bind (iterator knot end-p) (path-iterator-next iterator)
         (path-extend path iterator knot)
         (when end-p
           (return path))))))

;;; Classic iterator

(defstruct path-iterator-state
  path index)

(defun path-iterator (path)
  (make-path-iterator-state :path path :index nil))

(defmethod path-iterator-reset ((iterator path-iterator-state))
  (setf (path-iterator-state-index iterator) nil))

(defmethod path-iterator-next ((iterator path-iterator-state))
  (let* ((index (path-iterator-state-index iterator))
         (path (path-iterator-state-path iterator))
         (knots (path-knots path))
         (interpolations (path-interpolations path)))
    (cond
      ((zerop (length knots))
       (values nil nil t))
      (t
       ;; Update index to the next place
       (setf index
             (setf (path-iterator-state-index iterator)
                   (if (null index) 0 (mod (1+ index) (length knots)))))
       (values (aref interpolations index)
               (aref knots index)
               (= index (1- (length knots))))))))

;;; Segmented iterator
;;;
;;; This iterator iterate over segmented interpolation, if the
;;; interpolation is matched by the predicate. This is useful for
;;; algorithms that doesn't handle certain type of interpolations.
;;; The predicate could test the type, but also certain type of
;;; interpolation (such as arc of circle vs arc of ellipse, or degree
;;; of the Bezier curves.)

;;; Note: I use PI prefix instead of PATH-ITERATOR to shorten names.

(defstruct pi-segmented-state
  path index predicate end-p queue)

(defun path-iterator-segmented (path &optional (predicate (constantly t)))
  (make-pi-segmented-state :path path :index nil
                                      :predicate predicate
                                      :end-p nil :queue nil))

(defmethod path-iterator-reset ((iterator pi-segmented-state))
  (setf (pi-segmented-state-index iterator) nil
        (pi-segmented-state-queue iterator) nil))

(defmethod path-iterator-next ((iterator pi-segmented-state))
  (flet ((update-queue (interpolation k1 k2 last-p)
           (let (new-queue)
             (interpolation-segment interpolation k1 k2 (lambda (p) (push p new-queue)))
             (push k2 new-queue)
             (setf (pi-segmented-state-end-p iterator) last-p
                   (pi-segmented-state-queue iterator) (nreverse new-queue))))
         (dequeue ()
           (let* ((knot (pop (pi-segmented-state-queue iterator)))
                  (end-p (and (pi-segmented-state-end-p iterator)
                              (null (pi-segmented-state-queue iterator)))))
             (values (make-straight-line) knot (when end-p t)))))
    (cond
      ((pi-segmented-state-queue iterator)
       ;; Queue is not empty, process it first.
       (dequeue))
      (t
       ;; Either refill the queue, or return the next straight line
       ;; from the sub iterator.
       (let* ((index (pi-segmented-state-index iterator))
              (path (pi-segmented-state-path iterator))
              (knots (path-knots path))
              (interpolations (path-interpolations path)))
         (cond
           ((zerop (length knots))
            ;; Empty path.
            (values nil nil t))
           (t
            ;; Update index to the next place
            (setf index
                  (setf (pi-segmented-state-index iterator)
                        (if (null index) 0 (mod (1+ index) (length knots)))))
            (let ((interpolation (aref interpolations index))
                  (knot (aref knots index))
                  (end-p (= index (1- (length knots)))))
              ;; Check if we have to segment the next interpolation
              (if (funcall (pi-segmented-state-predicate iterator)
                           interpolation)
                  (let ((previous-index (mod (1- index) (length knots))))
                    (update-queue interpolation
                                  (aref knots previous-index)
                                  knot end-p)
                    (dequeue))
                  (values interpolation knot end-p))))))))))

;;; Iterate distinct

;;; This iterator filter out identical knots. That is, the knots with
;;; the same positions, with any interpolation. (All interpolations
;;; currently implemented are empty when knot around them are not
;;; distinct.)

;;; When cyclic-p is true, the first knot of the iterator is the first
;;; knot distinct from the first knot of the reference iterator.

;;; When cyclic-p is false, the first knot of the iterator if the
;;; first knot of the reference iterator, and if the path ends with a
;;; knot which is not distinct from the first, it is kept.

(defclass filter-distinct-state ()
  ((iterator :initarg :iterator)
   (cyclic-p :initarg :cyclic-p)
   (fixed :initarg :fixed)
   (next :initarg :next)
   (next-is-end-p)))

(defun filter-distinct (iterator &optional (preserve-cyclic-end-p nil))
  (make-instance 'filter-distinct-state
                 :iterator iterator
                 :cyclic-p (not preserve-cyclic-end-p)
                 :fixed nil
                 :next nil))

(defmethod path-iterator-reset ((iterator filter-distinct-state))
  (with-slots ((sub iterator) next next-is-end-p) iterator
    (path-iterator-reset sub)
    (setf next nil
          next-is-end-p nil)))

(defmethod path-iterator-next ((iterator filter-distinct-state))
  (with-slots ((sub iterator) cyclic-p fixed next next-is-end-p) iterator
    (when fixed
      ;; constant result cached
      (return-from path-iterator-next (values-list fixed)))
    (labels ((get-next ()
               "Get the next knot information as a list (not as
               multiple values)."
               (multiple-value-list (path-iterator-next sub)))
             (distinct-p (a b)
               "Test if A and B have distinct knots."
               (not (zerop (point-distance (second a) (second b)))))
             (move-to-next (previous loop-p)
               "Move iterator to find a knot distinct from the
               PREVIOUS. Also indicate if the resulting knot is
               the first of the sub iterator, and if end of path
               was encountered. This is needed to compute the
               effective END-P flag for the resulting iterator."
               (loop
                  with first-p = (third previous)
                  with end-encountered-p = (third previous)
                  for current = (get-next)
                  until (or (distinct-p previous current)
                            (and (not loop-p) first-p))
                  do (setf first-p (third current))
                  when (third current)
                  do (setf end-encountered-p t)
                  finally (return (values current first-p end-encountered-p)))))
      (let (result)
        (unless next
          ;; First time we iterate.
          (setf next-is-end-p nil)
          (let ((first (get-next)))
            (cond
              ((or (not (second first))
                   (third first))
               ;; It was an empty path or a single knot path. Cache it
               ;; and returns it for each further iterations.
               (setf fixed first
                     result first))
              (cyclic-p
               (multiple-value-bind (first-in-cycle first-p end-p) (move-to-next first nil)
                 (declare (ignore first-p))
                 (cond
                   (end-p
                    (setf (third first) t
                          fixed first
                          result first))
                   (t
                    (setf next first-in-cycle)))))
              (t
               (setf next first)))))
        (unless result
          ;; We copy NEXT because we need to modify RESULT, and since
          ;; NEXT is kept for the next iteration, we take care of not
          ;; modifying it.
          (setf result (copy-seq next)
                (third result) next-is-end-p)
          (multiple-value-bind (current first-p end-encountered-p) (move-to-next next cyclic-p)
            (setf next current)
            ;; Set end marker
            (cond
              (cyclic-p
               (setf next-is-end-p first-p)
               (when (and end-encountered-p (not first-p))
                 (setf (third result) t)))
              (t
               (setf (third result) end-encountered-p)))))
        (values-list result)))))

;;; Misc

(defun path-clone (path)
  (let ((new-interpolations (copy-seq (path-interpolations path))))
    (loop for i below (length new-interpolations)
       do (setf (aref new-interpolations i)
                (interpolation-clone (aref new-interpolations i))))
    (let ((new-path (create-path (path-type path))))
      (setf (path-knots new-path) (copy-seq (path-knots path))
            (path-interpolations new-path) new-interpolations)
      new-path)))

(defun path-reverse (path)
  ;; reverse the order of knots
  (setf (path-knots path) (nreverse (path-knots path)))
  ;; reverse the order of interpolations 1..n (not the first one,
  ;; which is the implicit straight line.)
  (loop with interpolations = (path-interpolations path)
     with length = (length interpolations)
     for i from 1 upto (floor (1- length) 2)
     do (rotatef (aref interpolations i)
                 (aref interpolations (- length i))))
  ;; reverse each interpolation
  (loop for interpolation across (path-interpolations path)
     do (interpolation-reverse interpolation)))

(defun path-reversed (path)
  (let ((new-path (path-clone path)))
    (path-reverse new-path)
    new-path))

(defmacro do-path ((path interpolation knot) &body body)
  (let ((path-sym (gensym))
        (knots (gensym))
        (interpolations (gensym))
        (index (gensym)))
    `(symbol-macrolet ((,interpolation (aref ,interpolations ,index))
                       (,knot (aref ,knots ,index)))
       (loop
          with ,path-sym = ,path
          with ,knots = (path-knots ,path-sym)
          with ,interpolations = (path-interpolations ,path-sym)
          for ,index below (length ,knots)
          do (progn ,@body)))))

(defun path-translate (path vector)
  "Translate the whole path accordingly to VECTOR."
  (if (listp path)
      (dolist (path-item path)
        (path-translate path-item vector))
      (unless (and (zerop (point-x vector))
                   (zerop (point-y vector)))
        (do-path (path interpolation knot)
          (setf knot (p+ knot vector))
          (interpolation-translate interpolation vector))))
  path)

(defun path-rotate (path angle &optional center)
  "Rotate the whole path by ANGLE radian around CENTER (which is
the origin if unspecified.)"
  (if (listp path)
      (dolist (path-item path)
        (path-rotate path-item angle center))
      (unless (zerop angle)
        (when center
          (path-translate path (p* center -1.0)))
        (do-path (path interpolation knot)
          (setf knot (point-rotate knot angle))
          (interpolation-rotate interpolation angle))
        (when center
          (path-translate path center))))
  path)

(defun path-scale (path scale-x scale-y &optional center)
  "Scale the whole path by (SCALE-X,SCALE-Y) from CENTER (which
is the origin if unspecified.) Warning: not all interpolations
support non uniform scaling (when scale-x /= scale-y)."
  (if (listp path)
      (dolist (path-item path)
        (path-scale path-item scale-x scale-y center))
      (progn
        (when center
          (path-translate path (p* center -1.0)))
        (do-path (path interpolation knot)
          (setf knot (p* knot scale-x scale-y))
          (interpolation-scale interpolation scale-x scale-y))
        (when center
          (path-translate path center))
        (when (minusp (* scale-x scale-y))
          (path-reverse path))))
  path)

;;;--[ Interpolations ]------------------------------------------------------

(defgeneric interpolation-segment (interpolation k1 k2 function)
  (:documentation "Segment the path between K1 and K2 described
by the INTERPOLATION. Call FUNCTION for each generated point on
the interpolation path."))

(defgeneric interpolation-normal (interpolation k1 k2 side)
  (:documentation "Compute the normal, going \"outside\" at
either K1 (if SIDE is false) or K2 (if SIDE is true). Return NIL
if the normal cannot be computed. Return a point otherwise."))

(defgeneric interpolation-clone (interpolation)
  (:documentation "Duplicate INTERPOLATION."))

(defgeneric interpolation-reverse (interpolation)
  (:documentation "Reverse the path described by INTERPOLATION
in-place."))

(defgeneric interpolation-reversed (interpolation)
  (:method (interpolation)
    (let ((cloned-interpolation (interpolation-clone interpolation)))
      (interpolation-reversed cloned-interpolation)
      cloned-interpolation))
  (:documentation "Duplicate and reverse the INTERPOLATION."))

(defgeneric interpolation-translate (interpolation vector))

(defgeneric interpolation-rotate (interpolation angle))

(defgeneric interpolation-scale (interpolation scale-x scale-y))

;;; Straight lines

(defun make-straight-line ()
  :straight-line)

(defun straight-line-p (value)
  (eq value :straight-line))

(defmethod interpolation-segment ((interpolation (eql :straight-line)) k1 k2 function)
  (declare (ignore interpolation k1 k2 function)))

(defmethod interpolation-normal ((interpolation (eql :straight-line)) k1 k2 side)
  (let* ((x1 (point-x k1))
         (y1 (point-y k1))
         (x2 (point-x k2))
         (y2 (point-y k2))
         (dx (- x2 x1))
         (dy (- y2 y1))
         (dist (sqrt (+ (expt dx 2) (expt dy 2)))))
    (when (plusp dist)
      (if side
          (make-point (/ dx dist)
                      (/ dy dist))
          (make-point (- (/ dx dist))
                      (- (/ dy dist)))))))

(defmethod interpolation-clone ((interpolation (eql :straight-line)))
  (make-straight-line))

(defmethod interpolation-reverse ((interpolation (eql :straight-line)))
  (declare (ignore interpolation)))

(defmethod interpolation-translate ((interpolation (eql :straight-line)) vector)
  (declare (ignore interpolation vector)))

(defmethod interpolation-rotate ((interpolation (eql :straight-line)) angle)
  (declare (ignore interpolation angle)))

(defmethod interpolation-scale ((interpolation (eql :straight-line)) scale-x scale-y)
  (declare (ignore interpolation scale-x scale-y)))

;;; Arc (SVG style)

(defclass arc ()
  ((rx :initarg rx)
   (ry :initarg ry)
   (x-axis-rotation :initarg x-axis-rotation)
   (large-arc-flag :initarg large-arc-flag) ; t = choose the longest arc, nil = choose the smallest arc
   (sweep-flag :initarg sweep-flag))) ; t = arc on the right, nil = arc on the left

(defun make-arc (rx ry &key (x-axis-rotation 0.0) (large-arc-flag nil) (sweep-flag nil))
  (make-instance 'arc
                 'rx rx
                 'ry ry
                 'x-axis-rotation x-axis-rotation
                 'large-arc-flag large-arc-flag
                 'sweep-flag sweep-flag))

(defun svg-arc-parameters/reverse (center rx ry rotation start-angle delta-angle)
  "Conversion from center to endpoint parameterization of SVG arc.

Returns values P1, P2, LARGE-ARC-FLAG-P, SWEEP-FLAG-P."
  (let ((p1 (point-rotate (make-point rx 0) start-angle))
        (p2 (point-rotate (make-point rx 0) (+ start-angle delta-angle))))
    (flet ((transform (p)
             (p+
              (point-rotate
               (p* p 1.0 (/ rx ry))
               rotation)
              center)))
      (values (transform p1) (transform p2)
              (> (abs delta-angle) pi)
              (plusp delta-angle)))))

(defun svg-arc-parameters (p1 p2 rx ry rotation large-arc-flag-p sweep-flag-p)
  "Conversion from endpoint to center parameterization of SVG arc.

Returns values RC, RX, RY, START-ANGLE and DELTA-ANGLE, where RC is
the center of the ellipse, RX and RY are the normalized
radii (needed if scaling was necessary)."
  (when (and (/= rx 0)
             (/= ry 0))
    ;; [SVG] "If rX or rY have negative signs, these are dropped; the
    ;; absolute value is used instead."
    (setf rx (abs rx)
          ry (abs ry))
    ;; normalize boolean value to nil/t
    (setf large-arc-flag-p (when large-arc-flag-p t)
          sweep-flag-p (when sweep-flag-p t))
    ;; rp1 and rp2 are p1 and p2 into the coordinate system such
    ;; that rotation is cancelled and ellipse ratio is 1 (a circle.)
    (let* ((rp1 (p* (point-rotate p1 (- rotation)) 1.0 (/ rx ry)))
           (rp2 (p* (point-rotate p2 (- rotation)) 1.0 (/ rx ry)))
           (rm (point-middle rp1 rp2))
           (drp1 (p- rm rp1))
           (dist (point-norm drp1)))
      (when (plusp dist)
        (let ((diff-sq (- (expt rx 2) (expt dist 2)))
              rc)
          (cond
            ((not (plusp diff-sq))
             ;; a/ scale the arc if it is too small to touch the points
             (setf ry (* dist (/ ry rx))
                   rx dist
                   rc rm))
            (t
             ;; b/ otherwise compute the center of the circle
             (let ((d (/ (sqrt diff-sq) dist)))
               (unless (eq large-arc-flag-p sweep-flag-p)
                 (setf d (- d)))
               (setf rc (make-point (+ (point-x rm) (* (point-y drp1) d))
                                    (- (point-y rm) (* (point-x drp1) d)))))))
          (let* ((start-angle (point-angle (p- rp1 rc)))
                 (end-angle (point-angle (p- rp2 rc)))
                 (delta-angle (- end-angle start-angle)))
            (when (minusp delta-angle)
              (incf delta-angle (* 2 pi)))
            (unless sweep-flag-p
              (decf delta-angle (* 2 pi)))
            (values (point-rotate (p* rc 1.0 (/ ry rx)) rotation) rx ry start-angle delta-angle)))))))

(defmethod interpolation-segment ((interpolation arc) k1 k2 function)
  (let ((rotation (slot-value interpolation 'x-axis-rotation)))
    (multiple-value-bind (rc rx ry start-angle delta-angle)
        (svg-arc-parameters k1 k2
                            (slot-value interpolation 'rx)
                            (slot-value interpolation 'ry)
                            rotation
                            (slot-value interpolation 'large-arc-flag)
                            (slot-value interpolation 'sweep-flag))
      (when rc
        (loop with n = (max 3 (* (max rx ry) (abs delta-angle)))
           for i from 1 below n
           for angle = (+ start-angle (/ (* delta-angle i) n))
           for p = (p+ (point-rotate
                        (p*
                         (make-point (* rx (cos angle))
                                     (* rx (sin angle)))
                         1.0 (/ ry rx))
                        rotation)
                       rc)
           do (funcall function p))))))

(defmethod interpolation-normal ((interpolation arc) k1 k2 side)
  (let ((rotation (slot-value interpolation 'x-axis-rotation)))
    (multiple-value-bind (rc rx ry start-angle delta-angle)
        (svg-arc-parameters k1 k2
                            (slot-value interpolation 'rx)
                            (slot-value interpolation 'ry)
                            rotation
                            (slot-value interpolation 'large-arc-flag)
                            (slot-value interpolation 'sweep-flag))
      (flet ((adjust (normal)
               (let* ((p (point-rotate (p* normal 1.0 (/ ry rx)) rotation))
                      (d (point-norm p)))
                 (when (plusp delta-angle)
                   (setf d (- d)))
                 (make-point (/ (point-x p) d) (/ (point-y p) d)))))
        (when rc
          (let ((end-angle (+ start-angle delta-angle)))
            (adjust (if side
                        (make-point (sin end-angle)
                                    (- (cos end-angle)))
                        (make-point (- (sin start-angle))
                                    (cos start-angle))))))))))

(defmethod interpolation-clone ((interpolation arc))
  (make-arc (slot-value interpolation 'rx)
            (slot-value interpolation 'ry)
            :x-axis-rotation (slot-value interpolation 'x-axis-rotation)
            :large-arc-flag (slot-value interpolation 'large-arc-flag)
            :sweep-flag (slot-value interpolation 'sweep-flag)))

(defmethod interpolation-reverse ((interpolation arc))
  (setf (slot-value interpolation 'sweep-flag)
        (not (slot-value interpolation 'sweep-flag))))

(defmethod interpolation-translate ((interpolation arc) vector)
  (declare (ignore interpolation vector)))

(defmethod interpolation-rotate ((interpolation arc) angle)
  (incf (slot-value interpolation 'x-axis-rotation) angle))

(defmethod interpolation-scale ((interpolation arc) scale-x scale-y)
  ;; FIXME: Return :segment-me if scaling is not possible?
  (assert (and (not (zerop scale-x))
               (= scale-x scale-y)))
  (with-slots (rx ry) interpolation
    (setf rx (* rx scale-x)
          ry (* ry scale-y))))

;;; Catmull-Rom

(defclass catmull-rom ()
  ((head
    :initarg head)
   (control-points
    :initform (make-array 0)
    :initarg control-points)
   (queue
    :initarg queue)))

(defun make-catmull-rom (head control-points queue)
  (make-instance 'catmull-rom
                 'head head
                 'control-points (coerce control-points 'vector)
                 'queue queue))

(defmethod interpolation-segment ((interpolation catmull-rom) k1 k2 function)
  (let* ((control-points (slot-value interpolation 'control-points))
         (points (make-array (+ (length control-points) 4))))
    (replace points control-points :start1 2)
    (setf (aref points 0) (slot-value interpolation 'head)
          (aref points 1) k1
          (aref points (- (length points) 2)) k2
          (aref points (- (length points) 1)) (slot-value interpolation 'queue))
    (labels ((eval-catmull-rom (a b c d p)
               ;; http://www.mvps.org/directx/articles/catmull/
               (* 0.5
                  (+ (* 2 b)
                     (* (+ (- a) c) p)
                     (* (+ (* 2 a) (* -5 b) (* 4 c) (- d)) (expt p 2))
                     (* (+ (- a) (* 3 b) (* -3 c) d) (expt p 3))))))
      (loop for s below (- (length points) 3)
         for a = (aref points (+ s 0)) then b
         for b = (aref points (+ s 1)) then c
         for c = (aref points (+ s 2)) then d
         for d = (aref points (+ s 3))
         do (funcall function b)
         (loop with n = 32
            for i from 1 below n
            for p = (/ (coerce i 'float) n)
            for x = (eval-catmull-rom (point-x a)
                                      (point-x b)
                                      (point-x c)
                                      (point-x d)
                                      p)
            for y = (eval-catmull-rom (point-y a)
                                      (point-y b)
                                      (point-y c)
                                      (point-y d)
                                      p)
            do (funcall function (make-point x y)))
         (funcall function c)))))

(defmethod interpolation-normal ((interpolation catmull-rom) k1 k2 side)
  (with-slots (head control-points queue) interpolation
    (let (a b)
      (if (zerop (length control-points))
          (if side
              (setf a k1
                    b queue)
              (setf a k2
                    b head))
          (if side
              (setf a (aref control-points (1- (length control-points)))
                    b queue)
              (setf a (aref control-points 0)
                    b head)))
      (let* ((x1 (point-x a))
             (y1 (point-y a))
             (x2 (point-x b))
             (y2 (point-y b))
             (dx (- x2 x1))
             (dy (- y2 y1))
             (dist (sqrt (+ (expt dx 2) (expt dy 2)))))
        (when (plusp dist)
          (make-point (/ dx dist)
                      (/ dy dist)))))))

(defmethod interpolation-clone ((interpolation catmull-rom))
  (make-catmull-rom (slot-value interpolation 'head)
                    (copy-seq (slot-value interpolation 'control-points))
                    (slot-value interpolation 'queue)))

(defmethod interpolation-reverse ((interpolation catmull-rom))
  (rotatef (slot-value interpolation 'head)
           (slot-value interpolation 'queue))
  (nreverse (slot-value interpolation 'control-points)))

(defmethod interpolation-translate ((interpolation catmull-rom) vector)
  (with-slots (head control-points queue) interpolation
    (setf head (p+ head vector)
          queue (p+ queue vector))
    (loop for i below (length control-points)
       do (setf (aref control-points i) (p+ (aref control-points i) vector)))))

(defmethod interpolation-rotate ((interpolation catmull-rom) angle)
  (with-slots (head control-points queue) interpolation
    (setf head (point-rotate head angle)
          queue (point-rotate queue angle))
    (loop for i below (length control-points)
       do (setf (aref control-points i) (point-rotate (aref control-points i) angle)))))

(defmethod interpolation-scale ((interpolation catmull-rom) scale-x scale-y)
  (with-slots (head control-points queue) interpolation
    (setf head (p* head scale-x scale-y)
          queue (p* queue scale-x scale-y))
    (loop for i below (length control-points)
       do (setf (aref control-points i) (p* (aref control-points i)
                                                     scale-x scale-y)))))

;;; Bezier curves

;;; [http://www.fho-emden.de/~hoffmann/bezier18122002.pdf]

(defclass bezier ()
  ((control-points
    :initform (make-array 0)
    :initarg control-points)))

(defun make-bezier-curve (control-points)
  (make-instance 'bezier
                 'control-points (make-array (length control-points)
                                             :initial-contents control-points)))

(defun split-bezier (points &optional (position 0.5))
  "Split the Bezier curve described by POINTS at POSITION into
two Bezier curves of the same degree. Returns the curves as 2
values."
  (let* ((size (length points))
         (stack (make-array size))
         (current points))
    (setf (aref stack 0) points)
    (loop for j from 1 below size
       for next-size from (1- size) downto 1
       do (let ((next (make-array next-size)))
            (loop for i below next-size
               for a = (aref current i)
               for b = (aref current (1+ i))
               do (setf (aref next i)
                        (make-point (+ (* (- 1.0 position) (point-x a))
                                       (* position (point-x b)))
                                    (+ (* (- 1.0 position) (point-y a))
                                       (* position (point-y b))))))
            (setf (aref stack j) next
                  current next)))
    (let ((left (make-array (length points)))
          (right (make-array (length points))))
      (loop for i from 0 below size
         for j from (1- size) downto 0
         do (setf (aref left i) (aref (aref stack i) 0)
                  (aref right i) (aref (aref stack j) i)))
      (values left right))))

(defun evaluate-bezier (points position)
  "Evaluate the point at POSITION on the Bezier curve described
by POINTS."
  (let* ((size (length points))
         (temp (make-array (1- size))))
    (loop for current = points then temp
       for i from (length temp) downto 1
       do (loop for j below i
             for a = (aref current j)
             for b = (aref current (1+ j))
             do (setf (aref temp j)
                      (make-point (+ (* (- 1.0 position) (point-x a))
                                     (* position (point-x b)))
                                  (+ (* (- 1.0 position) (point-y a))
                                     (* position (point-y b)))))))
    (let ((p (aref temp 0)))
      (values (point-x p) (point-y p)))))

(defun discrete-bezier-curve (points function
                              &key
                              (include-ends t)
                              (min-subdivide nil)
                              (max-subdivide 10)
                              (distance-tolerance *bezier-distance-tolerance*)
                              (angle-tolerance *bezier-angle-tolerance*))
  "Subdivize Bezier curve up to certain criterions."
  ;; FIXME: Handle cusps correctly!
  (unless min-subdivide
    (setf min-subdivide (floor (log (1+ (length points)) 2))))
  (labels ((norm (a b)
             (sqrt (+ (expt a 2) (expt b 2))))
           (refine-bezier (points depth)
             (let* ((a (aref points 0))
                    (b (aref points (1- (length points))))
                    (middle-straight (point-middle a b)))
               (multiple-value-bind (bx by) (evaluate-bezier points 0.5)
                 (when (or (< depth min-subdivide)
                           (and (<= depth max-subdivide)
                                (or (> (norm (- bx (point-x middle-straight))
                                             (- by (point-y middle-straight)))
                                       distance-tolerance)
                                    (> (abs (- (atan (- by (point-y a)) (- bx (point-x a)))
                                               (atan (- (point-y b) by) (- (point-x b) bx))))
                                       angle-tolerance))))
                   (multiple-value-bind (a b) (split-bezier points 0.5)
                     (refine-bezier a (1+ depth))
                     (funcall function bx by)
                     (refine-bezier b (1+ depth))))))))
    (when include-ends
      (let ((p (aref points 0)))
        (funcall function (point-x p) (point-y p))))
    (refine-bezier points 0)
    (when include-ends
      (let ((p (aref points (1- (length points)))))
        (funcall function (point-x p) (point-y p)))))
  (values))

(defmethod interpolation-segment ((interpolation bezier) k1 k2 function)
  (with-slots (control-points) interpolation
    (let ((points (make-array (+ 2 (length control-points)))))
      (replace points control-points :start1 1)
      (setf (aref points 0) k1
            (aref points (1- (length points))) k2)
      (discrete-bezier-curve points
                             (lambda (x y) (funcall function (make-point x y)))
                             :include-ends nil))))

(defmethod interpolation-normal ((interpolation bezier) k1 k2 side)
  (let ((control-points (slot-value interpolation 'control-points))
        a b)
    (if (zerop (length control-points))
        (if side
            (setf a k1
                  b k2)
            (setf a k2
                  b k1))
        (if side
            (setf a (aref control-points (1- (length control-points)))
                  b k2)
            (setf a (aref control-points 0)
                  b k1)))
    (let* ((x1 (point-x a))
           (y1 (point-y a))
           (x2 (point-x b))
           (y2 (point-y b))
           (dx (- x2 x1))
           (dy (- y2 y1))
           (dist (sqrt (+ (expt dx 2) (expt dy 2)))))
      (when (plusp dist)
        (make-point (/ dx dist)
                    (/ dy dist))))))

(defmethod interpolation-clone ((interpolation bezier))
  (let ((control-points (copy-seq (slot-value interpolation 'control-points))))
    (loop for i below (length control-points)
       do (setf (aref control-points i) (aref control-points i)))
    (make-bezier-curve control-points)))

(defmethod interpolation-reverse ((interpolation bezier))
  (nreverse (slot-value interpolation 'control-points)))

(defmethod interpolation-translate ((interpolation bezier) vector)
  (with-slots (control-points) interpolation
    (loop for i below (length control-points)
       do (setf (aref control-points i) (p+ (aref control-points i) vector)))))

(defmethod interpolation-rotate ((interpolation bezier) angle)
  (with-slots (control-points) interpolation
    (loop for i below (length control-points)
       do (setf (aref control-points i) (point-rotate (aref control-points i) angle)))))

(defmethod interpolation-scale ((interpolation bezier) scale-x scale-y)
  (with-slots (control-points) interpolation
    (loop for i below (length control-points)
       do (setf (aref control-points i) (p* (aref control-points i)
                                                     scale-x scale-y)))))

;;;--[ Building paths ]------------------------------------------------------

(defun make-discrete-path (path)
  "Construct a path with only straight lines."
  (let ((result (create-path (path-type path)))
        (knots (path-knots path))
        (interpolations (path-interpolations path)))
    (when (plusp (length knots))
      ;; nicer, but slower too.. (But not profiled. Premature optimization?)
      #+nil(loop with iterator = (path-iterator-segmented path)
              for (interpolation knot end-p) = (multiple-value-list (path-iterator-next iterator))
              do (path-extend result interpolation knot)
              until end-p)
      (path-reset result (aref knots 0))
      (loop
         for i below (1- (length knots))
         for k1 = (aref knots i)
         for k2 = (aref knots (1+ i))
         for interpolation = (aref interpolations (1+ i))
         do (interpolation-segment interpolation k1 k2
                                   (lambda (knot)
                                     (path-extend result
                                                  (make-straight-line)
                                                  knot)))
         do (path-extend result (make-straight-line) k2)
         finally (unless (eq (path-type path) :open-polyline)
                   (interpolation-segment (aref interpolations 0) k2 (aref knots 0)
                                          (lambda (knot)
                                            (path-extend result
                                                         (make-straight-line)