gauss.lsp

(defpackage :cl-gauss-elimination
  (:use :cl))

(in-package :cl-gauss-elimination)

(defun solve-upper (U)
  (let* ((d (array-dimension U 0))
	 (x (make-array (list d) :initial-element 0)))
    (loop for i from (1- d) downto 0
       do (setf (aref x i)
		(/
		 (- (aref U i (1- (array-dimension U 1)))
		    (loop for j from (1+ i) upto (1- d)
		       sum (*
			    (aref U i j)
			    (aref x j))))
		 (aref U i i))))
    x))

(defun solve-lower (L)
  (let* ((d (array-dimension L 0))
	 (x (make-array (list d) :initial-element 0)))
    (loop for i from 0 upto (1- d)
       do (setf (aref x i)
		(/
		 (- (aref L i (1- (array-dimension L 1)))
		    (loop for j from 0 upto (1- i)
		       sum (* (aref L i j)
			      (aref x j))))
		 (aref L i i))))
    x))

(defun make-ident-array (d)
  (let ((M (make-array (list d d) :initial-element 0)))
    (loop for i from 0 upto (1- d)
       do (setf (aref M i i) 1))
    M))

(defun T-matrix (d n m l)
  (let ((matrix (make-ident-array d)))
    (setf (aref matrix n m) l)
    matrix))

(defun multiply-matrix (a b)
  (let* ((n (array-dimension a 0))
	 (m (array-dimension b 1))
	 (x (make-array (list n m)
			:initial-element 0)))
    (loop
       for i from 0 upto (1- n)
       do (loop
	     for j from 0 upto (1- m)
	     do (setf (aref x i j)
		      (loop for k from 0 to (1- n)
			 summing (* (aref a i k)
				    (aref b k j))))))
    x))

(defun matrix-swap-rows (matrix i j)
  "Swap I-th row of MATRIX with J-th row of MATRIX in-place."
  (let ((b nil))
    (loop for k from 0 upto (1- (array-dimension matrix 1)) do
	 (setf b (aref matrix i k))
	 (setf (aref matrix i k) (aref matrix j k))
	 (setf (aref matrix j k) b)))
  matrix)

(defun transform-matrix-to-upper (x)
  "Returns upper triangular matrix equivalent(in Gauss elimination
method) to matrix X."
  (let ((m (array-dimension x 0))
	(n (array-dimension x 1)))
    (loop for j from 0 upto (- n 2) do
	 (loop for i from (1+ j) upto (1- m) do
	      (progn
		(when (= (aref x j j) 0)
		  (matrix-swap-rows x j
				    (loop for k from (1+ j) upto (- n 2)
				       when (/= (aref x k j) 0)
				       return k))
		  )
		(setf x
		      (multiply-matrix
		       (T-matrix m i j (- (/
					   (aref x i j)
					   (aref x j j))))
		       x))))))
  x)

Poprawiłem solve-lower. Źle liczył.