share/nelder_mead/la.lisp

   1 (in-package :neldermead)
   2
   3 ;; Simple matrix-vector facility. Optimized for staying out of the way.
   4 (defmacro with-matrix-dimensions (varforms &body body)
   5   (let ((form `(progn ,@body)))
   6     (dolist (vform varforms)
   7       (setf form
   8             `(destructuring-bind ,(car vform)
   9                  (array-dimensions ,(cadr vform))
  10                (declare (fixnum ,@(car vform))
  11                         (ignorable ,@(car vform)))
  12                ,form)))
  13     form))
  14
  15 (defun ip (v w &optional (start 0))
  16   (let ((l (length v))
  17         (ax 0.0d0))
  18     (dotimes (i (- l start))
  19       (incf ax (* (aref v (+ i start))
  20                   (aref w (+ i start)))))
  21     ax))
  22
  23 (defun norm (v &optional (start 0))
  24     (sqrt (ip v v start)))
  25
  26 (defun make-matrix (m n)
  27   (make-array (list m n)
  28               :element-type 'double-float
  29               :initial-element 0.0d0))
  30
  31 (defun make-vector (n &key (initial-element 0.0d0))
  32   (make-array n
  33               :element-type 'double-float
  34               :initial-element initial-element))
  35
  36 (defun v*c (vector constant)
  37   (let* ((n (length vector))
  38          (res (make-vector n)))
  39     (dotimes (i n)
  40       (setf (aref res i)
  41             (* constant (aref vector i))))
  42     res))
  43
  44 (defun v+w*c (v w c)
  45   (assert (= (length v) (length w)))
  46   (let* ((n (length v))
  47          (res (make-vector n)))
  48     (dotimes (i n)
  49       (setf (aref res i)
  50             (+ (aref v i)
  51                (* c (aref w i)))))
  52     res))
  53
  54 (defun qr-factorization (mat &key (with-q t))
  55   (declare (type (simple-array double-float (* *)) mat)
  56            (optimize (speed 3) (safety 0)))
  57   (with-matrix-dimensions (((m n) mat))
  58     (let ((q (when with-q
  59                (let ((pq (make-matrix m n)))
  60                  (declare (type (simple-array double-float (* *)) pq))
  61                  (dotimes (i (min m n))
  62                    (setf (aref pq i i) 1.0d0))
  63                  pq))))
  64
  65       (loop for i from 0 below (- m 1) do
  66             (loop for j from (+ i 1) below m do
  67                   (let ((a (aref mat i i))
  68                          (b (aref mat j i)))
  69                     (when (/= b 0.0d0)
  70                       (let* ((r (sqrt (+ (* a a) (* b b))))
  71                             (c (/ a r))
  72                             (s (/ b r)))
  73
  74                     (loop for k from i below m do
  75                           (let ((olda (aref mat i k))
  76                                 (oldb (aref mat j k)))
  77
  78                             (setf (aref mat i k)
  79                                   (+ (* c olda) (* s oldb))
  80                                   (aref mat j k)
  81                                   (+ (* (- s) olda) (* c oldb)))))
  82
  83                     (when with-q
  84                       (let ((q q))
  85                         (declare (type (simple-array double-float (* *)) q))
  86                         (loop for k from 0 below m do
  87                               (let ((olda (aref q k i))
  88                                     (oldb (aref q k j)))
  89
  90                                 (setf (aref q k i)
  91                                       (+ (* c olda) (* s oldb))
  92                                       (aref q k j)
  93                                       (+ (* (- s) olda) (* c oldb))))))))))))
  94
  95       (values mat q))))