;This code written in ANSI Common Lisp (Allegro CL 5.0) by Prof. Robert
;McGhee at the Naval Postgraduate School, Monterey, CA93943. Contact: 
;mcghee@cs.nps.navy.mil. Date of last modification: June 4, 2000.

(defun transpose (matrix)           ;A matrix is a list of row vectors.
  (cond ((null (cdr matrix)) (mapcar 'list (car matrix)))
        (t (mapcar 'cons (car matrix) (transpose (cdr matrix))))))

(defun dot-product (vector-1 vector-2);A vector is a list of numerical atoms. 
  (apply '+ (mapcar '* vector-1 vector-2)))     

(defun cross-product (vector-1 vector-2)
  (let ((x1 (first vector-1)) (y1 (second vector-1)) (z1 (third vector-1))
        (x2 (first vector-2)) (y2 (second vector-2)) (z2 (third vector-2)))
       (list (- (* y1 z2) (* y2 z1)) (- (* x2 z1) (* x1 z2)) 
             (- (* x1 y2) (* x2 y1)))))

(defun vector-magnitude (vector) (sqrt (dot-product vector vector)))

(defun normalize-vector (vector) 
  (scalar-multiply (/ (vector-magnitude vector)) vector)) 

(defun post-multiply (matrix vector) 
  (cond ((null (rest matrix)) (list (dot-product (first matrix) vector)))
        (t (cons (dot-product (first matrix) vector)
                 (post-multiply (rest matrix) vector)))))

(defun pre-multiply (vector matrix)
  (post-multiply (transpose matrix) vector))

(defun 3D-postmultiply (3D-array vector)
  (if (null (rest 3D-array)) (list (post-multiply (first 3D-array) vector))
      (cons (post-multiply (first 3D-array) vector)
            (3D-postmultiply (rest 3D-array) vector))))

(defun matrix-multiply (matrix1 matrix2)
  (cond ((null (rest matrix1)) (list (pre-multiply (first matrix1) matrix2)))
        (t (cons (pre-multiply (first matrix1) matrix2)
                 (matrix-multiply (rest matrix1) matrix2)))))

(defun chain-multiply (L)     ;L is a list of names of conformable matrices.
  (cond ((null (cddr L)) (matrix-multiply (eval (car L)) (eval (cadr L))))
        (t (matrix-multiply (eval (car L)) (chain-multiply (cdr L))))))

(defun cycle-left (matrix) (mapcar 'row-cycle-left matrix))

(defun row-cycle-left (row) (append (cdr row) (list (car row))))

(defun cycle-up (matrix) (append (cdr matrix) (list (car matrix))))

(defun unit-vector (one-column length)         ;Column count starts at 1.
  (do ((n length (1- n))
       (vector nil (cons (cond ((= one-column n) 1) (t 0)) vector)))
      ((zerop n) vector)))

(defun unit-matrix (size)
  (do ((row-number size (1- row-number))
       (I nil (cons (unit-vector row-number size) I)))
      ((zerop row-number) I)))

(defun concat-matrix (matrix1 matrix2)
  (if matrix1 (cons (append (first matrix1) (first matrix2))
                    (concat-matrix (rest matrix1) (rest matrix2)))))

(defun augment (matrix)
  (concat-matrix matrix (unit-matrix (length matrix))))

(defun normalize-row (row) (scalar-multiply (/ 1.0 (first row)) row))

(defun scalar-multiply (scalar vector)
  (cond ((null vector) nil) 
        (t (cons (* scalar (first vector))
                 (scalar-multiply scalar (rest vector))))))

(defun solve-first-column (matrix)     ;Reduces first column to (1 0 ... 0).
  (do* ((remaining-row-list matrix (rest remaining-row-list))
        (first-row (normalize-row (first matrix))) 
        (answer (list first-row)
                (cons (vector-add (first remaining-row-list)  
                        (scalar-multiply (- (caar remaining-row-list))
                                          first-row)) answer)))
       ((null (rest remaining-row-list)) (reverse answer))))

(defun vector-add (vector-1 vector-2) (mapcar '+ vector-1 vector-2))

(defun vector-subtract (vector-1 vector-2) (mapcar '- vector-1 vector-2))

(defun matrix-subtract (matrix-1 matrix-2)
  (mapcar #'vector-subtract matrix-1 matrix-2))

(defun subtract-unit-matrix (square-matrix)
  (matrix-subtract square-matrix (unit-matrix (length square-matrix))))

(defun sum-of-elements-squared (matrix)
  (apply '+ (mapcar #'dot-product matrix matrix)))

(defun rms-inverse-error-metric (matrix approximate-inverse-matrix)
  (let* ((M matrix) (M-inv approximate-inverse-matrix) (n (length M))
         (error-matrix (subtract-unit-matrix (matrix-multiply M M-inv)))
         (S (sum-of-elements-squared error-matrix)))
        (/ (sqrt S) n)))

(defun first-square (matrix)  ;Returns leftmost square matrix from argument.
  (do ((size (length matrix))
       (remainder matrix (rest remainder))
       (answer nil (cons (firstn size (first remainder)) answer)))
      ((null remainder) (reverse answer))))

(defun firstn (n list) 
  (cond ((zerop n) nil) 
         (t (cons (first list) (firstn (1- n) (rest list))))))

(defun pivot-row-firstn (n list)
  (append (pivot-row-first (firstn n list)) (nthcdr n list)))  

(defun matrix-inverse (matrix)
  (do* ((M (pivot-row-first (augment matrix))
           (pivot-row-firstn n (cycle-left (cycle-up M))))
        (n (1- (length matrix)) (1- n))
        (exit-flag (= 0 (caar M)) (= 0 (caar M))));Prevents division by zero.
       ((or (minusp n) exit-flag) (if (not exit-flag) (first-square M)))
       (setf M (solve-first-column M))))

(defun pivot-row-first (matrix)  ;This function finds row with largest first
  (cond ((null (cdr matrix)) matrix) ;element and moves it to top of matrix.
        (t (if (> (abs (caar matrix))
                  (abs (caar (pivot-row-first (cdr matrix))))) matrix
               (append (pivot-row-first (cdr matrix)) 
                       (list (car matrix)))))))

(defun dh-matrix (rotate twist length translate)
  (let ((cosrotate (cos rotate)) (sinrotate (sin rotate))
        (costwist (cos twist)) (sintwist (sin twist)))
       (list (list cosrotate (- (* costwist sinrotate)) 
                   (* sintwist sinrotate) (* length cosrotate))
             (list sinrotate (* costwist cosrotate)
                   (- (* sintwist cosrotate)) (* length sinrotate))
             (list 0. sintwist costwist translate)
             (list 0. 0. 0. 1.))))
 
(defun homogeneous-transform (orientation position)
  (let* ((roll (first orientation)) (elevation (second orientation))
         (azimuth (third orientation)) (x (first position))
         (y (second position)) (z (third position))
         (spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation))
         (cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll)))
        (list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi))
                    (+ (* cpsi sth cphi) (* spsi sphi)) x)
              (list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi))
                    (- (* spsi sth cphi) (* cpsi sphi)) y)
              (list (- sth) (* cth sphi) (* cth cphi) z)
              (list 0. 0. 0. 1.))))

(defun inverse-H (H)         ;H is a 4x4 homogeneous transformation matrix.
  (let* ((minus-P (list (- (fourth (first H)))
                        (- (fourth (second H)))
                        (- (fourth (third H)))))
         (inverse-R (transpose (first-square (reverse (rest (reverse H))))))
         (inverse-P (post-multiply inverse-R minus-P)))
        (append (concat-matrix inverse-R (transpose (list inverse-P)))
                (list (list 0 0 0 1)))))

(defun rotation-matrix (euler-angles)
  (let* ((roll (first euler-angles)) (elevation (second euler-angles))
         (azimuth (third euler-angles)) 
         (spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation))
         (cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll)))
        (list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi))
                    (+ (* cpsi sth cphi) (* spsi sphi)))
              (list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi))
                    (- (* spsi sth cphi) (* cpsi sphi)))
              (list (- sth) (* cth sphi) (* cth cphi)))))

