逆行列再び

ニューメリカルレシピinCを参考に書いてみました。
ublasにピボット無しのLU分解のメソッドがあることに気付いたので、
また逆行列やってみるときはそこら辺かな。

※何度か修正してます。

(in-package #:cl)

(defpackage #:lu
  (:use :cl)
  (:export
   #:m-inv))

(in-package #:lu)

(defmethod matrix-at ((m array) i j)
  (aref m i j))

(defmethod (setf matrix-at) (value (m array) i j)
  (setf (aref m i j) value))

(defun matrix-rows (m)
  (array-dimension m 0))

(defun matrix-cols (m)
  (array-dimension m 1))

(defun copy-matrix (m)
  (let ((copy (make-array
	       (list (array-dimension m 0)
		     (array-dimension m 1)))))
    (loop for i from 0 below (array-dimension m 0) do
	  (loop for j from 0 below (array-dimension m 1) do
		(setf (aref copy i j)
		    (aref m i j))))
    copy))

(defun matrix-print (m)
  (loop for i from 0 below (matrix-rows m) do
	(progn
	 (loop for j from 0 below (matrix-cols m) do
	      (format t "~7,2f  " (matrix-at m i j)))
	 (terpri))))

(defmacro do-matrix ((m i j &optional elt) &body body)
  `(dotimes (,i (matrix-rows ,m) ,m)
     (dotimes (,j (matrix-cols ,m))
       ,@(if elt
	     `((symbol-macrolet ((,elt (matrix-at ,m ,i ,j))) ,@body))
	   body))))

(defun m* (a b)
  (assert (= (matrix-cols a) (matrix-rows b)))
  (let ((result (make-array (list (matrix-rows a)
				  (matrix-cols b)))))
    (do-matrix (result i j elt)
	       (dotimes (k (matrix-cols a))
		 (incf elt
	      (* (matrix-at a i k) (matrix-at b k j)))))
    result))

(defun lu-elimination-1 (a i j to)
  (let ((sum (matrix-at a i j)))
    (loop for k from 0 below to do
	  (decf sum
		(* (matrix-at a i k)
		   (matrix-at a k j))))
    sum))

(defmacro lu-elimination-3! (a n j)
  (let ((i (gensym)))
    `(loop for ,i from (+ ,j 1) below ,n do
	   (setf (matrix-at ,a ,i ,j)
		 (/ (matrix-at ,a ,i ,j) (matrix-at ,a ,j ,j))))))

(defmacro lu-pivot! (mat scp from to size)
  (let ((k (gensym)))
    `(progn
       (loop for ,k from 0 below ,size do
	     (rotatef (matrix-at ,mat ,to ,k) (matrix-at ,mat ,from ,k)))
       (setf (aref ,scp ,to) (aref ,scp ,from))))) ;処理が戻ることがないので、rotatefにする必要が無い。

(defmacro aif (test-form then-form &optional else-form)
  `(let ((it ,test-form))
    (if it ,then-form ,else-form)))

(defun lu-factorize! (mat mat-size)
  (assert (= (matrix-rows mat) (matrix-cols mat)))
  (let ((pivot-result-indices (make-array mat-size))
	(maximum-abs-values-of-rows (make-array mat-size))
	(mat-size-minus-one (1- mat-size))
	(d 1))
    (loop for i from 0 below mat-size do
	  (aif (loop for j from 0 below mat-size
		     maximize (abs (matrix-at mat i j)) into max
		     finally (return (if (> max 0) max nil)))
	       (setf (aref maximum-abs-values-of-rows i) it)
	       (assert nil
		       nil
		       "Singular matrix.")))
    (loop for j from 0 below mat-size do
	  (symbol-macrolet ((pivot-to (aref pivot-result-indices j))
			    (matrix-at-ij (matrix-at mat i j)))
	    (let ((big 0))
	     (loop for i from 0 below mat-size do
		  (if (< i j)
		      (setf matrix-at-ij (lu-elimination-1 mat i j i))
		      (setf matrix-at-ij (lu-elimination-1 mat i j j)))
		  (unless (< i j)
		    (let ((dum (/ (abs matrix-at-ij)
				  (aref maximum-abs-values-of-rows i))))
		      (when (> dum big)
			(setq big dum)
			(setf pivot-to i))))))
	    (when (/= j pivot-to)
	      (lu-pivot! mat maximum-abs-values-of-rows j pivot-to mat-size)
	      (setq d (- d)))
	    ;; ここまでの計算結果が0だった場合の処理について削ってある。
	    ;; 必要な場合足すこと。
	    (when (< j mat-size-minus-one)
	      (lu-elimination-3! mat mat-size j))))
    (values mat pivot-result-indices d)))

(defmacro lu-backsubstitute-1 (sum a i j b)
  `(decf ,sum (* (matrix-at ,a ,i ,j)
		 (aref ,b ,j))))

(defun lu-backsubstitute! (lu-mat mat-size pivot-indices colwise-vec)
  (assert (= (matrix-rows lu-mat) (matrix-cols lu-mat)))
  (let ((ii 0)
	(before-1? t))
    (loop for i from 0 below mat-size do
	  (symbol-macrolet
	      ((pivoted (aref colwise-vec (aref pivot-indices i)))
	       (current (aref colwise-vec i)))
	   (rotatef pivoted current)
	   (if before-1?
	       (when (/= current 0)
		 (setf ii i)
		 (setf before-1? nil))
	     (loop for j from ii to (1- i) do
		   (lu-backsubstitute-1 current lu-mat i j colwise-vec)))))
    (loop for i from (1- mat-size) downto 0 do
	  (symbol-macrolet
	      ((current (aref colwise-vec i)))
	    (loop for j from (1+ i) below mat-size do
		  (lu-backsubstitute-1 current lu-mat i j colwise-vec))
	    (setf current (/ current
			    (matrix-at lu-mat i i)))))
    colwise-vec))

(defun slice-of-identity-matrix (size col)
  (make-array size :initial-contents
	      (loop for i from 0 below size collect
	       (if (= i col) 1 0))))

(defun m-inv (source)
  (assert (= (matrix-rows source) (matrix-cols source)))
  (let* ((n (matrix-rows source))
	 (result (make-array (list n n))))
    (multiple-value-bind
	(lu-mat pivot-indices)
	(lu-factorize! (copy-matrix source) n)
      (loop for j from 0 below n do
	    (let ((col
		   (lu-backsubstitute! lu-mat n pivot-indices
				       (slice-of-identity-matrix n j))))
	      (loop for i from 0 below n do
		   (setf (matrix-at result i j) (aref col i))))))
    result))

(defun test-print()
  (let ((my-mat 
	 (make-array '(4 4) :initial-contents
		     '((1 2 4 1) (3 1 4 1)  (3 2 1 4)  (3 2 4 1)))))
    (matrix-print my-mat)
    (terpri)
    (time (matrix-print (m* my-mat (m-inv my-mat))))))

(defun test-print2()
  (let* ((size 4)
	 (my-mat
	  (make-array (list size size) :initial-contents
		      (loop for i from 0 below size collect
			    (loop for i from 0 below size collect
				  (random 100.0))))))
    (if t
	(progn
	  (matrix-print my-mat)
	  (terpri)
	  (let ((my-inv (time (m-inv my-mat))))
	    (matrix-print (m* my-mat my-inv))))
      (progn
	(time (m-inv my-mat))))))

;(test-print)
(test-print2)