matrix.lisp

(defun matrixp (matrix)
  "Test whether the argument is a matrix"
  (declare (type (simple-array double-float) matrix))
  (and (arrayp matrix)
       (= (array-rank matrix) 2)))

(defun num-rows (matrix)
  "Return the number of rows of a matrix"
  (declare (type (simple-array double-float) matrix))
  (array-dimension matrix 0))

(defun num-cols (matrix)
  "Return the number of rows of a matrix"
  (declare (type (simple-array double-float) matrix))
  (array-dimension matrix 1))

(defun square-matrix? (matrix)
  "Is the matrix a square matrix?"
  (declare (type (simple-array double-float) matrix))
  (and (matrixp matrix)
       (= (num-rows matrix) (num-cols matrix))))
       
(defun make-matrix (rows &optional (cols rows))
  "Create a matrix filled with zeros.  If only one parameter is
  specified the matrix will be square."
  (declare (type fixnum rows cols))
  (make-array (list rows cols) :element-type 'double-float :initial-element 0d0))

(defun make-identity-matrix (size)
  "Make an identity matrix of the specified size."
  (declare (type fixnum size))
  (let ((matrix (make-matrix size)))
    (declare (type (simple-array double-float) matrix))
    (loop for i fixnum from 0 below size do
      (setf (aref matrix i i) 1d0))
    matrix))

(defun copy-matrix (matrix)
  "Return a copy of the matrix."
  (declare (type (simple-array double-float) matrix))
  (let* ((rows (num-rows matrix))
         (cols (num-cols matrix))
         (copy (make-matrix rows cols)))
    (dotimes (row rows copy)
      (dotimes (col cols)
        (setf (aref copy row col) (aref matrix row col))))))

(defun print-matrix (matrix &optional (destination t) (control-string " ~$"))
  "Print a matrix.  The optional control string indicates how each
  entry should be printed."
  (let ((rows (num-Rows matrix))
        (cols (num-Cols matrix)))
    (dotimes (row rows)
      (format destination "~%")
      (dotimes (col cols)
        (format destination control-string (aref matrix row col))))
    (format destination "~%")))

(defun transpose-matrix (matrix)
  "Transpose a matrix"
  (declare (type (simple-array double-float) matrix))
  (let* ((rows (num-rows matrix))
         (cols (num-cols matrix))
         (transpose (make-matrix cols rows)))
    (loop for row fixnum from 0 below rows do
      (loop for col fixnum from 0 below cols do
        (setf (aref transpose col row)
              (aref matrix row col))))))

(defun multiply-scalar-matrix (scalar matrix &optional result)
  (declare (optimize (speed 3) (safety 0))
           (type double-float scalar)
           (type (simple-array double-float) matrix))
  (let* ((rows (num-rows matrix))
	 (cols (num-cols matrix))
	 (result (if result
                     result
                     (make-matrix rows cols))))
    (declare (type fixnum rows cols)
             (type (simple-array double-float) result))
    (loop for row fixnum from 0 below rows do
      (loop for col fixnum from 0 below cols do
	(setf (aref result row col)
	      (* scalar (aref matrix row col)))))
    result))

(defun set-zero (mat)
  (declare (optimize (speed 3) (debug 0) (safety 0))
           (type (simple-array double-float) mat))
  (dotimes (i (num-rows mat) mat)
    (dotimes (j (num-cols mat))
      (setf (aref mat i j) 0d0))))

(defun multiply-matrix (m1 m2 &optional result)
  (declare (optimize (speed 3) (debug 0) (safety 0) (space 0)))
  (declare (type (simple-array double-float) m1 m2))
  (let ((rows1 (num-rows m1))
        (cols1 (num-cols m1))
        (cols2 (num-cols m2)))
    (declare (type fixnum rows1 cols1 cols2))
    (let ((result (if result
                      (set-zero result)
                      (make-matrix rows1 cols2))))
      (declare (type (simple-array double-float) result))
      (loop for row fixnum from 0 below rows1 do
        (loop for k fixnum from 0 below cols1 do
          (let ((cell (aref m1 row k))
                (m2-index (array-row-major-index m2 k 0))
                (result-index (array-row-major-index result row 0)))
            (declare (type double-float cell)
                     (type fixnum m2-index result-index))
            (loop for col fixnum from 0 below cols2 do
              (incf (row-major-aref result result-index)
                    (* cell (row-major-aref m2 m2-index)))
              (incf m2-index)
              (incf result-index)))))
      result)))

(defun add-matrix (m1 m2 &optional result)
  (declare (optimize (speed 3) (safety 0))
           (type (simple-array double-float) m1 m2))
  (let* ((rows (num-rows m1))
         (cols (num-cols m1))
         (result (if result
                     result
                     (make-matrix rows cols))))
    (declare (type fixnum rows cols)
             (type (simple-array double-float) result))
    (loop for row fixnum from 0 below rows do
      (loop for col fixnum from 0 below cols do
        (setf (aref result row col)
              (+ (aref m1 row col)
                 (aref m2 row col)))))
    result))

(defun add-matrices (matrices &optional result)
  "Add matrices"
  (when matrices
    (reduce (lambda (m1 m2)
              (add-matrix m1 m2 result)) matrices)))

(defun sub-matrix (m1 m2 &optional result)
  (declare (optimize (speed 3) (safety 0))
           (type (simple-array double-float) m1 m2))
  (let* ((rows (num-rows m1))
         (cols (num-cols m1))
         (result (if result
                     result
                     (make-matrix rows cols))))
    (declare (type fixnum rows cols)
             (type (simple-array double-float) result))
    (loop for row fixnum from 0 below rows do
      (loop for col fixnum from 0 below cols do
        (setf (aref result row col)
              (- (aref m1 row col)
                 (aref m2 row col)))))
    result))

(defun sub-matrices (matrices &optional result)
  "Add matrices"
  (when matrices
    (reduce (lambda (m1 m2)
              (sub-matrix m1 m2 result)) matrices)))

(defun invert-matrix (matrix &optional (destructive T))
  "Find the inverse of a matrix.  By default this operation is
  destructive.  If you want to preserve the original matrix, call this
  function with an argument of NIL to destructive."
  (declare (optimize (speed 3) (safety 0))
           (type (simple-array double-float) matrix))
  (let ((result (if destructive matrix (copy-matrix matrix)))
        (size (num-rows matrix))
        (temp 0d0))
    (declare (type (simple-array double-float) result)
             (type fixnum size)
             (type double-float temp))
    (loop for i fixnum from 0 below size do
      (setf temp (aref result i i))
      (loop for j fixnum from 0 below size do
        (setf (aref result i j)
              (if (= i j)
                  (/ (aref result i j))
                  (/ (aref result i j) temp))))
      (loop for j fixnum from 0 below size do
        (unless (= i j)
          (setf temp (aref result j i)
                (aref result j i) 0d0)
          (loop for k fixnum from 0 below size do
            (setf (aref result j k)
                  (- (aref result j k)
                     (* temp (aref result i k))))))))
    result))