linalg.lsp

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