60 lines
2.6 KiB
Common Lisp
60 lines
2.6 KiB
Common Lisp
(defun multiply-matrices (matrix-0 matrix-1)
|
|
"Takes two 2D arrays and returns their product, or an error if they cannot be multiplied"
|
|
(let* ((m0-dims (array-dimensions matrix-0))
|
|
(m1-dims (array-dimensions matrix-1))
|
|
(m0-dim (length m0-dims))
|
|
(m1-dim (length m1-dims)))
|
|
(if (or (/= 2 m0-dim) (/= 2 m1-dim))
|
|
(error "Array given not a matrix")
|
|
(let ((m0-rows (car m0-dims))
|
|
(m0-cols (cadr m0-dims))
|
|
(m1-rows (car m1-dims))
|
|
(m1-cols (cadr m1-dims)))
|
|
(if (/= m0-cols m1-rows)
|
|
(error "Incompatible dimensions")
|
|
(do ((rarr (make-array (list m0-rows m1-cols)
|
|
:initial-element 0) rarr)
|
|
(n 0 (if (= n (1- m0-cols)) 0 (1+ n)))
|
|
(cc 0 (if (= n (1- m0-cols))
|
|
(if (/= cc (1- m1-cols))
|
|
(1+ cc) 0) cc))
|
|
(cr 0 (if (and (= (1- m0-cols) n)
|
|
(= (1- m1-cols) cc))
|
|
(1+ cr)
|
|
cr)))
|
|
((= cr m0-rows) rarr)
|
|
(setf (aref rarr cr cc)
|
|
(+ (aref rarr cr cc)
|
|
(* (aref matrix-0 cr n)
|
|
(aref matrix-1 n cc))))))))))
|
|
|
|
(defun matrix-identity (dim)
|
|
"Creates a new identity matrix of size dim*dim"
|
|
(do ((rarr (make-array (list dim dim)
|
|
:initial-element 0) rarr)
|
|
(n 0 (1+ n)))
|
|
((= n dim) rarr)
|
|
(setf (aref rarr n n) 1)))
|
|
|
|
(defun matrix-expt (matrix exp)
|
|
"Takes the first argument (a matrix) and multiplies it by itself exp times"
|
|
(let* ((m-dims (array-dimensions matrix))
|
|
(m-rows (car m-dims))
|
|
(m-cols (cadr m-dims)))
|
|
(cond
|
|
((/= m-rows m-cols) (error "Non-square matrix"))
|
|
((zerop exp) (matrix-identity m-rows))
|
|
((= 1 exp) (do ((rarr (make-array (list m-rows m-cols)) rarr)
|
|
(cc 0 (if (= cc (1- m-cols))
|
|
0
|
|
(1+ cc)))
|
|
(cr 0 (if (= cc (1- m-cols))
|
|
(1+ cr)
|
|
cr)))
|
|
((= cr m-rows) rarr)
|
|
(setf (aref rarr cr cc) (aref matrix cr cc))))
|
|
((zerop (mod exp 2)) (let ((me2 (matrix-expt matrix (/ exp 2))))
|
|
(multiply-matrices me2 me2)))
|
|
(t (let ((me2 (matrix-expt matrix (/ (1- exp) 2))))
|
|
(multiply-matrices matrix (multiply-matrices me2 me2)))))))
|