;;;   struct matrices
;;;
;;; A struct-based matrix and vector library with support of
;;; 2- and 3-dimensional vectors and 2x2 and 3x3 matrices.
;;;
;;; Author: Max-Gerd Retzlaff <m.retzlaff@gmx.net>
;;; Written on May 23, 2007.
;;;
;;; Supported opperations:
;;;  - (destructive and non-destructive) transposition
;;;  - Euclidean norm
;;;  - vector normalization (using the Euclidean norm)
;;;  - dot product for vectors
;;;  - vector/matrix multiplication
;;;  - scalar multiplication, addition, and subtraction (only double-floats allowed)
;;;  - scalar division for two and three dimensinoal vectors
;;;  - element-wise vector and matrix addition and subtraction
;;;  - Matlisp-like MATRIX-REF / MREF access methods
;;;  - Matlisp-like MATRIX function

(defpackage #:struct-matrices
  (:use #:cl)
  (:export #:vector2 #:vector3
           #:x #:y #:z
           #:matrix2x2 #:matrix3x3
           #:m00 #:m01 #:m02 #:m10 #:m11 #:m12 #:m20 #:m21 #:m22
           #:transpose! #:transpose
           #:norm
           #:normalize
           #:m* #:m/ #:m+ #:m-
           #:cross-product
           #:mref #:matrix-ref
           #:matrix #:make-real-matrix))
           
(in-package :struct-matrices)

;;(declaim (optimize (speed 3) (safety 1) (debug 0)))

;;; printer functions

(defun vector2-printer (object stream)
  (print-unreadable-object (object stream :type t)
    (format stream " (~s  ~s)~:[T~;~] "
            (x object)
            (y object)
            (transposedp object))))

(defun vector3-printer (object stream)
  (print-unreadable-object (object stream :type t)
    (format stream " (~s  ~s  ~s)~:[T~;~] "
            (x object)
            (y object)
            (z object)
            (transposedp object))))

(defun matrix2x2-printer (object stream)
  (print-unreadable-object (object stream :type t)
    (format stream "~&   (~s  ~s)~&   (~s  ~s) "
            (m00 object) (m01 object)
            (m10 object) (m11 object))))

(defun matrix3x3-printer (object stream)
  (print-unreadable-object (object stream :type t)
    (format stream "~&   (~s  ~s  ~s)~&   (~s  ~s  ~s)~&   (~s  ~s  ~s) "
            (m00 object) (m01 object) (m02 object)
            (m10 object) (m11 object) (m12 object)
            (m20 object) (m21 object) (m22 object))))


;;; types

(declaim (inline vector2 x y))
(defstruct (vector2 (:conc-name)
                    (:constructor vector2 (x y &optional transposedp))
                    (:print-object vector2-printer))
  (transposedp nil :type boolean)
  (x 0.0 :type double-float)
  (y 0.0 :type double-float))

(declaim (inline vector3 z))
(defstruct (vector3 (:include vector2)
                    (:conc-name)
                    (:constructor vector3 (x y z &optional transposedp))
                    (:print-object vector3-printer))
  (z 0.0 :type double-float))

(declaim (inline matrix2x2 m00 m01 m10 m11))
(defstruct (matrix2x2 (:conc-name)
                      (:constructor matrix2x2 (m00 m01
                                               m10 m11))
                      (:print-object matrix2x2-printer))
  (m00 0.0 :type double-float)
  (m01 0.0 :type double-float)
  (m10 0.0 :type double-float)
  (m11 0.0 :type double-float))

(declaim (inline matrix3x3 m02 m12 m20 m21 m22))
(defstruct (matrix3x3 (:include matrix2x2)
                      (:conc-name)
                      (:constructor matrix3x3 (m00 m01 m02
                                               m10 m11 m12
                                               m20 m21 m22))
                      (:print-object matrix3x3-printer))                      
  (m02 0.0 :type double-float)
  (m12 0.0 :type double-float)
  (m20 0.0 :type double-float)
  (m21 0.0 :type double-float)
  (m22 0.0 :type double-float))


;;; transposition

(defun transpose-vector! (vector)
  (declare (type (or vector2 vector3) vector))
  (setf (transposedp vector)
        (not (transposedp vector)))
  vector)


(defgeneric transpose! (vector))

(defmethod transpose! ((vector vector2))
  (transpose-vector! vector))

(defmethod transpose! ((vector vector3))
  (transpose-vector! vector))

(defmethod transpose! ((matrix matrix2x2))
  (let ((m01 (m01 matrix)))
    (setf (m01 matrix) (m10 matrix)
          (m10 matrix) m01))
  matrix)

(defmethod transpose! ((matrix matrix3x3))
  (let ((m01 (m01 matrix))
        (m02 (m02 matrix))
        (m12 (m12 matrix)))
    (setf (m01 matrix) (m10 matrix)
          (m02 matrix) (m20 matrix)
          (m12 matrix) (m21 matrix)
          (m10 matrix) m01
          (m20 matrix) m02
          (m21 matrix) m12))
  matrix)


(defgeneric transpose (vector/matrix))

(defmethod transpose ((vector vector2))
  (vector2 (x vector)
           (y vector)
           (not (transposedp vector))))

(defmethod transpose ((vector vector3))
  (vector3 (x vector)
           (y vector)
           (z vector)
           (not (transposedp vector))))

(defmethod transpose ((matrix matrix2x2))
  (matrix2x2 (m00 matrix) (m10 matrix)
             (m01 matrix) (m11 matrix)))

(defmethod transpose ((matrix matrix3x3))
  (matrix3x3 (m00 matrix) (m10 matrix) (m20 matrix)
             (m01 matrix) (m11 matrix) (m21 matrix)
             (m02 matrix) (m12 matrix) (m22 matrix)))


;;; vector dot-product and vector/matrix multiplication

(defgeneric m* (vector/matrix/double-float vector/matrix/double-float))

;; vector2, matrix2x2, and combinations

(defmethod m* ((a vector2) (b vector2))
  (cond
    ((and (transposedp a)
          (transposedp b))
     (error "Can't multiply 1x2 and 1x2."))
    ((transposedp a)
     (+ (* (x a) (x b))
        (* (y a) (y b))))
    ((transposedp b)
     (matrix2x2 (* (x a) (x b))
                (* (x a) (y b))
                (* (y a) (x b))
                (* (y a) (y b))))
    (t
     (error "Can't multiply 2x1 and 2x1."))))

(defmethod m* ((vector vector2) (matrix matrix2x2))
  (cond
    ((transposedp vector)
     (vector2 (+ (* (x vector) (m00 matrix))
                 (* (y vector) (m10 matrix)))
              (+ (* (x vector) (m01 matrix))
                 (* (y vector) (m11 matrix)))
              t))
    (t
     (error "Can't multiply 2x1 and 2x2."))))

(defmethod m* ((matrix matrix2x2) (vector vector2))
  (cond
    ((transposedp vector)
     (error "Can't multiply 2x2 and 1x2."))
    (t
     (vector2 (+ (* (m00 matrix) (x vector))
                 (* (m01 matrix) (y vector)))
              (+ (* (m10 matrix) (x vector))
                 (* (m11 matrix) (y vector)))))))

(defmethod m* ((a matrix2x2) (b matrix2x2))
  (matrix2x2 (+ (* (m00 a) (m00 b))
                (* (m01 a) (m10 b)))
             (+ (* (m00 a) (m01 b))
                (* (m01 a) (m11 b)))

             (+ (* (m10 a) (m00 b))
                (* (m11 a) (m10 b)))
             (+ (* (m10 a) (m01 b))
                (* (m11 a) (m11 b)))))

;; vector3, matrix3x3, and combinations

(defmethod m* ((a vector3) (b vector3))
  (cond
    ((and (transposedp a)
          (transposedp b))
     (error "Can't multiply 1x3 and 1x3."))
    ((transposedp a)
     (+ (* (x a) (x b))
        (* (y a) (y b))
        (* (z a) (z b))))
    ((transposedp b)
     (matrix3x3 (* (x a) (x b))
                (* (x a) (y b))
                (* (x a) (z b))
                
                (* (y a) (x b))
                (* (y a) (y b))
                (* (y a) (z b))
                
                (* (z a) (x b))
                (* (z a) (y b))
                (* (z a) (z b))))
    (t
     (error "Can't multiply 3x1 and 3x1."))))

(defmethod m* ((vector vector3) (matrix matrix3x3))
  (cond
    ((transposedp vector)
     (vector3 (+ (* (x vector) (m00 matrix))
                 (* (y vector) (m10 matrix))
                 (* (z vector) (m20 matrix)))
              (+ (* (x vector) (m01 matrix))
                 (* (y vector) (m11 matrix))
                 (* (z vector) (m21 matrix)))
              (+ (* (x vector) (m02 matrix))
                 (* (y vector) (m12 matrix))
                 (* (z vector) (m22 matrix)))
              t))
    (t
     (error "Can't multiply 3x1 and 3x3."))))

(defmethod m* ((matrix matrix3x3) (vector vector3))
  (cond
    ((transposedp vector)
     (error "Can't multiply 3x3 and 1x3."))
    (t
     (vector3 (+ (* (m00 matrix) (x vector))
                 (* (m01 matrix) (y vector))
                 (* (m02 matrix) (z vector)))
              (+ (* (m10 matrix) (x vector))
                 (* (m11 matrix) (y vector))
                 (* (m12 matrix) (z vector)))
              (+ (* (m20 matrix) (x vector))
                 (* (m21 matrix) (y vector))
                 (* (m22 matrix) (z vector)))))))

(defmethod m* ((a matrix3x3) (b matrix3x3))
  (matrix3x3 (+ (* (m00 a) (m00 b))
                (* (m01 a) (m10 b))
                (* (m02 a) (m20 b)))
             (+ (* (m00 a) (m01 b))
                (* (m01 a) (m11 b))
                (* (m02 a) (m21 b)))
             (+ (* (m00 a) (m02 b))
                (* (m01 a) (m12 b))
                (* (m02 a) (m22 b)))

             (+ (* (m10 a) (m00 b))
                (* (m11 a) (m10 b))
                (* (m12 a) (m20 b)))
             (+ (* (m10 a) (m01 b))
                (* (m11 a) (m11 b))
                (* (m12 a) (m21 b)))
             (+ (* (m10 a) (m02 b))
                (* (m11 a) (m12 b))
                (* (m12 a) (m22 b)))

             (+ (* (m20 a) (m00 b))
                (* (m21 a) (m10 b))
                (* (m22 a) (m20 b)))
             (+ (* (m20 a) (m01 b))
                (* (m21 a) (m11 b))
                (* (m22 a) (m21 b)))
             (+ (* (m20 a) (m02 b))
                (* (m21 a) (m12 b))
                (* (m22 a) (m22 b)))))


;;; scalar multiplication

(defmethod m* ((scalar double-float) (vector vector2))
  (vector2 (* scalar (x vector))
           (* scalar (y vector))
           (transposedp vector)))

(defmethod m* ((vector vector2) (scalar double-float))
  (m* scalar vector))

(defmethod m* ((scalar double-float) (vector vector3))
  (vector3 (* scalar (x vector))
           (* scalar (y vector))
           (* scalar (z vector))
           (transposedp vector)))

(defmethod m* ((vector vector3) (scalar double-float))
  (m* scalar vector))

(defmethod m* ((scalar double-float) (matrix matrix2x2))
  (matrix2x2 (* scalar (m00 matrix))
             (* scalar (m01 matrix))

             (* scalar (m10 matrix))
             (* scalar (m11 matrix))))

(defmethod m* ((matrix matrix2x2) (scalar double-float))
  (m* scalar matrix))             

(defmethod m* ((scalar double-float) (matrix matrix3x3))
  (matrix3x3 (* scalar (m00 matrix))
             (* scalar (m01 matrix))
             (* scalar (m02 matrix))

             (* scalar (m10 matrix))
             (* scalar (m11 matrix))
             (* scalar (m12 matrix))

             (* scalar (m20 matrix))
             (* scalar (m21 matrix))
             (* scalar (m22 matrix))))

(defmethod m* ((matrix matrix3x3) (scalar double-float))
  (m* scalar matrix))


;;; scalar division

(defgeneric m/ (vector/double-float vector/double-float))

(defmethod m/ ((scalar double-float) (vector vector2))
  (vector2 (/ scalar (x vector))
           (/ scalar (y vector))
           (transposedp vector)))

(defmethod m/ ((vector vector2) (scalar double-float))
  (vector2 (/ (x vector) scalar)
           (/ (y vector) scalar)
           (transposedp vector)))

(defmethod m/ ((scalar double-float) (vector vector3))
  (vector3 (/ scalar (x vector))
           (/ scalar (y vector))
           (/ scalar (z vector))
           (transposedp vector)))

(defmethod m/ ((vector vector3) (scalar double-float))
  (vector3 (/ (x vector) scalar)
           (/ (y vector) scalar)
           (/ (z vector) scalar)
           (transposedp vector)))


;;; scalar addition

(defgeneric m+ (vector/matrix/double-float vector/matrix/double-float))

(defmethod m+ ((scalar double-float) (vector vector2))
  (vector2 (+ scalar (x vector))
           (+ scalar (y vector))
           (transposedp vector)))

(defmethod m+ ((vector vector2) (scalar double-float))
  (m+ scalar vector))

(defmethod m+ ((scalar double-float) (vector vector3))
  (vector3 (+ scalar (x vector))
           (+ scalar (y vector))
           (+ scalar (z vector))
           (transposedp vector)))

(defmethod m+ ((vector vector3) (scalar double-float))
  (m+ scalar vector))

(defmethod m+ ((scalar double-float) (matrix matrix2x2))
  (matrix2x2 (+ scalar (m00 matrix))
             (+ scalar (m01 matrix))

             (+ scalar (m10 matrix))
             (+ scalar (m11 matrix))))

(defmethod m+ ((matrix matrix2x2) (scalar double-float))
  (m+ scalar matrix))             

(defmethod m+ ((scalar double-float) (matrix matrix3x3))
  (matrix3x3 (+ scalar (m00 matrix))
             (+ scalar (m01 matrix))
             (+ scalar (m02 matrix))

             (+ scalar (m10 matrix))
             (+ scalar (m11 matrix))
             (+ scalar (m12 matrix))

             (+ scalar (m20 matrix))
             (+ scalar (m21 matrix))
             (+ scalar (m22 matrix))))

(defmethod m+ ((matrix matrix3x3) (scalar double-float))
  (m+ scalar matrix))


;;; scalar subtraction (little bit different, and only (- vector/matrix double-float))

(defgeneric m- (vector/matrix/double-float vector/matrix/double-float))

(defmethod m- ((vector vector2) (scalar double-float))
  (vector2 (- (x vector) scalar)
           (- (y vector) scalar)
           (transposedp vector)))

(defmethod m- ((vector vector3) (scalar double-float))
  (vector3 (- (x vector) scalar)
           (- (y vector) scalar)
           (- (z vector) scalar)
           (transposedp vector)))

(defmethod m- ((matrix matrix2x2) (scalar double-float))
  (matrix2x2 (- (m00 matrix) scalar)
             (- (m01 matrix) scalar)

             (- (m10 matrix) scalar)
             (- (m11 matrix) scalar)))

(defmethod m- ((matrix matrix3x3) (scalar double-float))
  (matrix3x3 (- (m00 matrix) scalar)
             (- (m01 matrix) scalar)
             (- (m02 matrix) scalar)

             (- (m10 matrix) scalar)
             (- (m11 matrix) scalar)
             (- (m12 matrix) scalar)

             (- (m20 matrix) scalar)
             (- (m21 matrix) scalar)
             (- (m22 matrix) scalar)))


;;; vector and matrix addition

(defmethod m+ ((a vector2) (b vector2))
  (cond
    ((and (transposedp a)
          (not (transposedp b)))
     (error "Can't add 1x2 and 2x1."))
    ((and (not (transposedp a))
          (transposedp b))
     (error "Can't add 2x1 and 1x2."))
    (t
     (vector2 (+ (x a) (x b))
              (+ (y a) (y b))
              (transposedp a)))))

(defmethod m+ ((a vector3) (b vector3))
  (cond
    ((and (transposedp a)
          (not (transposedp b)))
     (error "Can't add 1x3 and 3x1."))
    ((and (not (transposedp a))
          (transposedp b))
     (error "Can't add 3x1 and 1x3."))
    (t
     (vector3 (+ (x a) (x b))
              (+ (y a) (y b))
              (+ (z a) (z b))
              (transposedp a)))))

(defmethod m+ ((a matrix2x2) (b matrix2x2))
  (matrix2x2 (+ (m00 a) (m00 b)) (+ (m01 a) (m01 b))
             (+ (m10 a) (m10 b)) (+ (m11 a) (m11 b))))

(defmethod m+ ((a matrix3x3) (b matrix3x3))
  (matrix3x3 (+ (m00 a) (m00 b)) (+ (m01 a) (m01 b)) (+ (m02 a) (m02 b))
             (+ (m10 a) (m10 b)) (+ (m11 a) (m11 b)) (+ (m12 a) (m12 b))
             (+ (m20 a) (m20 b)) (+ (m21 a) (m21 b)) (+ (m22 a) (m22 b))))


;;; vector and matrix subtraction

(defmethod m- ((a vector2) (b vector2))
  (cond
    ((and (transposedp a)
          (not (transposedp b)))
     (error "Can't add 1x2 and 2x1."))
    ((and (not (transposedp a))
          (transposedp b))
     (error "Can't add 2x1 and 1x2."))
    (t
     (vector2 (- (x a) (x b))
              (- (y a) (y b))
              (transposedp a)))))

(defmethod m- ((a vector3) (b vector3))
  (cond
    ((and (transposedp a)
          (not (transposedp b)))
     (error "Can't add 1x3 and 3x1."))
    ((and (not (transposedp a))
          (transposedp b))
     (error "Can't add 3x1 and 1x3."))
    (t
     (vector3 (- (x a) (x b))
              (- (y a) (y b))
              (- (z a) (z b))
              (transposedp a)))))

(defmethod m- ((a matrix2x2) (b matrix2x2))
  (matrix2x2 (- (m00 a) (m00 b)) (- (m01 a) (m01 b))
             (- (m10 a) (m10 b)) (- (m11 a) (m11 b))))

(defmethod m- ((a matrix3x3) (b matrix3x3))
  (matrix3x3 (- (m00 a) (m00 b)) (- (m01 a) (m01 b)) (- (m02 a) (m02 b))
             (- (m10 a) (m10 b)) (- (m11 a) (m11 b)) (- (m12 a) (m12 b))
             (- (m20 a) (m20 b)) (- (m21 a) (m21 b)) (- (m22 a) (m22 b))))


;; Euclidean norm

(declaim (inline norm))
(defgeneric norm (vector))
  
(defmethod norm ((vector vector2))
  (coerce (sqrt (the (double-float 0d0)
                  (+ (expt (x vector) 2)
                     (expt (y vector) 2))))
          'double-float))
  
(defmethod norm ((vector vector3))
  (coerce (sqrt (the (double-float 0d0)
                  (+ (expt (x vector) 2)
                     (expt (y vector) 2)
                     (expt (z vector) 2))))
          'double-float))


;;; vector normalization

(declaim (inline normalize))
(defgeneric normalize (vector))

(defmethod normalize ((vector vector2))
  (let ((norm (the double-float (norm vector))))
    (when (zerop norm)
      (error "Can't normalize a vector whose norm is zero."))
    (m/ vector norm)))

(defmethod normalize ((vector vector3))
  (let ((norm (the double-float (norm vector))))
    (when (zerop norm)
      (error "Can't normalize a vector whose norm is zero."))
    (m/ vector norm)))


;;; cross-product

(declaim (inline cross-product))
(defgeneric cross-product (vector vector))

(defmethod cross-product ((a vector3) (b vector3))
  (vector3 (- (* (y a) (z b)) (* (z a) (y b)))
           (- (* (z a) (x b)) (* (x a) (z b)))
           (- (* (x a) (y b)) (* (y a) (x b)))))


;;; Matlisp-like MATRIX-REF / MREF

(defgeneric mref (vector/matrix row &optional column))

(defmethod mref ((vector vector2) row &optional column)
  (if column
      (if (transposedp vector)
          (if (zerop row)
              (mref vector column)
              (error "Row ~d out of bounds for 1x2 vector."
                     row))
          (if (zerop column)
              (mref vector row)
              (error "Column ~d out of bounds for 1x2 vector."
                     column)))
      (case row
        (0 (x vector))
        (1 (y vector))
        (otherwise (error "Index ~d out of bounds for vector2."
                          row)))))

(defmethod mref ((vector vector3) row &optional column)
  (if column
      (if (transposedp vector)
          (if (zerop row)
              (mref vector column)
              (error "Row ~d out of bounds for 1x3 vector."
                     row))
          (if (zerop column)
              (mref vector row)
              (error "Column ~d out of bounds for 1x3 vector."
                     column)))
      (case row
        (0 (x vector))
        (1 (y vector))
        (2 (z vector))
        (otherwise (error "Index ~d out of bounds for vector3."
                          row)))))

(defmacro make-matrix-access-case (matrix #+(or) type lower-bound upper-bound)
  `(case row
     ,@(loop for row from lower-bound to upper-bound
             collect `(,row (case column
                              ,@(loop for column from lower-bound to upper-bound
                                      collect `(,column (,(intern (format nil "M~d~d" row column)) ,matrix))))
                            #+(or)
                            (otherwise (error "Column index ~d out of bounds for ~a."
                                              column ,type))))
     #+(or)
     (otherwise (error "Row index ~d out of bounds for ~a."
                       column ,type))))

(defmethod mref ((matrix matrix2x2) row &optional column)
  (unless column
    (error "Column necessary when accessing matrices."))
  (unless (typep row '(integer 0 1))
    (error "Row index ~d out of bounds for 2x2 matrix."
           row))
  (unless (typep column '(integer 0 1))
    (error "Column index ~d out of bounds for 2x2 matrix."
           column))
  (make-matrix-access-case matrix 0 1))

(defmethod mref ((matrix matrix3x3) row &optional column)
  (unless column
    (error "Column necessary when accessing matrices."))
  (unless (typep row '(integer 0 2))
    (error "Row index ~d out of bounds for 3x3 matrix."
           row))
  (unless (typep column '(integer 0 2))
    (error "Column index ~d out of bounds for 3x3 matrix."
           column))
  (make-matrix-access-case matrix 0 2))


(setf (symbol-function 'matrix-ref) #'mref)


;;; Matlisp-like MATRIX / MAKE-REAL-MATRIX

(defun matrix (list)
  (flet ((make-vector (list rows)
           (case rows
             (2 (apply #'vector2 list))
             (3 (apply #'vector3 list))
             (otherwise (error "Vectors of length ~d not supported."
                               rows)))))
  (unless (consp list)
    (error "argument is not a list, or an empty list."))
  (let ((rows (length list)))
    (if (consp (car list))
        ;; matrix or transposed vector
        (let ((cols (length (car list))))
          (case rows
            (1 (transpose! (make-vector (car list) cols)))
            (2 (if (= cols 2)
                   (apply #'matrix2x2 (apply #'append list))
                   (error "Matrixes of dimension ~dx~d not supported."
                          rows cols)))
            (3 (if (= cols 3)
                   (apply #'matrix3x3 (apply #'append list))
                   (error "Matrixes of dimension ~dx~d not supported."
                          rows cols)))))
        ;; untransposed vector
        (make-vector list rows)))))


(setf (symbol-function 'make-real-matrix) #'matrix)