逆行列再び
ニューメリカルレシピ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)