Paste number 53085: matrix.lisp

Paste number 53085: matrix.lisp
Pasted by: NightShadow
9 months, 2 weeks ago
#lispcafe | Context in IRC logs
Paste contents:
Raw Source | XML | Display As
(in-package :com.teisenberger.math.matrix)

; Return a n-1 x n-1 matrix equal to the original matrix without the rowth row and columnth column
(defun matrix-minor (row column matrix)
  (let ((result (copy-tree matrix)))
    (if (eql row 1)
      (pop result)
      (setf (cdr (nthcdr (- row 2) result)) (nthcdr row result)))
    (dotimes (current-row-index (length result))
      (let ((current-row (nth current-row-index result)))
        (if (eql column 1)
          (pop (nth current-row-index result))
          (setf (cdr (nthcdr (- column 2) current-row)) (nthcdr column current-row)))))
    result))

; Return the row count of matrix
(defun matrix-rows (matrix)
  (length matrix))

; Return the column count of matrix
(defun matrix-columns (matrix)
  (length (first matrix)))

; Get the nth row
(defun matrix-row (n matrix)
(if (or (> n (matrix-rows matrix)) (< n 1)) (error "Invalid row."))
        (list (copy-list (nth (1- n) matrix))))

; Get the nth column
(defun matrix-column (n matrix)
        (if (or (> n (matrix-columns matrix)) (< n 1)) (error "Invalid column."))
        (mapcar #'(lambda (r) (list (nth (1- n) r))) matrix))

; Get the element at i,j
(defmacro matrix-element (i j matrix)
        `(nth (1- ,j) (nth (1- ,i) ,matrix)))

; Find the rank of the matrix
(defun matrix-rank (matrix)
        (let ((ref (matrix-ref matrix)) (rank 0) (zero-row (first (matrix-zero 1 (matrix-columns matrix)))))
                (dolist (row ref)
                        (if (equalp row zero-row) (return-from matrix-rank rank) (incf rank)))
                rank))

; Merge two matricies of equal numbers of rows
(defun matrix-augment (matrix-1 matrix-2)
        (if (not (or (eql (matrix-rows matrix-1) (matrix-rows matrix-2)) (null matrix-1) (null matrix-2))) (error "Invalid dimensions."))
        (cond
                ((null matrix-1) (copy-tree matrix-2))
                ((null matrix-2) (copy-tree matrix-1))
                (t (let ((result (copy-tree matrix-1)))
                                 (dotimes (i (matrix-rows matrix-1))
                                         (setf (nth i result) (nconc (nth i result) (nth i matrix-2))))
                                 result))))

; Split a matrix after column; Returns the two resulting matrices
(defun matrix-split (column matrix)
        (if (or (< column 1) (> column (matrix-columns matrix))) (error "Invalid split point"))
        (let ((matrix-1) (matrix-2) (split-point (- (matrix-columns matrix) column)))
                (do ((i (matrix-rows matrix) (1- i)))
                        ((< i 1))
                        (let ((current-row (matrix-row i matrix)))
                                (push (butlast (first current-row) split-point) matrix-1)
                                (push (last (first current-row) split-point) matrix-2)))
                (values matrix-1 matrix-2)))

; Call func on all elements
(defun matrix-apply (func matrix)
        (let ((result (copy-tree matrix)))
                (dotimes (i (matrix-rows result))
                        (dotimes (j (matrix-columns result))
                                (let ((i (1+ i)) (j (1+ j)))
                                        (setf (matrix-element i j result) (funcall func (matrix-element i j result))))))
                result))

; Define a matrix as a list of lists (rows) which have equal lengths (columns)
(defun matrixp (matrix)
  (if (atom matrix) (return-from matrixp nil))
        ; The reduce statement returns the common number of columns or -1 if not a matrix
        (not (eql (reduce #'(lambda (row-1-length row-2-length)
                                                        (if (eql row-1-length row-2-length) row-1-length -1)) matrix :key #'length) -1)))

; Square matrix predicate
(defun matrix-squarep (matrix)
        (eql (matrix-columns matrix) (matrix-rows matrix)))

; Upper triangular matrix predicate
(defun matrix-upper-triangularp (matrix &key (tolerance 1E-5))
        (dotimes (i (matrix-rows matrix))
                (let ((i (1+ i)))
                        (dotimes (j (1- i))
                                (let ((j (1+ j)))
                                        (if (> (abs (matrix-element i j matrix)) tolerance) (return-from matrix-upper-triangularp nil))))))
        t)

; Symmetric matrix predicate
(defun matrix-symmetricp (matrix)
        (if (not (matrix-squarep matrix)) (return-from matrix-symmetricp nil))
        (do ((i 1 (1+ i)))
                ((>= i (matrix-rows matrix)))
                (do ((j (1+ i) (1+ j)))
                        ((> j (matrix-columns matrix)))
                        (if (not (equalp (matrix-element i j matrix) (matrix-element j i matrix))) (return-from matrix-symmetricp nil))))
        t)

; Orthogonal matrix predicate
(defun matrix-orthogonalp (matrix)
        (dotimes (i (matrix-columns matrix))
                (do* ((i (1+ i)) (j (1+ i) (1+ j)) (column-i (matrix-transpose (matrix-column i matrix))))
                        ((> j (matrix-columns matrix)))
                        ; If dot product is not zero between two different columns
                        (if (/= (matrix-element 1 1 (matrix-multiply column-i (matrix-column j matrix))) 0) (return-from matrix-orthogonalp nil))))
        t)

; Invertible predicate
; This will return nil if the matrix is symbolic
(defun matrix-invertiblep (matrix)
        ; If the determinant is not zero, it is invertible
        (if (matrix-squarep matrix) (equalp (simplify (matrix-determinant matrix)) 0) nil))

; Make a m x n matrix consisting of all zeros
(defun matrix-zero (m n)
        (let ((result nil))
                (dotimes (i m)
                        (push (make-list n :initial-element 0) result))
                result))

; Make a m x n matrix consisting of all ones
(defun matrix-ones (m n)
        (let ((result nil))
                (dotimes (i m)
                        (push (make-list n :initial-element 1) result))
                result))

; Make a n x n identity matrix
(defun matrix-identity (n)
        (let ((result (matrix-zero n n)))
                (dotimes (i n)
                        (let ((i (1+ i)))
                                (setf (matrix-element i i result) 1)))
                result))

; Generate a random m x n  matrix with numbers in [0,maximum)
(defun matrix-random (m n &optional (maximum 1e0))
        (let ((matrix nil))
                (dotimes (i m)
                        (push nil matrix)
                        (dotimes (j n)
                                (push (random maximum) (nth 0 matrix))))
                matrix))

; Add corresponding elements in matricies of equal dimension
(defun matrix-add (matrix-1 matrix-2)
        (if (not (and (eql (matrix-rows matrix-1) (matrix-rows matrix-2)) (eql (matrix-columns matrix-1) (matrix-columns matrix-2)))) (error "Invalid dimensions."))
        (let ((result (copy-tree matrix-1)))
                (dotimes (row (matrix-rows result))
                        (dotimes (column (matrix-columns result))
                                (let ((row (1+ row)) (column (1+ column)))
                                        (setf (matrix-element row column result) `(+ ,(matrix-element row column result) ,(matrix-element row column matrix-2))))))
                result))