(defun body-rate-to-euler-rate-matrix (euler-angles)
  (let* ((roll (first euler-angles)) (elevation (second euler-angles))
         (sth (sin elevation)) (cth (cos elevation)) (tth (tan elevation))
         (sphi (sin roll)) (cphi (cos roll)))
       (list (list 1 (* tth sphi) (* tth cphi))
             (list 0 cphi (- sphi))
             (list 0 (/ sphi cth) (/ cphi cth)))))

(defun rad-to-deg (angle) (* 57.29577951308232 angle))

(defun deg-to-rad (angle) (* 0.017453292519943295 angle))

(defvar M '((1 1 -1) (-1 3 -1) (3 -5 -2)))

(defvar N '((1 2 3) (4 5 6) (7 8 9)))

(defvar L '((3 2 1) (4 5 6) (7 8 9)))

(defun test1 () (matrix-inverse M)) ;Problem 2-9(a) in Kuo.

(defun test2 () (matrix-inverse N))

(defun test3 () (matrix-inverse L))

(defun test4 () (matrix-multiply L (test3)))

(defun test5 () (rms-inverse-error-metric L (matrix-inverse L)))

(defun test6 () (rms-inverse-error-metric M (matrix-inverse M)))

(defvar v '(1 2 3))

(defvar 3D-array '(((1 1 1) (2 2 2)) ((3 3 3) (4 4 4))))

(defun test () (3D-postmultiply 3D-array v))