lagrange.lisp

(defpackage interpole
  (:use :common-lisp)
  (:use :iterate))

(in-package interpole)

(declaim (optimize (speed 0) (debug 3) (safety 3)))

;;; lagrange
(defun lagrange-interpole (xs ys)
  (let ((xds (iter (for i from 0)
				   (for xi in xs)
				   (collect (iter (for k from 0)
								  (for xk in xs)
								  (unless (= i k)
									(multiply (- xi xk))))))))
	(lambda (x)
	  (iter (for i from 0)
			(for y in ys)
			(for xd in xds)
			(sum (/ (* y
					   (iter (for k from 0)
							 (for xk in xs)
							 (unless (= i k)
							   (multiply (- x xk)))))
					xd))))))

;;; spline

(defun make-huvs (xs ys)
  (values (iter (for xi in xs)
				(for xj in (cdr xs))
				(collect (- xj xi)))
		  (iter (for xi in xs)
				(for xj in (cddr xs))
				(collect (* 2 (- xj xi))))
		  (iter (for xi in xs)
				(for xj in (cdr xs))
				(for xk in (cddr xs))
				(for yi in ys)
				(for yj in (cdr ys))
				(for yk in (cddr ys))
				(collect (* 6 (- (/ (- yk yj) (- xk xj))
								 (/ (- yj yi) (- xj xi))))))))

(defun make-ps (hs us vs)
  (let ((abs (cons (list 0 0)
				   (iter (for hi in hs)
						 (for hj in (cdr hs))
						 (for uj in us)
						 (for vj in vs)
						 (for aj next (/ (- hj) (+ uj (* hi ai))))
						 (for bj next (/ (- vj (* hi bi)) (+ uj (* hi ai))))
						 (for ai previous aj initially 0.0)
						 (for bi previous bj initially 0.0)
						 (collect (list aj bj))))))
	`(,@(iter (for (a b) in (reverse abs))
			  (for p next (+ (* a q) b))
			  (for q previous p initially 0)
			  (collect p at beginning))
		0)))

(defun make-spline-table (xs ys)
  (let ((ps (multiple-value-call #'make-ps (make-huvs xs ys))))
	(iter (for xi in xs)
		  (for xj in (cdr xs))
		  (for yi in ys)
		  (for yj in (cdr ys))
		  (for p in ps)
		  (for q in (cdr ps))
		  (for c0 next yi)
		  (for c1 next (- (/ (- yj yi) (- xj xi))
						  (/ (* (- xj xi) (+ q (* 2 p))) 6.0)))
		  (for c2 next (/ p 2))
		  (for c3 next (/ (- q p) (* 6 (- xj xi))))
		  (collect (list xi c3 c2 c1 c0)))))

(defun spline-interpole (xs ys)
  (let ((table (make-spline-table xs ys)))
	(lambda (x)
	  (iter loop
			(for i in table)
			(for j on table)
			(when (or (null (cdr j)) (< x (caadr j)))
			  (destructuring-bind (xk a b c d) i
				(let ((w (- x xk)))
				  (return-from loop (+ (* a w w w) (* b w w) (* c w) d)))))))))

(defun make-sample (f lower upper num)
  (iter (for i from 0 below num)
		(for x next (+ lower (* i (/ (- upper lower) (1- num)))))
		(collect x into xs)
		(collect (funcall f x) into ys)
		(finally (return (values xs ys)))))

(defun f (x)
  (/ (+ 1.0 (* x x))))

(defun make-plot (f fname lower upper dx)
  (with-open-file (o fname
					 :direction :output
					 :if-does-not-exist :create
					 :if-exists :supersede)
	(iter (for x from lower to upper by dx)
		  (format o "~a ~a~%" x (funcall f x)))))

(defun make-lagrange-plot (f fname lower upper num dx)
  (multiple-value-bind (xs ys) (make-sample f lower upper num)
	(make-plot (lagrange-interpole xs ys)
			   fname lower upper dx)))

(defun make-spline-plot (f fname lower upper num dx)
  (multiple-value-bind (xs ys) (make-sample f lower upper num)
	(make-plot (spline-interpole xs ys)
			   fname lower upper dx)))


(make-plot #'f "f.dat" -10.0 10.0 0.1)
(make-lagrange-plot #'f "lagrange.dat" -10.0 10.0 41 0.1)
(make-spline-plot #'f "spline.dat" -10.0 10.0 41 0.1)