(defun matrix-multiply (matrix-1 matrix-2)
        (cond
                ; Scalar multiplicaton, scalar first
                ((or (atom matrix-1) (atom (first matrix-1)))
                 (if (and (numberp matrix-1) (numberp matrix-2))
                         (* matrix-1 matrix-2) ; Both scalars
                         (matrix-apply #'(lambda (x) `(* ,matrix-1 ,x)) matrix-2)))
                ; Scalar multiplication, scalar second
                ((or (atom matrix-2) (atom (first matrix-2))) (matrix-multiply matrix-2 matrix-1))
                ((not (eql (matrix-columns matrix-1) (matrix-rows matrix-2))) (error "Invalid dimensions."))
                (t
                        (let ((result (matrix-zero (matrix-rows matrix-1) (matrix-columns matrix-2))))
                                ; Perform multiplication, row interpretation used
                                (dotimes (i (matrix-rows matrix-1))
                                        (dotimes (j (matrix-columns matrix-1))
                                                (let ((i (1+ i)) (j (1+ j)))
                                                        (setf (nth (1- i) result) (first (matrix-add (list (nth (1- i) result)) (matrix-multiply (matrix-element i j matrix-1) (list (nth (1- j) matrix-2)))))))))
                                result))))

; Normalizes a 1 x n or n x 1 matrix
(defun matrix-normalize (matrix)
        (if (and (/= (matrix-rows matrix) 1) (/= (matrix-columns matrix) 1)) (error "Invalid dimensions."))
        (let ((length-squared))
                (if (eql (matrix-columns matrix) 1)
                        (setf length-squared (matrix-element 1 1 (matrix-multiply (matrix-transpose matrix) matrix)))
                        (setf length-squared (matrix-element 1 1 (matrix-multiply matrix (matrix-transpose matrix)))))
                (if (equalp (simplify length-squared) 0)
                        matrix
                        (matrix-multiply matrix `(/ 1 (expt ,length-squared 1/2))))))

; Gram-Schmidt method
(defun matrix-qr (matrix)
        (if (not (matrix-squarep matrix)) (error "Invalid dimensions."))
        (let ((matrix-q nil) (matrix-r (matrix-zero (matrix-rows matrix) (matrix-rows matrix))))
                (dotimes (i (matrix-columns matrix))
                        ; Set next-q as the current column of matrix and index as i
                        (do ((next-q (matrix-column (1+ i) matrix)) (current-index i (1- current-index)))
                                ; When index is 0, no more terms to subtract, augment matrix-q
                                ((<= current-index 0)
                                 (setf matrix-q (matrix-augment matrix-q (matrix-normalize next-q)))
                                 ; This line the diagonal of r, the only terms not calculated in Gram-Schmidt
                                 (setf (matrix-element (1+ i) (1+ i) matrix-r) `(expt ,(matrix-element 1 1 (matrix-multiply (matrix-transpose next-q) next-q)) 1/2)))
                                ; Define current-column as the orthonormal vector at current-index
                                (let ((current-column (matrix-column current-index matrix-q)))
                                        ; The following set statement finds q_i-1^T * A_i, or r_i
                                        (setf (matrix-element current-index (1+ i) matrix-r) (simplify (matrix-element 1 1 (matrix-multiply (matrix-transpose current-column) (matrix-column (1+ i) matrix)))))
                                        ; The following set statement subtracts one term, so that upon the
                                        ; completion of the do loop, q_i is as follows:
                                        ; q_i = A_i - (q_i-1^T * A_i) / (q_i-1^T * q_i-1) * q_i-1)
                                        ;                                         - (q_i-2^T * A_i) / (q_i-2^T * q_i-2) * q_i-2)
                                        ;                                         ...
                                        ;                                         - (q_1^T * A_i) / (q_1^T * q_1) * q_1)
                                        (setf next-q (matrix-apply #'simplify (matrix-add next-q (matrix-multiply `(* -1 ,(matrix-element current-index (1+ i) matrix-r)) current-column)))))))
                (values matrix-q matrix-r)))

; LU Decomposition
; Returns multiple values (matrix-l matrix-u)
(defun matrix-lu (matrix)
        (if (not (matrix-squarep matrix)) (error "Invalid dimensions."))
        (let ((matrix-u nil) (matrix-l (matrix-identity (matrix-rows matrix))))
                (dotimes (i (matrix-rows matrix))
                        (let* ((i (1+ i)) (next-u (matrix-row i matrix)))
                                (do ((current-index 1 (1+ current-index)))
                                        ((> current-index (matrix-rows matrix-u)) (setf matrix-u (nconc matrix-u next-u)))
                                        ; Store the multiplier of row current-index to be subtracted from row i
                                        (setf (matrix-element i current-index matrix-l) `(/ ,(matrix-element 1 current-index next-u) ,(matrix-element current-index current-index matrix-u)))
                                        ; Reduce the row
                                        (setf next-u (matrix-add next-u (matrix-multiply `(* -1 ,(matrix-element i current-index matrix-l)) (matrix-row current-index matrix-u)))))))
                (values matrix-l matrix-u)))

; Return the transpose of matrix
(defun matrix-transpose (matrix)
        (let ((result (matrix-zero (matrix-columns matrix) (matrix-rows matrix))))
                (dotimes (i (matrix-rows result))
                        (dotimes (j (matrix-columns result))
                                (let ((i (1+ i)) (j (1+ j)))
                                        (setf (matrix-element i j result) (matrix-element j i matrix)))))
                result))

; Count the zeros in a matrix, helps optimize determinant
(defun matrix-count-zeros (matrix)
        (let ((result 0))
                (dotimes (i (matrix-rows matrix))
                        (dotimes (j (matrix-columns matrix))
                                ; When simplify is better, use here
                                (let ((i (1+ i)) (j (1+ j)))
                                        (if (eql (matrix-element i j matrix) 0) (incf result)))))
                result))

; Return the determinant of matrix.
(defun matrix-determinant (matrix)
        (if (not (matrix-squarep matrix)) (error "Invalid dimensions."))
  (cond
    ((eql (length matrix) 1) (matrix-element 1 1 matrix))
    (t (let ((result (list '+)) (row-index 1))
                                 ; If more than 4 columns, optimize by counting zeros on the rows
         (when (> (matrix-columns matrix) 4)
                                         (let ((most-zeros 0))
                                                 (dotimes (i (matrix-columns matrix))
                                                         (let ((zeros (matrix-count-zeros (matrix-row (1+ i) matrix))))
                                                                 (if (> zeros most-zeros) (setf most-zeros zeros row-index (1+ i)))))))
                                 (dotimes (column-index (matrix-columns matrix))
                                         (let ((column-index (1+ column-index)))
                                                 ; Don't bother finding determinants that will be multiplied by 0
                                                 (if (eql (matrix-element row-index column-index matrix) 0)
                                                         (push 0 result)
                                                         (push `(* ,(expt -1 (+ column-index row-index)) ,(matrix-element row-index column-index matrix) ,(matrix-determinant (matrix-minor row-index column-index matrix))) result))))
                                 (nreverse result)))))

; Swap rows row-1-index and row-2-index
(defun matrix-row-swap (row-1-index row-2-index matrix)
        ; Nth is used over matrix-row for efficiency
        (let* ((result (copy-tree matrix)) (tmp (nth (1- row-1-index) result)))
                (setf (nth (1- row-1-index) result) (nth (1- row-2-index) result))
                (setf (nth (1- row-2-index) result) tmp)
                result))

; Print the matrix prettily, the last two parameters are for use with the ~/ format directive
(defun print-matrix (outputstream matrix &optional colon at)
        ; Get rid of compile warnings of being unused
        (declare (ignore colon at))
        ; The inner format creates a format string with a column width specified by calculate-column-width
        (let ((width (calculate-column-width matrix)))
                (format outputstream "~& _~vt _~%" (* width (matrix-columns matrix)))
                (format outputstream "| ~vt  |~%" (* width (matrix-columns matrix)))
                (format outputstream (format nil "~~{|~~{~~~a@a~~} |~~%~~}" width) matrix)
                (format outputstream "|_~vt _|~%" (* width (matrix-columns matrix)))))

; Iterate though all elements and find longest line
(defun calculate-column-width (matrix)
        (let ((width 0))
                (dolist (row matrix)
                        (dolist (element row)
                                ; The nil in the format tells it to include quotes in the output
                                (let ((element-width (length (format nil "~a" element))))
                                        (if (> element-width width) (setf width element-width)))))
                (1+ width)))

; Round <= tolerance to 0, necessary for QR algorithm to prevent underflows
(defun correct-near-zero (matrix &key (tolerance 1E-5))
        (dotimes (i (matrix-rows matrix))
                (let ((i (1+ i)))
                        (dotimes (j (matrix-columns matrix))
                                (let ((j (1+ j)))
                                        (if (<= (abs (matrix-element i j matrix)) tolerance) (setf (matrix-element i j matrix) 0))))))
        matrix)

; Uses the QR algorithm (unoptimized) to find the eigenvalues
(defun matrix-eigenvalues (matrix &key (max-iterations 40) (tolerance 1E-5))
        (setf matrix (matrix-apply #'eval matrix))
        (do ((iterations 0 (1+ iterations)))
                ((matrix-upper-triangularp matrix :tolerance tolerance) (format t "Iterations: ~a~%" iterations))
                (when (> iterations max-iterations)
                        (print-matrix t matrix)
                        (error "Too many iteratons"))
                (setf matrix (multiple-value-bind (q r) (matrix-qr matrix)
                                                                         (matrix-multiply r q)))
                (setf matrix (correct-near-zero (matrix-apply #'eval matrix) :tolerance tolerance)))
        ; Load the eigenvalues into a list
        (let ((eigenvalues nil))
                (dotimes (index (matrix-rows matrix))
                        (let ((index (1+ index)))
                            &nbs