mat4_inv.lisp

(macrolet
    ((s-row (s row) `(car (nth ,row ,s)))
     (s-element (s row col) `(nth ,col (s-row ,s ,row)))
     (s-scp(s row) `(cdr (nth ,row ,s)))
     (setf- (x y) `(setf ,x (- ,x ,y)))
     (setf/ (x y) `(setf ,x (/ ,x ,y))))
  (labels
      ((elimination! (s row)
 	 (labels ((k-loop (s row j mji k)
			  (if (< k 8)
			      (progn
				(setf- (s-element s j k)
				       (* mji (s-element s row k)))
				(k-loop s row j mji (1+ k)))))
		  (j-loop (s row j)
			  (if (< j 4)
			      (progn
				(k-loop s row j
					(/ (s-element s j row) (s-element s row row))
					(1+ row))
				(setf (s-element s j row) 0)
				(j-loop s row (1+ j))))))
	   (j-loop s row (1+ row))))
       (substitute-step1! (s)
	 (labels
	     ((k-loop (s i j mij k)
		      (if (< k 8)
			  (progn
			    (setf- (s-element s j k)
				   (* mij (s-element s i k)))
			    (k-loop s i j mij (1+ k)))))
	      (j-loop (s i j)
		      (if (>= j 0)
			  (progn
			    (k-loop s i j
				    (/ (s-element s j i)
				       (s-element s i i))
				    (1+ j))
			    (j-loop s i (1- j)))))
	      (i-loop (s i)
		      (if (> i 0)
			  (progn
			    (j-loop s i (1- i))
			    (i-loop s (1- i))))))
	   (i-loop s 3)))
       (identity-matrix ()
		 (make-array '(4 4) :initial-contents
			     (loop for i from 0 below 4 collect
				   (loop for j from 0 below 4 collect
					 (if (= i j) 1 0)))))
       (lu-slice (mat row)
		 (let ((source-row
			(loop for i from 0 below 4 collect
			      (aref mat row i))))
		   (cons
		    (nconc
		     source-row
		     (loop for i from 0 below 4 collect
			   (if (= i row)
			       1
			     0)))
		    (apply #'max (mapcar #'abs source-row)))))
       (bad-scp? ( s &optional (i 0))
		 (if (< i 4)
		     (if (= (s-scp s i) 0)
			 t
		       (bad-scp? s (1+ i)))
		   nil))
       (find-pivot (i s row pivot scp-max)
		     (if (< i 4)
			 (let ((c (abs (/ (s-element s i row)
					  (s-scp s i)))))
			   (if (> c scp-max)
			       (find-pivot (1+ i) s row i c)
			     (find-pivot (1+ i) s row pivot scp-max)))
		       pivot))
       (substitute-step2 (row i)
			 (let ((denom (nth i row)))
			   (mapcar
			    #'(lambda (x) (/ x denom))
			    (nthcdr 4 row)))))
    (defun m4-inv% ( mat )
      (let ((s (loop for i from 0 below 4 collect
		     (lu-slice mat i))))
	(if (bad-scp? s)
	    (return-from m4-inv%
	      (identity-matrix)))
	(loop for i from 0 below 4 do
	      (progn
		(let ((pivot-to (find-pivot i s i i 0)))
		  (if (/= pivot-to i)
		      (rotatef (nth i s) (nth pivot-to s))))
		(elimination! s i)))
	(if (= (s-element s 3 3) 0)
	    (return-from m4-inv%
	      (identity-matrix))
	  (progn
	    (substitute-step1! s)
	    (make-array
	     '(4 4)
	     :initial-contents
	     (loop for i from 0 below 4 collect
		   (substitute-step2 (s-row s i) i)))))))))