LU decomposition in portable Common Lisp

lu-decomposition.lisp

;;; Matrix utilities.

(defun matrix= (matrix-1 matrix-2)
  (destructuring-bind (m1y m1x) (array-dimensions matrix-1)
    (destructuring-bind (m2y m2x) (array-dimensions matrix-2)
      (and (= m1y m2y)
           (= m1x m2x)
           (loop for y below m1y
                 always (loop for x below m1x
                              always (= (aref matrix-1 y x)
                                        (aref matrix-2 y x))))))))

(defun matrix-multiply (matrix-1 matrix-2)
  (destructuring-bind (m1y m1x) (array-dimensions matrix-1)
    (destructuring-bind (m2y m2x) (array-dimensions matrix-2)
      (assert (= m1x m2y) () "Invalid matrix dimensions.")
      (let ((result (make-empty-matrix m1y m2x)))
        (dotimes (y m1y)
          (dotimes (x m2x)
            (setf (aref result y x)
                  (loop for i below m1x
                        sum (* (aref matrix-1 y i)
                               (aref matrix-2 i x))))))
        result))))

(defun matrix-transpose (matrix)
  (destructuring-bind (my mx) (array-dimensions matrix)
    (let ((result (make-empty-matrix mx my)))
      (dotimes (x mx)
        (dotimes (y my)
          (setf (aref result x y) (aref matrix y x))))
      result)))

(defun make-empty-matrix (n &optional (m n))
  (make-array (list n m) :element-type 'real
                         :initial-element 0))

(defun make-identity-matrix (n)
  (let ((matrix (make-empty-matrix n)))
    (dotimes (i n) (setf (aref matrix i i) 1))
    matrix))

(defun rotate-rows (matrix row-1 row-2)
  (dotimes (i (second (array-dimensions matrix)))
    (rotatef (aref matrix row-1 i) (aref matrix row-2 i)))
  matrix)

;;; LU decomposition.

(defun make-pivoting-matrix (matrix)
  (destructuring-bind (mx my) (array-dimensions matrix)
    (assert (= mx my) () "Invalid matrix dimensions.")
    (loop with result = (make-identity-matrix mx)
          for x below mx
          for max = (abs (aref matrix x x))
          for result-row = x
          do (loop for y from x below mx
                   for new = (abs (aref matrix y x))
                   when (> new max)
                     do (setf max new
                              result-row y))
          unless (= x result-row)
            do (rotate-rows result x result-row)
          finally (return result))))

(defun lu-decompose (original-matrix)
  (destructuring-bind (mx my) (array-dimensions original-matrix)
    (assert (= mx my) () "Invalid matrix dimensions.")
    (let* ((pivot (make-pivoting-matrix original-matrix))
           (matrix (matrix-multiply pivot original-matrix))
           (lower (make-identity-matrix mx))
           (upper (make-empty-matrix mx)))
      (dotimes (j mx)
        ;; Work on upper matrix.
        (loop for i from 0 to j do
          (setf (aref upper i j)
                (- (aref matrix i j)
                   (loop for k from 0 to (- i 1)
                         sum (* (aref upper k j)
                                (aref lower i k))))))
        ;; Work on lower matrix.
        (loop for i from j to (- mx 1) do
          (setf (aref lower i j)
                (/ (- (aref matrix i j)
                      (loop for k from 0 to (- j 1)
                            sum (* (aref upper k j)
                                   (aref lower i k))))
                   (aref upper j j)))))
      (values pivot lower upper))))

;;; User interface.

(defun test-decomposition (matrix)
  (multiple-value-bind (pivot lower upper) (lu-decompose matrix)
    (let ((result (matrix-multiply (matrix-transpose pivot)
                                   (matrix-multiply lower upper))))
      (assert (matrix= matrix result) ()
              "Validation error.~%Expected:~%~S~%Got:~%~S" matrix result)
      (format t "MATRIX:~%~S~%LOWER:~%~S~%UPPER:~%~S~%PIVOT:~%~S~%"
              matrix lower upper pivot))))

;;; Example tests

(test-decomposition
 #2A((1 2 3 4 5)
     (0 2 0 3 1)
     (0 0 1 4 -1)
     (-4 0 0 1 -3)
     (0 0 2 -4 1)))

#|
MATRIX:
#2A((1 2 3 4 5) (0 2 0 3 1) (0 0 1 4 -1) (-4 0 0 1 -3) (0 0 2 -4 1))
LOWER:
#2A((1 0 0 0 0) (0 1 0 0 0) (0 0 1 0 0) (0 0 1/2 1 0) (-1/4 1 3/2 29/24 1))
UPPER:
#2A((-4 0 0 1 -3) (0 2 0 3 1) (0 0 2 -4 1) (0 0 0 6 -3/2) (0 0 0 0 57/16))
PIVOT:
#2A((0 0 0 1 0) (0 1 0 0 0) (0 0 0 0 1) (0 0 1 0 0) (1 0 0 0 0))
|#

(test-decomposition
 #2A((1 4 5)
     (-5 0 7)
     (0 -3 5)))

#|
MATRIX:
#2A((1 4 5) (-5 0 7) (0 -3 5))
LOWER:
#2A((1 0 0) (0 1 0) (-1/5 -4/3 1))
UPPER:
#2A((-5 0 7) (0 -3 5) (0 0 196/15))
PIVOT:
#2A((0 1 0) (0 0 1) (1 0 0))
|#