ANSI-fication of in-package; types are parameters and use +; AJR copyright
[CommonLispStat.git] / linalg.lsp
blob083c14d840b8925e0ecc5fa5bf173481f8f6c141
1 ;;; -*- mode: lisp -*-
2 ;;; Copyright (c) 2005--2007, by A.J. Rossini <blindglobe@gmail.com>
3 ;;; See COPYRIGHT file for any additional restrictions (BSD license).
4 ;;; Since 1991, ANSI was finally finished. Edited for ANSI Common Lisp.
5 ;;;
6 ;;; what this should do:
7 ;;; #1 - use CFFI (and possibly Verazanno) to import C/C++.
8 ;;; #2 - what to do for Fortran? Possibly: C <-> bridge, or CLapack?
9 ;;; problem: would be better to have access to Fortran. For
10 ;;; example, think of Doug Bates comment on reverse-calls (as
11 ;;; distinct from callbacks). It would be difficult if we don't
12 ;;; -- however, has anyone run Lapack or similar through F2CL?
13 ;;; Answer: yes, Matlisp does this.
16 ;;;; linalg -- Lisp-Stat interface to basic linear algebra routines.
17 ;;;;
18 ;;;; Copyright (c) 1991, by Luke Tierney. Permission is granted for
19 ;;;; unrestricted use.
21 ;;;;
22 ;;;; Package Setup
23 ;;;;
25 (in-package #:lisp-stat-basics)
27 (export '(chol-decomp lu-decomp lu-solve determinant inverse sv-decomp
28 qr-decomp rcondest make-rotation spline kernel-dens kernel-smooth
29 fft make-sweep-matrix sweep-operator ax+y numgrad numhess
30 split-list eigen))
32 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
33 ;;;;
34 ;;;; Lisp to C number conversion and checking
35 ;;;;
36 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
38 ;;;;
39 ;;;; Lisp to/from C sequence and matrix conversion and checking
40 ;;;;
42 (defun is-cons (a)
43 "FIXME:AJR this is not used anywhere?"
44 (if (consp a) 1 0))
46 (defun check-fixnum (a)
47 (if (/= 0 (la-data-mode a)) (error "not an integer sequence - ~s" a)))
49 (defun check-real (data)
50 (let ((data (compound-data-seq data)))
51 (cond
52 ((vectorp data)
53 (let ((n (length data)))
54 (declare (fixnum n))
55 (dotimes (i n)
56 (declare (fixnum i))
57 (check-one-real (aref data i)))))
58 ((consp data) (dolist (x data) (check-one-real x)))
59 (t (error "bad sequence - ~s" data)))))
61 (defun vec-assign (a i x) (setf (aref a i) x))
63 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
64 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
65 ;;;;
66 ;;;; Lisp Interfaces to Linear Algebra Routines
67 ;;;;
68 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
69 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
71 ;;; FIXME: use dpbt[f2|rf], dpbstf, dpot[f2|rf]; dpptrf, zpbstf, zpbt[f2|rf]
72 ;;; remember: factorization = decomposition, depending on training.
74 (defun chol-decomp (a &optional (maxoffl 0.0))
75 "Args: (a)
76 Modified Cholesky decomposition. A should be a square, symmetric matrix.
77 Computes lower triangular matrix L such that L L^T = A + D where D is a
78 diagonal matrix. If A is strictly positive definite D will be zero.
79 Otherwise, D is as small as possible to make A + D numerically strictly
80 positive definite. Returns a list (L (max D))."
81 (check-square-matrix a)
82 (check-real a)
83 (let* ((n (array-dimension a 0))
84 (result (make-array (list n n)))
85 (dpars (list maxoffl 0.0)))
86 (check-real dpars)
87 (let ((mat (la-data-to-matrix a mode-re))
88 (dp (la-data-to-vector dpars mode-re)))
89 (unwind-protect
90 (progn
91 (chol-decomp-front mat n dp)
92 (la-matrix-to-data mat n n mode-re result)
93 (la-vector-to-data dp 2 mode-re dpars))
94 (la-free-matrix mat n)
95 (la-free-vector dp)))
96 (list result (second dpars))))
99 ;;; REPLACE with
100 ;;; (matlisp:lu M)
101 ;;; i.e. result use by:
102 ;;; (setf (values (lu-out1 lu-out2 lu-out3)) (matlisp:lu my-matrix))
103 ;;; for solution, ...
104 ;;; for lu-solve:
105 ;;; (matlisp:gesv a b &opt ipivot)
107 (defun lu-decomp (a)
108 "Args: (a)
109 A is a square matrix of numbers (real or complex). Computes the LU
110 decomposition of A and returns a list of the form (LU IV D FLAG), where
111 LU is a matrix with the L part in the lower triangle, the U part in the
112 upper triangle (the diagonal entries of L are taken to be 1), IV is a vector
113 describing the row permutation used, D is 1 if the number of permutations
114 is odd, -1 if even, and FLAG is T if A is numerically singular, NIL otherwise.
115 Used bu LU-SOLVE."
116 (check-square-matrix a)
117 (let* ((n (array-dimension a 0))
118 (mode (max mode-re (la-data-mode a)))
119 (result (list (make-array (list n n)) (make-array n) nil nil)))
120 (let ((mat (la-data-to-matrix a mode))
121 (iv (la-vector n mode-in))
122 (d (la-vector 1 mode-re))
123 (singular 0))
124 (unwind-protect
125 (progn
126 (setf singular (lu-decomp-front mat n iv mode d))
127 (la-matrix-to-data mat n n mode (first result))
128 (la-vector-to-data iv n mode-in (second result))
129 (setf (third result) (la-get-double d 0))
130 (setf (fourth result) (if (= singular 0.0) nil t)))
131 (la-free-matrix mat n)
132 (la-free-vector iv)
133 (la-free-vector d)))
134 result))
136 (defun lu-solve (lu lb)
137 "Args: (lu b)
138 LU is the result of (LU-DECOMP A) for a square matrix A, B is a sequence.
139 Returns the solution to the equation Ax = B. Signals an error if A is
140 singular."
141 (let ((la (first lu))
142 (lidx (second lu)))
143 (check-square-matrix la)
144 (check-sequence lidx)
145 (check-sequence lb)
146 (check-fixnum lidx)
147 (let* ((n (num-rows la))
148 (result (make-sequence (if (consp lb) 'list 'vector) n))
149 (a-mode (la-data-mode la))
150 (b-mode (la-data-mode lb)))
151 (if (/= n (length lidx)) (error "index sequence is wrong length"))
152 (if (/= n (length lb)) (error "right hand side is wrong length"))
153 (let* ((mode (max mode-re a-mode b-mode))
154 (a (la-data-to-matrix la mode))
155 (indx (la-data-to-vector lidx mode-in))
156 (b (la-data-to-vector lb mode))
157 (singular 0))
158 (unwind-protect
159 (progn
160 (setf singular (lu-solve-front a n indx b mode))
161 (la-vector-to-data b n mode result))
162 (la-free-matrix a n)
163 (la-free-vector indx)
164 (la-free-vector b))
165 (if (/= 0.0 singular) (error "matrix is (numerically) singular"))
166 result))))
168 (defun determinant (a)
169 "Args: (m)
170 Returns the determinant of the square matrix M."
171 (let* ((lu (lu-decomp a))
172 (la (first lu))
173 (n (num-rows a))
174 (d1 (third lu))
175 (d2 0.d0))
176 (declare (fixnum n))
177 (flet ((fabs (x) (float (abs x) 0.d0)))
178 (dotimes (i n (* d1 (exp d2)))
179 (declare (fixnum i))
180 (let* ((x (aref la i i))
181 (magn (fabs x)))
182 (if (= 0.0 magn) (return 0.d0))
183 (setf d1 (* d1 (/ x magn)))
184 (setf d2 (+ d2 (log magn))))))))
186 (defun inverse (a)
187 "Args: (m)
188 Returns the inverse of the the square matrix M; signals an error if M is ill
189 conditioned or singular"
190 (check-square-matrix a)
191 (let ((n (num-rows a))
192 (mode (max mode-re (la-data-mode a))))
193 (declare (fixnum n))
194 (let ((result (make-array (list n n) :initial-element 0)))
195 (dotimes (i n)
196 (declare (fixnum i))
197 (setf (aref result i i) 1))
198 (let ((mat (la-data-to-matrix a mode))
199 (inv (la-data-to-matrix result mode))
200 (iv (la-vector n mode-in))
201 (v (la-vector n mode))
202 (singular 0))
203 (unwind-protect
204 (progn
205 (setf singular (lu-inverse-front mat n iv v mode inv))
206 (la-matrix-to-data inv n n mode result))
207 (la-free-matrix mat n)
208 (la-free-matrix inv n)
209 (la-free-vector iv)
210 (la-free-vector v))
211 (if (/= singular 0) (error "matrix is (numerically) singular"))
212 result))))
214 ;;;;
215 ;;;; SV Decomposition
216 ;;;;
218 (defun sv-decomp (a)
219 "Args: (a)
220 A is a matrix of real numbers with at least as many rows as columns.
221 Computes the singular value decomposition of A and returns a list of the form
222 (U W V FLAG) where U and V are matrices whose columns are the left and right
223 singular vectors of A and W is the sequence of singular values of A. FLAG is T
224 if the algorithm converged, NIL otherwise."
225 (check-matrix a)
226 (let* ((m (num-rows a))
227 (n (num-cols a))
228 (mode (max mode-re (la-data-mode a)))
229 (result (list (make-array (list m n))
230 (make-array n)
231 (make-array (list n n))
232 nil)))
233 (if (< m n) (error "number of rows less than number of columns"))
234 (if (= mode mode-cx) (error "complex SVD not available yet"))
235 (let ((mat (la-data-to-matrix a mode))
236 (w (la-vector n mode-re))
237 (v (la-matrix n n mode-re))
238 (converged 0))
239 (unwind-protect
240 (progn
241 (setf converged (sv-decomp-front mat m n w v))
242 (la-matrix-to-data mat m n mode (first result))
243 (la-vector-to-data w n mode (second result))
244 (la-matrix-to-data v n n mode (third result))
245 (setf (fourth result) (if (/= 0.0 converged) t nil)))
246 (la-free-matrix mat m)
247 (la-free-vector w)
248 (la-free-matrix v n))
249 result)))
252 ;;;;
253 ;;;; QR Decomposition
254 ;;;;
256 (defun qr-decomp (a &optional pivot)
257 "Args: (a &optional pivot)
258 A is a matrix of real numbers with at least as many rows as columns. Computes
259 the QR factorization of A and returns the result in a list of the form (Q R).
260 If PIVOT is true the columns of X are first permuted to place insure the
261 absolute values of the diagonal elements of R are nonincreasing. In this case
262 the result includes a third element, a list of the indices of the columns in
263 the order in which they were used."
264 (check-matrix a)
265 (let* ((m (num-rows a))
266 (n (num-cols a))
267 (mode (max mode-re (la-data-mode a)))
268 (p (if pivot 1 0))
269 (result (if pivot
270 (list (make-array (list m n))
271 (make-array (list n n))
272 (make-array n))
273 (list (make-array (list m n)) (make-array (list n n))))))
274 (if (< m n) (error "number of rows less than number of columns"))
275 (if (= mode mode-cx) (error "complex QR decomposition not available yet"))
276 (let ((mat (la-data-to-matrix a mode))
277 (v (la-matrix n n mode-re))
278 (jpvt (la-vector n mode-in)))
279 (unwind-protect
280 (progn
281 (qr-decomp-front mat m n v jpvt p)
282 (la-matrix-to-data mat m n mode (first result))
283 (la-matrix-to-data v n n mode (second result))
284 (if pivot (la-vector-to-data jpvt n mode-in (third result))))
285 (la-free-matrix mat m)
286 (la-free-matrix v n)
287 (la-free-vector jpvt))
288 result)))
290 ;;;;
291 ;;;; Estimate of Condition Number for Lower Triangular Matrix
292 ;;;;
294 (defun rcondest (a)
295 "Args: (a)
296 Returns an estimate of the reciprocal of the L1 condition number of an upper
297 triangular matrix a."
298 (check-square-matrix a)
299 (let ((mode (max mode-re (la-data-mode a)))
300 (n (num-rows a)))
301 (if (= mode mode-cx)
302 (error "complex condition estimate not available yet"))
303 (let ((mat (la-data-to-matrix a mode))
304 (est 0.0))
305 (unwind-protect
306 (setf est (rcondest-front mat n))
307 (la-free-matrix mat n))
308 est)))
310 ;;;;
311 ;;;; Make Rotation Matrix
312 ;;;;
314 (defun make-rotation (x y &optional alpha)
315 "Args: (x y &optional alpha)
316 Returns a rotation matrix for rotating from X to Y, or from X toward Y
317 by angle ALPHA, in radians. X and Y are sequences of the same length."
318 (check-sequence x)
319 (check-sequence y)
320 (if alpha (check-one-real alpha))
321 (let* ((n (length x))
322 (mode (max mode-re (la-data-mode x) (la-data-mode y)))
323 (use-angle (if alpha 1 0))
324 (angle (if alpha (float alpha 0.0) 0.0))
325 (result (make-array (list n n))))
326 (if (/= n (length y)) (error "sequences not the same length"))
327 (if (= mode mode-cx) (error "complex data not supported yet"))
328 (let ((px (la-data-to-vector x mode-re))
329 (py (la-data-to-vector y mode-re))
330 (rot (la-matrix n n mode-re)))
331 (unwind-protect
332 (progn
333 (make-rotation-front n rot px py use-angle angle)
334 (la-matrix-to-data rot n n mode-re result))
335 (la-free-vector px)
336 (la-free-vector py)
337 (la-free-matrix rot n))
338 result)))
340 ;;;;
341 ;;;; Eigenvalues and Vectors
342 ;;;;
344 (defun eigen (a)
345 "Args: (a)
346 Returns list of list of eigenvalues and list of eigenvectors of square,
347 symmetric matrix A. Third element of result is NIL if algorithm converges.
348 If the algorithm does not converge, the third element is an integer I.
349 In this case the eigenvalues 0, ..., I are not reliable."
350 (check-square-matrix a)
351 (let ((mode (max mode-re (la-data-mode a)))
352 (n (num-rows a)))
353 (if (= mode mode-cx) (error "matrix must be real and symmetric"))
354 (let ((evals (make-array n))
355 (evecs (make-list (* n n)))
356 (pa (la-data-to-vector (compound-data-seq a) mode-re))
357 (w (la-vector n mode-re))
358 (z (la-vector (* n n) mode-re))
359 (fv1 (la-vector n mode-re))
360 (ierr 0))
361 (unwind-protect
362 (progn
363 (setf ierr (eigen-front pa n w z fv1))
364 (la-vector-to-data w n mode-re evals)
365 (la-vector-to-data z (* n n) mode-re evecs))
366 (la-free-vector pa)
367 (la-free-vector z)
368 (la-free-vector w)
369 (la-free-vector fv1))
370 (list (nreverse evals)
371 (nreverse (mapcar #'(lambda (x) (coerce x 'vector))
372 (split-list evecs n)))
373 (if (/= 0 ierr) (- n ierr))))))
375 ;;;;
376 ;;;; Spline Interpolation
377 ;;;;
379 (defun make-smoother-args (x y xvals)
380 (check-sequence x)
381 (check-real x)
382 (when y
383 (check-sequence y)
384 (check-real y))
385 (unless (integerp xvals)
386 (check-sequence xvals)
387 (check-real xvals))
388 (let* ((n (length x))
389 (ns (if (integerp xvals) xvals (length xvals)))
390 (result (list (make-list ns) (make-list ns))))
391 (if (and y (/= n (length y))) (error "sequences not the same length"))
392 (list x y n (if (integerp xvals) 0 1) ns xvals result)))
394 (defun get-smoother-result (args) (seventh args))
396 (defmacro with-smoother-data ((x y xvals is-reg) &rest body)
397 `(progn
398 (check-sequence ,x)
399 (check-real ,x)
400 (when ,is-reg
401 (check-sequence ,y)
402 (check-real ,y))
403 (unless (integerp ,xvals)
404 (check-sequence ,xvals)
405 (check-real ,xvals))
406 (let* ((supplied (not (integerp ,xvals)))
407 (n (length ,x))
408 (ns (if supplied (length ,xvals) ,xvals))
409 (result (list (make-list ns) (make-list ns))))
410 (if (and ,is-reg (/= n (length ,y)))
411 (error "sequences not the same length"))
412 (if (and (not supplied) (< ns 2))
413 (error "too few points for interpolation"))
414 (let* ((px (la-data-to-vector ,x mode-re))
415 (py (if ,is-reg (la-data-to-vector ,y mode-re)))
416 (pxs (if supplied
417 (la-data-to-vector ,xvals mode-re)
418 (la-vector ns mode-re)))
419 (pys (la-vector ns mode-re)))
420 (unless supplied (la-range-to-rseq n px ns pxs))
421 (unwind-protect
422 (progn ,@body
423 (la-vector-to-data pxs ns mode-re (first result))
424 (la-vector-to-data pys ns mode-re (second result)))
425 (la-free-vector px)
426 (if ,is-reg (la-free-vector py))
427 (la-free-vector pxs)
428 (la-free-vector pys))
429 result))))
431 (defun spline (x y &key (xvals 30))
432 "Args: (x y &key xvals)
433 Returns list of x and y values of natural cubic spline interpolation of (X,Y).
434 X must be strictly increasing. XVALS can be an integer, the number of equally
435 spaced points to use in the range of X, or it can be a sequence of points at
436 which to interpolate."
437 (with-smoother-data (x y xvals t)
438 (let ((work (la-vector (* 2 n) mode-re))
439 (error 0))
440 (unwind-protect
441 (setf error (spline-front n px py ns pxs pys work))
442 (la-free-vector work))
443 (if (/= error 0) (error "bad data for splines")))))
445 ;;;;
446 ;;;; Kernel Density Estimators and Smoothers
447 ;;;;
449 (defun kernel-type-code (type)
450 (cond ((eq type 'u) 0)
451 ((eq type 't) 1)
452 ((eq type 'g) 2)
453 (t 3)))
455 (defun kernel-dens (x &key (type 'b) (width -1.0) (xvals 30))
456 "Args: (x &key xvals width type)
457 Returns list of x and y values of kernel density estimate of X. XVALS can be an
458 integer, the number of equally spaced points to use in the range of X, or it
459 can be a sequence of points at which to interpolate. WIDTH specifies the
460 window width. TYPE specifies the lernel and should be one of the symbols G, T,
461 U or B for gaussian, triangular, uniform or bisquare. The default is B."
462 (check-one-real width)
463 (with-smoother-data (x nil xvals nil) ;; warning about deleting unreachable code is TRUE -- 2nd arg=nil!
464 (let ((code (kernel-type-code type))
465 (error 0))
466 (setf error (kernel-dens-front px n width pxs pys ns code))
467 (if (/= 0 error) (error "bad kernel density data")))))
469 (defun kernel-smooth (x y &key (type 'b) (width -1.0) (xvals 30))
470 "Args: (x y &key xvals width type)
471 Returns list of x and y values of kernel smooth of (X,Y). XVALS can be an
472 integer, the number of equally spaced points to use in the range of X, or it
473 can be a sequence of points at which to interpolate. WIDTH specifies the
474 window width. TYPE specifies the lernel and should be one of the symbols G, T,
475 U or B for Gaussian, triangular, uniform or bisquare. The default is B."
476 (check-one-real width)
477 (with-smoother-data (x y xvals t)
478 (let ((code (kernel-type-code type))
479 (error 0))
480 ;;(kernel-smooth-front px py n width pxs pys ns code)
481 (kernel-smooth-Cport px py n width pxs pys ns code)
482 (if (/= 0 error) (error "bad kernel density data")))))
484 (defun kernel-smooth-Cport (px py n width ;;wts wds ;; see above for mismatch?
485 xs ys ns ktype)
486 "Port of kernel_smooth (Lib/kernel.c) to Lisp.
487 FIXME:kernel-smooth-Cport"
488 (cond ((< n 1) 1.0)
489 ((and (< n 2) (<= width 0)) 1.0)
490 (t (let* ((xmin (min px))
491 (xmax (max px))
492 (width (/ (- xmax xmin) (+ 1.0 (log n)))))
493 (dotimes (i (- ns 1))
494 (setf (aref ys i)
495 (let ((wsum 0.0)
496 (ysum 0.0))
497 (dotimes (j (- n 1))
498 (let* ;; FIXME!?
499 ((lwidth (if wds (* width (aref wds j)) width))
500 (lwt (* (kernel-Cport (aref xs i) (aref px j) lwidth ktype) ;; px?
501 (if wts (aref wts j) 1.0))))
502 (setf wsum (+ wsum lwt))
503 (setf ysum (if py (+ ysum (* lwt (aref py j))))))) ;; py? y?
504 (if py
505 (if (> wsum 0.0)
506 (/ ysum wsum)
507 0.0)
508 (/ wsum n)))))
509 (values ys)))))
511 (defun kernel-Cport (x y w ktype)
512 "Port of kernel() (Lib/kernel.c) to Lisp.
513 x,y,w are doubles, type is an integer"
514 (if (<= w 0.0)
516 (let ((z (- x y)))
517 (cond ((eq ktype "B")
518 (let* ((w (* w 2.0))
519 (z (* z 0.5)))
520 (if (and (> z -0.5)
521 (< z 0.5))
522 (/ (/ (* 15.0 (* (- 1.0 (* 4 z z)) ;; k/w
523 (- 1.0 (* 4 z z)))) ;; k/w
524 8.0)
526 0)))
527 ((eq ktype "G")
528 (let* ((w (* w 0.25))
529 (z (* z 4.0))
530 (k (/ (exp (* -0.5 z z))
531 (sqrt (* 2 PI)))))
532 (/ k w)))
533 ((eq ktype "U")
534 (let* ((w (* 1.5 w))
535 (z (* z 0.75))
536 (k (if (< (abs z) 0.5)
538 0.0)))
539 (/ k w)))
540 ((eq ktype "T")
541 (cond ((and (> z -1.0)
542 (< z 0.0))
543 (+ 1.0 z)) ;; k
544 ((and (> z 0.0)
545 (< z 1.0))
546 (- 1.0 z)) ;; k
547 (t 0.0)))
548 (t (values 0.0))))))
551 ;;;;
552 ;;;; Lowess Smoother Interface
553 ;;;;
555 (defun |base-lowess| (s1 s2 f nsteps delta)
556 (check-sequence s1)
557 (check-sequence s2)
558 (check-real s1)
559 (check-real s2)
560 (check-one-real f)
561 (check-one-fixnum nsteps)
562 (check-one-real delta)
563 (let* ((n (length s1))
564 (result (make-list n)))
565 (if (/= n (length s2)) (error "sequences not the same length"))
566 (let ((x (la-data-to-vector s1 mode-re))
567 (y (la-data-to-vector s2 mode-re))
568 (ys (la-vector n mode-re))
569 (rw (la-vector n mode-re))
570 (res (la-vector n mode-re))
571 (error 0))
572 (unwind-protect
573 (progn
574 (setf error (base-lowess-front x y n f nsteps delta ys rw res))
575 (la-vector-to-data ys n mode-re result))
576 (la-free-vector x)
577 (la-free-vector y)
578 (la-free-vector ys)
579 (la-free-vector rw)
580 (la-free-vector res))
581 (if (/= error 0) (error "bad data for lowess"))
582 result)))
585 static LVAL add_contour_point(i, j, k, l, x, y, z, v, result)
586 int i, j, k, l;
587 RVector x, y;
588 RMatrix z;
589 double v;
590 LVAL result;
592 LVAL pt;
593 double p, q;
595 if ((z[i][j] <= v && v < z[k][l]) || (z[k][l] <= v && v < z[i][j])) {
596 xlsave(pt);
597 pt = mklist(2, NIL);
598 p = (v - z[i][j]) / (z[k][l] - z[i][j]);
599 q = 1.0 - p;
600 rplaca(pt, cvflonum((FLOTYPE) (q * x[i] + p * x[k])));
601 rplaca(cdr(pt), cvflonum((FLOTYPE) (q * y[j] + p * y[l])));
602 result = cons(pt, result);
603 xlpop();
605 return(result);
608 LVAL xssurface_contour()
610 LVAL s1, s2, mat, result;
611 RVector x, y;
612 RMatrix z;
613 double v;
614 int i, j, n, m;
616 s1 = xsgetsequence();
617 s2 = xsgetsequence();
618 mat = xsgetmatrix();
619 v = makedouble(xlgetarg());
620 xllastarg();
622 n = seqlen(s1); m = seqlen(s2);
623 if (n != numrows(mat) || m != numcols(mat)) xlfail("dimensions do not match");
624 if (data_mode(s1) == CX || data_mode(s2) == CX || data_mode(mat) == CX)
625 xlfail("data must be real");
627 x = (RVector) data_to_vector(s1, RE);
628 y = (RVector) data_to_vector(s2, RE);
629 z = (RMatrix) data_to_matrix(mat, RE);
631 xlsave1(result);
632 result = NIL;
633 for (i = 0; i < n - 1; i++) {
634 for (j = 0; j < m - 1; j++) {
635 result = add_contour_point(i, j, i, j+1, x, y, z, v, result);
636 result = add_contour_point(i, j+1, i+1, j+1, x, y, z, v, result);
637 result = add_contour_point(i+1, j+1, i+1, j, x, y, z, v, result);
638 result = add_contour_point(i+1, j, i, j, x, y, z, v, result);
641 xlpop();
643 free_vector(x);
644 free_vector(y);
645 free_matrix(z, n);
647 return(result);
652 ;;; FFT
654 ;;; FIXME:ajr
655 ;;; replace with matlisp:fft and matlisp:ifft (the latter for inverse mapping)
657 (defun fft (x &optional inverse)
658 "Args: (x &optional inverse)
659 Returns unnormalized Fourier transform of X, or inverse transform if INVERSE
660 is true."
661 (check-sequence x)
662 (let* ((n (length x))
663 (mode (la-data-mode x))
664 (isign (if inverse -1 1))
665 (result (if (consp x) (make-list n) (make-array n))))
666 (let ((px (la-data-to-vector x mode-cx))
667 (work (la-vector (+ (* 4 n) 15) mode-re)))
668 (unwind-protect
669 (progn
670 (fft-front n px work isign)
671 (la-vector-to-data px n mode-cx result))
672 (la-free-vector px)
673 (la-free-vector work))
674 result)))
677 ;;; SWEEP Operator: FIXME: use matlisp
680 (defun make-sweep-front (x y w n p mode has_w x_mean result)
681 (declare (fixnum n p mode has_w))
682 (let ((x_data nil)
683 (result_data nil)
684 (val 0.0)
685 (dxi 0.0)
686 (dyi 0.0)
687 (dv 0.0)
688 (dw 0.0)
689 (sum_w 0.0)
690 (dxik 0.0)
691 (dxjk 0.0)
692 (dyj 0.0)
693 (dx_meani 0.0)
694 (dx_meanj 0.0)
695 (dy_mean 0.0)
696 (has-w (if (/= 0 has_w) t nil))
697 (RE 1))
698 (declare (long-float val dxi dyi dv dw sum_w dxik dxjk dyj
699 dx_meani dx_meanj dy_mean))
700 ;; (declare-double val dxi dyi dv dw sum_w dxik dxjk dyj
701 ;; dx_meani dx_meanj dy_mean)
703 (if (> mode RE) (error "not supported for complex data yet"))
705 (setf x_data (compound-data-seq x))
706 (setf result_data (compound-data-seq result))
708 ;; find the mean of y
709 (setf val 0.0)
710 (setf sum_w 0.0)
711 (dotimes (i n)
712 (declare (fixnum i))
713 (setf dyi (makedouble (aref y i)))
714 (when has-w
715 (setf dw (makedouble (aref w i)))
716 (incf sum_w dw)
717 (setf dyi (* dyi dw)))
718 (incf val dyi))
719 (if (not has-w) (setf sum_w (float n 0.0)))
720 (if (<= sum_w 0.0) (error "non positive sum of weights"))
721 (setf dy_mean (/ val sum_w))
723 ;; find the column means
724 (dotimes (j p)
725 (declare (fixnum j))
726 (setf val 0.0)
727 (dotimes (i n)
728 (declare (fixnum i))
729 (setf dxi (makedouble (aref x_data (+ (* p i) j))))
730 (when has-w
731 (setf dw (makedouble (aref w i)))
732 (setf dxi (* dxi dw)))
733 (incf val dxi))
734 (setf (aref x_mean j) (/ val sum_w)))
736 ;; put 1/sum_w in topleft, means on left, minus means on top
737 (setf (aref result_data 0) (/ 1.0 sum_w))
738 (dotimes (i p)
739 (declare (fixnum i))
740 (setf dxi (makedouble (aref x_mean i)))
741 (setf (aref result_data (+ i 1)) (- dxi))
742 (setf (aref result_data (* (+ i 1) (+ p 2))) dxi))
743 (setf (aref result_data (+ p 1)) (- dy_mean))
744 (setf (aref result_data (* (+ p 1) (+ p 2))) dy_mean)
746 ;; put sums of adjusted cross products in body
747 (dotimes (i p)
748 (declare (fixnum i))
749 (dotimes (j p)
750 (declare (fixnum j))
751 (setf val 0.0)
752 (dotimes (k n)
753 (declare (fixnum k))
754 (setf dxik (makedouble (aref x_data (+ (* p k) i))))
755 (setf dxjk (makedouble (aref x_data (+ (* p k) j))))
756 (setf dx_meani (makedouble (aref x_mean i)))
757 (setf dx_meanj (makedouble (aref x_mean j)))
758 (setf dv (* (- dxik dx_meani) (- dxjk dx_meanj)))
759 (when has-w
760 (setf dw (makedouble (aref w k)))
761 (setf dv (* dv dw)))
762 (incf val dv))
763 (setf (aref result_data (+ (* (+ i 1) (+ p 2)) (+ j 1))) val)
764 (setf (aref result_data (+ (* (+ j 1) (+ p 2)) (+ i 1))) val))
765 (setf val 0.0)
766 (dotimes (j n)
767 (declare (fixnum j))
768 (setf dxik (makedouble (aref x_data (+ (* p j) i))))
769 (setf dyj (makedouble (aref y j)))
770 (setf dx_meani (makedouble (aref x_mean i)))
771 (setf dv (* (- dxik dx_meani) (- dyj dy_mean)))
772 (when has-w
773 (setf dw (makedouble (aref w j)))
774 (setf dv (* dv dw)))
775 (incf val dv))
776 (setf (aref result_data (+ (* (+ i 1) (+ p 2)) (+ p 1))) val)
777 (setf (aref result_data (+ (* (+ p 1) (+ p 2)) (+ i 1))) val))
778 (setf val 0.0)
779 (dotimes (j n)
780 (declare (fixnum j))
781 (setf dyj (makedouble (aref y j)))
782 (setf dv (* (- dyj dy_mean) (- dyj dy_mean)))
783 (when has-w
784 (setf dw (makedouble (aref w j)))
785 (setf dv (* dv dw)))
786 (incf val dv))
787 (setf (aref result_data (+ (* (+ p 1) (+ p 2)) (+ p 1))) val)))
789 ;;; FIXME: use matlisp
790 (defun sweep-in-place-front (a rows cols mode k tol)
791 "Sweep algorithm for linear regression."
792 (declare (long-float tol))
793 (declare (fixnum rows cols mode k))
794 (let ((data nil)
795 (pivot 0.0)
796 (aij 0.0)
797 (aik 0.0)
798 (akj 0.0)
799 (akk 0.0)
800 (RE 1))
801 (declare (long-float pivot aij aik akj akk))
803 (if (> mode RE) (error "not supported for complex data yet"))
804 (if (or (< k 0) (>= k rows) (>= k cols)) (error "index out of range"))
806 (setf tol (max tol machine-epsilon))
807 (setf data (compound-data-seq a))
809 (setf pivot (makedouble (aref data (+ (* cols k) k))))
811 (cond
812 ((or (> pivot tol) (< pivot (- tol)))
813 (dotimes (i rows)
814 (declare (fixnum i))
815 (dotimes (j cols)
816 (declare (fixnum j))
817 (when (and (/= i k) (/= j k))
818 (setf aij (makedouble (aref data (+ (* cols i) j))))
819 (setf aik (makedouble (aref data (+ (* cols i) k))))
820 (setf akj (makedouble (aref data (+ (* cols k) j))))
821 (setf aij (- aij (/ (* aik akj) pivot)))
822 (setf (aref data (+ (* cols i) j)) aij))))
824 (dotimes (i rows)
825 (declare (fixnum i))
826 (setf aik (makedouble (aref data (+ (* cols i) k))))
827 (when (/= i k)
828 (setf aik (/ aik pivot))
829 (setf (aref data (+ (* cols i) k)) aik)))
831 (dotimes (j cols)
832 (declare (fixnum j))
833 (setf akj (makedouble (aref data (+ (* cols k) j))))
834 (when (/= j k)
835 (setf akj (- (/ akj pivot)))
836 (setf (aref data (+ (* cols k) j)) akj)))
838 (setf akk (/ 1.0 pivot))
839 (setf (aref data (+ (* cols k) k)) akk)
841 (t 0))))
843 ;;; FIXME: use matlisp
844 (defun make-sweep-matrix (x y &optional w)
845 "Args: (x y &optional weights)
846 X is a matrix, Y and WEIGHTS are sequences. Returns the sweep matrix for the
847 (possibly weighted) regression of Y on X."
848 (check-matrix x)
849 (check-sequence y)
850 (if w (check-sequence w))
851 (let ((n (num-rows x))
852 (p (num-cols x)))
853 (if (/= n (length y)) (error "dimensions do not match"))
854 (if (and w (/= n (length w))) (error "dimensions do not match"))
855 (let ((mode (max (la-data-mode x)
856 (la-data-mode x)
857 (if w (la-data-mode w) 0)))
858 (result (make-array (list (+ p 2) (+ p 2))))
859 (x-mean (make-array p))
860 (y (coerce y 'vector))
861 (w (if w (coerce w 'vector)))
862 (has-w (if w 1 0)))
863 (make-sweep-front x y w n p mode has-w x-mean result)
864 result)))
866 (defun sweep-in-place (a k tol)
867 (check-matrix a)
868 (check-one-fixnum k)
869 (check-one-real tol)
870 (let ((rows (num-rows a))
871 (cols (num-cols a))
872 (mode (la-data-mode a)))
873 (let ((swept (sweep-in-place-front a rows cols mode k tol)))
874 (if (/= 0 swept) t nil))))
876 (defun sweep-operator (a columns &optional tolerances)
877 "Args: (a indices &optional tolerances)
878 A is a matrix, INDICES a sequence of the column indices to be swept. Returns
879 a list of the swept result and the list of the columns actually swept. (See
880 MULTREG documentation.) If supplied, TOLERANCES should be a list of real
881 numbers the same length as INDICES. An index will only be swept if its pivot
882 element is larger than the corresponding element of TOLERANCES."
883 (check-matrix a)
884 (check-sequence columns)
885 (if tolerances (check-sequence tolerances))
886 (check-real a)
887 (check-fixnum columns)
888 (if tolerances (check-real tolerances))
889 (do ((tol .0000001)
890 (result (copy-array a))
891 (swept-columns nil)
892 (columns (coerce columns 'list) (cdr columns))
893 (tolerances (if (consp tolerances) (coerce tolerances 'list))
894 (if (consp tolerances) (cdr tolerances))))
895 ((null columns) (list result swept-columns))
896 (let ((col (first columns))
897 (tol (if (consp tolerances) (first tolerances) tol)))
898 (if (sweep-in-place result col tol)
899 (setf swept-columns (cons col swept-columns))))))
903 ;;; AX+Y
906 ;;; matlisp:axpy
908 (defun ax+y (a x y &optional lower)
909 "Args (a x y &optional lower)
910 Returns (+ (matmult A X) Y). If LOWER is not nil, A is taken to be lower
911 triangular.
912 This could probably be made more efficient."
913 (check-square-matrix a)
914 (check-sequence x)
915 (check-sequence y)
916 (check-real a)
917 (check-real x)
918 (check-real y)
919 (let* ((n (num-rows a))
920 (result (make-list n))
921 (a (compound-data-seq a)))
922 (declare (fixnum n))
923 (if (or (/= n (length x)) (/= n (length y)))
924 (error "dimensions do not match"))
925 (do* ((tx (make-next-element x) (make-next-element x))
926 (ty (make-next-element y))
927 (tr (make-next-element result))
928 (i 0 (+ i 1))
929 (start 0 (+ start n))
930 (end (if lower (+ i 1) n) (if lower (+ i 1) n)))
931 ((<= n i) result)
932 (declare (fixnum i start end))
933 (let ((val (get-next-element ty i)))
934 (dotimes (j end)
935 (declare (fixnum j))
936 (setf val (+ val (* (get-next-element tx j)
937 (aref a (+ start j))))))
938 (set-next-element tr i val)))))
940 ;;;;
941 ;;;; Maximization and Numerical Derivatives
942 ;;;;
944 (defvar *maximize-callback-function* nil)
945 (defvar *maximize-callback-arg* nil)
947 (defun data2double (n data ptr)
948 (declare (fixnum n))
949 (let* ((seq (compound-data-seq data))
950 (elem (make-next-element seq)))
951 (if (/= (length seq) n) (error "bad data size"))
952 (dotimes (i n)
953 (declare (fixnum i))
954 (la-put-double ptr i (get-next-element elem i)))))
956 (defun maximize-callback (n px pfval pgrad phess pderivs)
957 (la-vector-to-data px n mode-re *maximize-callback-arg*)
958 (let* ((val (funcall *maximize-callback-function* *maximize-callback-arg*))
959 (derivs (if (consp val) (- (length val) 1) 0)))
960 (la-put-integer pderivs 0 derivs)
961 (la-put-double pfval 0 (if (consp val) (first val) val))
962 (if (<= 1 derivs) (data2double n (second val) pgrad))
963 (if (<= 2 derivs) (data2double (* n n) (third val) phess))))
965 (defun numgrad (f x &optional scale (h -1.0))
966 "Args: (f x &optional scale derivstep)
967 Computes the numerical gradient of F at X."
968 (check-sequence x)
969 (check-real x)
970 (when scale
971 (check-sequence scale)
972 (check-real scale))
973 (check-one-real h)
974 (let* ((n (length x))
975 (result (make-list n)))
976 (if (and scale (/= n (length scale)))
977 (error "scale not the same length as x"))
978 (let ((*maximize-callback-function* f)
979 (*maximize-callback-arg* (make-list n)))
980 (let ((px (la-data-to-vector x mode-re))
981 (pgrad (la-vector n mode-re))
982 (pscale (la-data-to-vector
983 (if scale scale (make-list n :initial-element 1.0))
984 mode-re)))
985 (unwind-protect
986 (progn
987 (numgrad-front n px pgrad h pscale)
988 (la-vector-to-data pgrad n mode-re result))
989 (la-free-vector px)
990 (la-free-vector pgrad)
991 (la-free-vector pscale))))
992 result))
994 (defun numhess (f x &optional scale (h -1.0) all)
995 "Args: (f x &optional scale derivstep)
996 Computes the numerical Hessian matrix of F at X."
997 (check-sequence x)
998 (check-real x)
999 (when scale
1000 (check-sequence scale)
1001 (check-real scale))
1002 (check-one-real h)
1003 (let* ((n (length x))
1004 (result (if all
1005 (list nil (make-list n) (make-array (list n n)))
1006 (make-array (list n n)))))
1007 (if (and scale (/= n (length scale)))
1008 (error "scale not the same length as x"))
1009 (let ((*maximize-callback-function* f)
1010 (*maximize-callback-arg* (make-list n)))
1011 (let ((hess-data (compound-data-seq (if all (third result) result)))
1012 (px (la-data-to-vector x mode-re))
1013 (pf (la-vector 1 mode-re))
1014 (pgrad (la-vector n mode-re))
1015 (phess (la-vector (* n n) mode-re))
1016 (pscale (la-data-to-vector
1017 (if scale scale (make-list n :initial-element 1.0))
1018 mode-re)))
1019 (unwind-protect
1020 (progn
1021 (numhess-front n px pf pgrad phess h pscale)
1022 (when all
1023 (setf (first result) (la-get-double pf 0))
1024 (la-vector-to-data pgrad n mode-re (second result)))
1025 (la-vector-to-data phess (* n n) mode-re hess-data))
1026 (la-free-vector pf)
1027 (la-free-vector px)
1028 (la-free-vector pgrad)
1029 (la-free-vector phess)
1030 (la-free-vector pscale))))
1031 result))
1033 (defun init-minfo-ipar-values (n ipars)
1034 (let* ((TRUE 1)
1035 (FALSE 0)
1036 (k 0)
1037 (m 0)
1038 (itnlimit -1)
1039 (backtrack TRUE)
1040 (verbose 0)
1041 (vals_suppl FALSE)
1042 (exptilt TRUE)
1043 (count 0)
1044 (termcode 0))
1045 (setf (aref ipars 0) n)
1046 (setf (aref ipars 1) m)
1047 (setf (aref ipars 2) k)
1048 (setf (aref ipars 3) itnlimit)
1049 (setf (aref ipars 4) backtrack)
1050 (setf (aref ipars 5) verbose)
1051 (setf (aref ipars 6) vals_suppl)
1052 (setf (aref ipars 7) exptilt)
1053 (setf (aref ipars 8) count)
1054 (setf (aref ipars 9) termcode)))
1056 (defun init-minfo-dpar-values (h dpars)
1057 (let ((typf 1.0)
1058 (gradtol -1.0)
1059 (steptol -1.0)
1060 (maxstep -1.0)
1061 (dflt 0.0)
1062 (tilt 0.0)
1063 (newtilt 0.0)
1064 (hessadd 0.0))
1065 (setf (aref dpars 0) typf)
1066 (setf (aref dpars 1) h)
1067 (setf (aref dpars 2) gradtol)
1068 (setf (aref dpars 3) steptol)
1069 (setf (aref dpars 4) maxstep)
1070 (setf (aref dpars 5) dflt)
1071 (setf (aref dpars 6) tilt)
1072 (setf (aref dpars 7) newtilt)
1073 (setf (aref dpars 8) hessadd)))
1075 (defun init-minfo-internals (n h internals)
1076 (let ((ipars (aref internals 8))
1077 (dpars (aref internals 9)))
1078 (init-minfo-ipar-values n ipars)
1079 (init-minfo-dpar-values h dpars)))
1081 (defun new-minfo-internals (f x &key scale ((:derivstep h) -1.0))
1082 (check-sequence x)
1083 (check-real x)
1084 (check-one-real h)
1085 (let ((n (length x)))
1086 (when scale
1087 (check-sequence scale)
1088 (check-real scale)
1089 (if (/= n (length scale)) (error "scale and x not the same length")))
1090 (let ((internals (make-array 12)))
1091 (setf (aref internals 0) f)
1092 (setf (aref internals 3) (if (consp x) (copy-list x) (coerce x 'list)))
1093 (setf (aref internals 4)
1094 (if scale (copy-seq scale) (make-array n :initial-element 1.0)))
1095 (setf (aref internals 5) (make-list (+ 1 n (* n n))))
1096 (setf (aref internals 8) (make-array 10))
1097 (setf (aref internals 9) (make-array 9))
1098 (init-minfo-internals n h internals)
1099 internals)))
1101 (defun minfo-maximize (internals &optional verbose)
1102 "This function does what?"
1103 (let* ((f (aref internals 0))
1104 (x (aref internals 3))
1105 (fvals (aref internals 5))
1106 (n (length x))
1107 (v (if verbose (if (integerp verbose) verbose 1) -1)))
1108 (setf (aref internals 3) (copy-list x))
1109 (setf (aref internals 5) (copy-list fvals))
1110 (let ((*maximize-callback-function* f)
1111 (*maximize-callback-arg* (make-list n)))
1112 (let* ((x (aref internals 3))
1113 (scale (aref internals 4))
1114 (fvals (aref internals 5))
1115 (ip (aref internals 8))
1116 (dp (aref internals 9))
1117 (px (la-data-to-vector x mode-re))
1118 (pscale (la-data-to-vector scale mode-re))
1119 (pfvals (la-vector (length fvals) mode-re))
1120 (pip (la-data-to-vector ip mode-in))
1121 (pdp (la-data-to-vector dp mode-re)))
1122 (unwind-protect
1123 (progn
1124 (base-minfo-maximize px pfvals pscale pip pdp v)) ;; access to C
1125 (la-vector-to-data px n mode-re x)
1126 (la-vector-to-data pfvals (+ 1 n (* n n)) mode-re fvals)
1127 (la-vector-to-data pip (length ip) mode-in ip)
1128 (la-vector-to-data pdp (length dp) mode-re dp))
1129 (get-buf)))))
1131 ;;;;
1132 ;;;; Miscellaneous Routines
1133 ;;;;
1136 (defun split-list (x n)
1137 "Args: (list cols)
1138 Returns a list of COLS lists of equal length of the elements of LIST.
1139 Example: (split-list '(1 2 3 4 5 6) 2) returns ((1 2 3) (4 5 6))"
1140 (check-one-fixnum n)
1141 (if (/= (rem (length x) n) 0) (error "length not divisible by ~a" n))
1142 (flet ((next-split ()
1143 (let ((result nil)
1144 (end nil))
1145 (dotimes (i n result)
1146 (declare (fixnum i))
1147 (let ((c-elem (list (first x))))
1148 (cond ((null result)
1149 (setf result c-elem)
1150 (setf end result))
1152 (setf (rest end) c-elem)
1153 (setf end (rest end)))))
1154 (setf x (rest x))))))
1155 (let ((result nil)
1156 (end nil)
1157 (k (/ (length x) n)))
1158 (declare (fixnum k))
1159 (dotimes (i k result)
1160 (declare (fixnum i))
1161 (let ((c-sub (list (next-split))))
1162 (cond ((null result)
1163 (setf result c-sub)
1164 (setf end result))
1166 (setf (rest end) c-sub)
1167 (setf end (rest end)))))))))