#| -*- Mode:LISP; Base: 10; Package:(VISTA-LIBRARY :use (LISP)); Syntax: Common-lisp; Readtable: CL; -*- |# ;;; Copyright (C) LISP Machine, Inc. 1984, 1985, 1986 ;;; See filename "Copyright" for ;;; licensing and release information. (in-package 'vista-library :use '(lisp)) ;;;; Matrix transformations (export '(MAKE-TRANSFORM MAKE-SHORT-FLOAT-IDENTITY-TRANSFORM FILL-TRANSFORM MATRIX-TRANSFORM MULTIPLY-TWO-TRANSFORMS COPY-TRANSFORM INVERT-TRANSFORM)) ;;; Changes ;;; 01/03/86 MPG,PECANN Written, with input from GJC ;;; 04/23/86 EFH export MULTIPLY-TWO-TRANSFORMS,COPY-TRANSFORM, INVERT-TRANSFORM ;;; 05/12/86 PW Fixed typo in multiply-xyz-by-transform-4 ;;; 05/29/86 EFH Fixed typo in multiply-xyz-by-transform-4 ;;; 07/03/86 EFH Snarfed once-only from CommonLoops ;;; 07/04/86 PW Put zl:once-only back in due to lossage ;;; 07/09/86 PW fixed once-only so it only compiles off a lisp-machine ;;; 07/10/86 EFH removed LOOPs from MATRIX-TRANSFORM and fixed it up ;;; 07/10/86 PECANN brought in math:invert-matrix, adapted for commonlisp. ;;; MULTIPLICATION OF POINT WITH TRANSFORM: ;;; ;;; X---> 00 01 02 03 ;;; Y---> 10 11 12 13 ;;; Z---> 20 21 22 23 ;;; W---> 30 31 32 33 ;;; | | | | ;;; V V V V ;;; X' Y' Z' W' (defstruct (TRANSFORM (:conc-name transform-) (:print-function print-transform) (:constructor make-transform (&optional |00| |01| |02| |03| |10| |11| |12| |13| |20| |21| |22| |23| |30| |31| |32| |33|))) |00| |01| |02| |03| |10| |11| |12| |13| |20| |21| |22| |23| |30| |31| |32| |33|) (defun PRINT-TRANSFORM (obj stream ignore) (format stream "~&{~D ~17T ~D ~33T ~D ~49T ~D}~ ~%{~D ~17T ~D ~33T ~D ~49T ~D}~ ~%{~D ~17T ~D ~33T ~D ~49T ~D}~ ~%{~D ~17T ~D ~33T ~D ~49T ~D}" (transform-00 obj) (transform-01 obj) (transform-02 obj) (transform-03 obj) (transform-10 obj) (transform-11 obj) (transform-12 obj) (transform-13 obj) (transform-20 obj) (transform-21 obj) (transform-22 obj) (transform-23 obj) (transform-30 obj) (transform-31 obj) (transform-32 obj) (transform-33 obj))) (eval-when (eval compile load) #+lispm (defmacro once-only (&body body) `(zl:once-only ,@body)) ;;; Lifted from CommonLoops ;;; ONCE-ONLY does the same thing as it does in zetalisp. I should have just lifted ;;; it from there but I am honest. Not only that but this one is written in Common ;;; Lisp. I feel a lot like bootstrapping, or maybe more like rebuilding Rome. #-lispm (defmacro once-only (vars &body body) (let ((gensym-var (gensym)) (run-time-vars (gensym)) (run-time-vals (gensym)) (expand-time-val-forms ())) (dolist (var vars) (push `(if (or (symbolp ,var) (numberp ,var) (and (listp ,var) (member (car ,var) '(quote function)))) ,var (let ((,gensym-var (gensym))) (push ,gensym-var ,run-time-vars) (push ,var ,run-time-vals) ,gensym-var)) expand-time-val-forms)) `(let* (,run-time-vars ,run-time-vals (wrapped-body ((lambda ,vars . ,body) . ,(reverse expand-time-val-forms)))) `((lambda ,(nreverse ,run-time-vars) ,wrapped-body) . ,(nreverse ,run-time-vals))))) ) (defmacro FILL-TRANSFORM (obj |00| |01| |02| |03| |10| |11| |12| |13| |20| |21| |22| |23| |30| |31| |32| |33|) (once-only (obj) `(setf (transform-00 ,obj) ,|00| (transform-01 ,obj) ,|01| (transform-02 ,obj) ,|02| (transform-03 ,obj) ,|03| (transform-10 ,obj) ,|10| (transform-11 ,obj) ,|11| (transform-12 ,obj) ,|12| (transform-13 ,obj) ,|13| (transform-20 ,obj) ,|20| (transform-21 ,obj) ,|21| (transform-22 ,obj) ,|22| (transform-23 ,obj) ,|23| (transform-30 ,obj) ,|30| (transform-31 ,obj) ,|31| (transform-32 ,obj) ,|32| (transform-33 ,obj) ,|33|))) (defmacro transform-as-rows (obj) `(values (list (transform-00 ,obj) (transform-01 ,obj) (transform-02 ,obj) (transform-03 ,obj)) (list (transform-10 ,obj) (transform-11 ,obj) (transform-12 ,obj) (transform-13 ,obj)) (list (transform-20 ,obj) (transform-21 ,obj) (transform-22 ,obj) (transform-23 ,obj)) (list (transform-30 ,obj) (transform-31 ,obj) (transform-32 ,obj) (transform-33 ,obj)) ) ) (defmacro transform-as-columns (obj) `(values (list (transform-00 ,obj) (transform-10 ,obj) (transform-20 ,obj) (transform-30 ,obj)) (list (transform-01 ,obj) (transform-11 ,obj) (transform-21 ,obj) (transform-31 ,obj)) (list (transform-02 ,obj) (transform-12 ,obj) (transform-22 ,obj) (transform-32 ,obj)) (list (transform-03 ,obj) (transform-13 ,obj) (transform-23 ,obj) (transform-33 ,obj)) ) ) (defun fill-transform-function (obj |00| |01| |02| |03| |10| |11| |12| |13| |20| |21| |22| |23| |30| |31| |32| |33|) (setf (transform-00 obj) |00| (transform-01 obj) |01| (transform-02 obj) |02| (transform-03 obj) |03| (transform-10 obj) |10| (transform-11 obj) |11| (transform-12 obj) |12| (transform-13 obj) |13| (transform-20 obj) |20| (transform-21 obj) |21| (transform-22 obj) |22| (transform-23 obj) |23| (transform-30 obj) |30| (transform-31 obj) |31| (transform-32 obj) |32| (transform-33 obj) |33|) obj ) (defun MAKE-SHORT-FLOAT-IDENTITY-TRANSFORM () (make-transform 1.0s0 0.0s0 0.0s0 0.0s0 0.0s0 1.0s0 0.0s0 0.0s0 0.0s0 0.0s0 1.0s0 0.0s0 0.0s0 0.0s0 0.0s0 1.0s0 )) ;(defstruct (POINT (:conc-name point-) :named (:print-function print-point) ; (:constructor make-point (&optional |0| |1| |2| |3|))) ; |0| |1| |2| |3|) ;(defun PRINT-POINT (obj stream ignore) ; (format stream "{~D ~D ~D ~D}" ; (point-0 obj) (point-1 obj) (point-2 obj) (point-3 obj))) ;(defsubst FILL-POINT (obj |0| |1| |2| |3|) ; (setf (point-0 obj) |0| ; (point-1 obj) |1| ; (point-2 obj) |2| ; (point-3 obj) |3|) ; obj) ;(defun MAKE-SHORT-FLOAT-0001-POINT () ; (make-point 0.0s0 0.0s0 0.0s0 1.0s0)) ;;; Hairy macros to generate efficient matrix multiply ;;; Written by the ever hirsute EFH. #| (MATRIX-TRANSFORM 1 0 0 0 0 cos sin 0 0 -sin cos 0 0 0 0 1 trans) (MATRIX-TRANSFORM 1 0 0 0 0 (* cos sin) (* sin cos) 0 0 -sin cos 0 0 0 0 1 trans) (MATRIX-TRANSFORM 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 trans) (MATRIX-TRANSFORM 2 0 0 0 0 3 0 0 0 0 4 0 0 0 0 5 trans) |# (defmacro 4DOT-PRODUCT (v1 v2 v3 v4 u1 u2 u3 u4) (let ((args (delete 0 (mapcan #'(lambda (v u) (cond ((numberp u) (cond ((zerop u) '()) ((= u 1) (list v)) ((numberp v) (list (* u v))) (t `((* ,u ,v))))) ((numberp v) (cond ((zerop v) '()) ((= v 1) (list u)) (t `((* ,u ,v))))) (t `((* ,u ,v))))) (list v1 v2 v3 v4) (list u1 u2 u3 u4))))) (cond ((null args) 0) ((null (cdr args)) (car args)) (t (cons '+ args))))) (eval-when (eval compile load) (defconstant *transform-accessors* #2A((transform-00 transform-01 transform-02 transform-03) (transform-10 transform-11 transform-12 transform-13) (transform-20 transform-21 transform-22 transform-23) (transform-30 transform-31 transform-32 transform-33)))) (defmacro MATRIX-TRANSFORM (m00 m01 m02 m03 m10 m11 m12 m13 m20 m21 m22 m23 m30 m31 m32 m33 matrix) (once-only (m00 m01 m02 m03 m10 m11 m12 m13 m20 m21 m22 m23 m30 m31 m32 m33 matrix) (let* ((c0 (list m00 m10 m20 m30)) (c1 (list m01 m11 m21 m31)) (c2 (list m02 m12 m22 m32)) (c3 (list m03 m13 m23 m33)) (mrefs '()) (forms (mapcan #'(lambda (row) (mapcar #'(lambda (col) `((.mref. ,row ,col) ,(macroexpand `(4dot-product (.mref. 0 ,col) (.mref. 1 ,col) (.mref. 2 ,col) (.mref. 3 ,col) ,(nth row c0) ,(nth row c1) ,(nth row c2) ,(nth row c3))))) '(0 1 2 3))) '(0 1 2 3)))) (labels ((hack-mrefs (form) (if (atom form) form (if (eq (car form) '.mref.) (progn (let ((ref (cons (second form) (third form)))) (unless (member ref mrefs :test #'equal) (push ref mrefs))) (intern (format nil "~a-~a~a" matrix (second form) (third form)))) (cons (car form) (mapcar #'hack-mrefs (cdr form))))))) (setq forms (mapcar #'(lambda (f) `(setf (,(aref *transform-accessors* (second (first f)) (third (first f))) ,matrix) ,(hack-mrefs (second f)))) (delete-if #'(lambda (f) (equal (first f) (second f))) forms)))) `(let ,(mapcar #'(lambda (r) `(,(intern (format nil "~a-~a~a" matrix (car r) (cdr r))) (,(aref *transform-accessors* (car r) (cdr r)) ,matrix))) mrefs) ,@forms)))) (defun multiply-2-xy-by-transform-2 (x1 y1 x2 y2 transform) (declare (values mx1 my1 mx2 my2)) (let ((T-00 (transform-00 transform)) (T-01 (transform-01 transform)) ; (T-02 (transform-02 transform)) (T-03 (transform-03 transform)) (T-10 (transform-10 transform)) (T-11 (transform-11 transform)) ; (T-12 (transform-12 transform)) (T-13 (transform-13 transform)) ; (T-20 (transform-20 transform)) (T-21 (transform-21 transform)) ; (T-22 (transform-22 transform)) (T-23 (transform-23 transform)) (T-30 (transform-30 transform)) (T-31 (transform-31 transform)) ; (T-32 (transform-32 transform)) (T-33 (transform-33 transform)) ) (values (4dot-product x1 y1 0 1 T-00 T-10 T-20 T-30) (4dot-product x1 y1 0 1 T-01 T-11 T-21 T-31) (4dot-product x2 y2 0 1 T-00 T-10 T-20 T-30) (4dot-product x2 y2 0 1 T-01 T-11 T-21 T-31)))) (defun multiply-xy-by-transform-2 (x y transform) "Transforms a 2D point (x, y) assuming z is 0, and returns x and y" (declare (values x y)) (values (4dot-product x y 0 1 (transform-00 transform) (transform-10 transform) (transform-20 transform) (transform-30 transform)) (4dot-product x y 0 1 (transform-01 transform) (transform-11 transform) (transform-21 transform) (transform-31 transform)))) (defun multiply-xy-by-transform-4 (x y transform) "Transforms a 2D point (x, y) assuming z is 0, and returns x, y, z, and w." (declare (values x y z w)) (values (4dot-product x y 0 1 (transform-00 transform) (transform-10 transform) (transform-20 transform) (transform-30 transform)) (4dot-product x y 0 1 (transform-01 transform) (transform-11 transform) (transform-21 transform) (transform-31 transform)) (4dot-product x y 0 1 (transform-02 transform) (transform-12 transform) (transform-22 transform) (transform-32 transform)) (4dot-product x y 0 1 (transform-03 transform) (transform-13 transform) (transform-23 transform) (transform-32 transform)))) (defun MULTIPLY-2-XY-BY-TRANSFORM-4 (x1 y1 x2 y2 transform) (declare (values x1 y1 z1 w1 x2 y2 z2 w2)) (let ((T-00 (transform-00 transform)) (T-01 (transform-01 transform)) (T-02 (transform-02 transform)) (T-03 (transform-03 transform)) (T-10 (transform-10 transform)) (T-11 (transform-11 transform)) (T-12 (transform-12 transform)) (T-13 (transform-13 transform)) ; not really needed ; (T-20 (transform-20 transform)) (T-21 (transform-21 transform)) ; (T-22 (transform-22 transform)) (T-23 (transform-23 transform)) (T-30 (transform-30 transform)) (T-31 (transform-31 transform)) (T-32 (transform-32 transform)) (T-33 (transform-33 transform))) (values (4dot-product x1 y1 0 1 T-00 T-10 T-20 T-30) (4dot-product x1 y1 0 1 T-01 T-11 T-21 T-31) (4dot-product x1 y1 0 1 T-02 T-12 T-22 T-32) (4dot-product x1 y1 0 1 T-03 T-13 T-23 T-33) (4dot-product x2 y2 0 1 T-00 T-10 T-20 T-30) (4dot-product x2 y2 0 1 T-01 T-11 T-21 T-31) (4dot-product x2 y2 0 1 T-02 T-12 T-22 T-32) (4dot-product x2 y2 0 1 T-03 T-13 T-23 T-33)))) (defun MULTIPLY-XYZ-BY-TRANSFORM-4 (x y z transform) (let* ((tr transform)) (values (4dot-product x y z 1 (transform-00 tr) (transform-10 tr) (transform-20 tr) (transform-30 tr)) (4dot-product x y z 1 (transform-01 tr) (transform-11 tr) (transform-21 tr) (transform-31 tr)) (4dot-product x y z 1 (transform-02 tr) (transform-12 tr) (transform-22 tr) (transform-32 tr)) (4dot-product x y z 1 (transform-03 tr) (transform-13 tr) (transform-23 tr) (transform-33 tr)) ))) ;(defun MULTIPLY-POINT-BY-TRANSFORM (point transform output-point) ; (let* ((p point) ; (tr transform) ; (P-0 (point-0 p)) ; (P-1 (point-1 p)) ; (P-2 (point-2 p)) ; (P-3 (point-3 p)) ; ) ; (fill-point ; output-point ; (4dot-product p-0 p-1 p-2 p-3 (transform-00 tr) (transform-10 tr) (transform-20 tr) (transform-30 tr)) ; (4dot-product p-0 p-1 p-2 p-3 (transform-01 tr) (transform-11 tr) (transform-21 tr) (transform-31 tr)) ; (4dot-product p-1 p-1 p-2 p-3 (transform-02 tr) (transform-12 tr) (transform-22 tr) (transform-32 tr)) ; (4dot-product p-0 p-1 p-2 p-3 (transform-03 tr) (transform-13 tr) (transform-23 tr) (transform-32 tr)) ; ))) (defun MULTIPLY-2-XYZ-BY-TRANSFORM-4 (x1 y1 z1 x2 y2 z2 transform) (declare (values x1 y1 z1 w1 x2 y2 z2 w2)) (let ((T-00 (transform-00 transform)) (T-01 (transform-01 transform)) (T-02 (transform-02 transform)) (T-03 (transform-03 transform)) (T-10 (transform-10 transform)) (T-11 (transform-11 transform)) (T-12 (transform-12 transform)) (T-13 (transform-13 transform)) (T-20 (transform-20 transform)) (T-21 (transform-21 transform)) (T-22 (transform-22 transform)) (T-23 (transform-23 transform)) (T-30 (transform-30 transform)) (T-31 (transform-31 transform)) (T-32 (transform-32 transform)) (T-33 (transform-33 transform))) (values (4dot-product x1 y1 z1 1 T-00 T-10 T-20 T-30) (4dot-product x1 y1 z1 1 T-01 T-11 T-21 T-31) (4dot-product x1 y1 z1 1 T-02 T-12 T-22 T-32) (4dot-product x1 y1 z1 1 T-03 T-13 T-23 T-33) (4dot-product x2 y2 z2 1 T-00 T-10 T-20 T-30) (4dot-product x2 y2 z2 1 T-01 T-11 T-21 T-31) (4dot-product x2 y2 z2 1 T-02 T-12 T-22 T-32) (4dot-product x2 y2 z2 1 T-03 T-13 T-23 T-33)))) ;(defun MULTIPLY-TWO-POINTS-BY-TRANSFORM (point1 point2 transform output-point1 output-point2) ; (let* ((p1 point1) (p2 point2) (tr transform) ; (P1-0 (point-0 p1)) (P1-1 (point-1 p1)) (P1-2 (point-2 p1)) (P1-3 (point-3 p1)) ; (P2-0 (point-0 p2)) (P2-1 (point-1 p2)) (P2-2 (point-2 p2)) (P2-3 (point-3 p2)) ; (T-00 (transform-00 tr)) (T-01 (transform-01 tr)) ; (T-02 (transform-02 tr)) (T-03 (transform-03 tr)) ; (T-10 (transform-10 tr)) (T-11 (transform-11 tr)) ; (T-12 (transform-12 tr)) (T-13 (transform-13 tr)) ; (T-20 (transform-20 tr)) (T-21 (transform-21 tr)) ; (T-22 (transform-22 tr)) (T-23 (transform-23 tr)) ; (T-30 (transform-30 tr)) (T-31 (transform-31 tr)) ; (T-32 (transform-32 tr)) (T-33 (transform-33 tr)) ; ) ; (fill-point ; output-point1 ; (4dot-product p1-0 p1-1 p1-2 p1-3 T-00 T-10 T-20 T-30) ; (4dot-product p1-0 p1-1 p1-2 p1-3 T-01 T-11 T-21 T-31) ; (4dot-product p1-0 p1-1 p1-2 p1-3 T-02 T-12 T-22 T-32) ; (4dot-product p1-0 p1-1 p1-2 p1-3 T-03 T-13 T-23 T-33) ; ) ; (fill-point ; output-point2 ; (4dot-product p2-0 p2-1 p2-2 p2-3 T-00 T-10 T-20 T-30) ; (4dot-product p2-0 p2-1 p2-2 p2-3 T-01 T-11 T-21 T-31) ; (4dot-product p2-0 p2-1 p2-2 p2-3 T-02 T-12 T-22 T-32) ; (4dot-product p2-0 p2-1 p2-2 p2-3 T-03 T-13 T-23 T-33) ; ) ; )) (defun MULTIPLY-TWO-TRANSFORMS (transform1 transform2 destination-transform) (let* ((t1 transform1) (t2 transform2) (T1-00 (transform-00 t1)) (T1-01 (transform-01 t1)) (T1-02 (transform-02 t1)) (T1-03 (transform-03 t1)) (T1-10 (transform-10 t1)) (T1-11 (transform-11 t1)) (T1-12 (transform-12 t1)) (T1-13 (transform-13 t1)) (T1-20 (transform-20 t1)) (T1-21 (transform-21 t1)) (T1-22 (transform-22 t1)) (T1-23 (transform-23 t1)) (T1-30 (transform-30 t1)) (T1-31 (transform-31 t1)) (T1-32 (transform-32 t1)) (T1-33 (transform-33 t1)) (T2-00 (transform-00 t2)) (T2-01 (transform-01 t2)) (T2-02 (transform-02 t2)) (T2-03 (transform-03 t2)) (T2-10 (transform-10 t2)) (T2-11 (transform-11 t2)) (T2-12 (transform-12 t2)) (T2-13 (transform-13 t2)) (T2-20 (transform-20 t2)) (T2-21 (transform-21 t2)) (T2-22 (transform-22 t2)) (T2-23 (transform-23 t2)) (T2-30 (transform-30 t2)) (T2-31 (transform-31 t2)) (T2-32 (transform-32 t2)) (T2-33 (transform-33 t2)) ) (fill-transform destination-transform (4dot-product T1-00 T1-01 T1-02 T1-03 T2-00 T2-10 T2-20 T2-30) (4dot-product T1-00 T1-01 T1-02 T1-03 T2-01 T2-11 T2-21 T2-31) (4dot-product T1-00 T1-01 T1-02 T1-03 T2-02 T2-12 T2-22 T2-32) (4dot-product T1-00 T1-01 T1-02 T1-03 T2-03 T2-13 T2-23 T2-33) (4dot-product T1-10 T1-11 T1-12 T1-13 T2-00 T2-10 T2-20 T2-30) (4dot-product T1-10 T1-11 T1-12 T1-13 T2-01 T2-11 T2-21 T2-31) (4dot-product T1-10 T1-11 T1-12 T1-13 T2-02 T2-12 T2-22 T2-32) (4dot-product T1-10 T1-11 T1-12 T1-13 T2-03 T2-13 T2-23 T2-33) (4dot-product T1-20 T1-21 T1-22 T1-23 T2-00 T2-10 T2-20 T2-30) (4dot-product T1-20 T1-21 T1-22 T1-23 T2-01 T2-11 T2-21 T2-31) (4dot-product T1-20 T1-21 T1-22 T1-23 T2-02 T2-12 T2-22 T2-32) (4dot-product T1-20 T1-21 T1-22 T1-23 T2-03 T2-13 T2-23 T2-33) (4dot-product T1-30 T1-31 T1-32 T1-33 T2-00 T2-10 T2-20 T2-30) (4dot-product T1-30 T1-31 T1-32 T1-33 T2-01 T2-11 T2-21 T2-31) (4dot-product T1-30 T1-31 T1-32 T1-33 T2-02 T2-12 T2-22 T2-32) (4dot-product T1-30 T1-31 T1-32 T1-33 T2-03 T2-13 T2-23 T2-33) ) )) ;;; would copy-array-contents be faster on lambda? ;;; replace on other common lisps ? (defun COPY-TRANSFORM (from-transform to-transform) (setf (transform-00 to-transform) (transform-00 from-transform) (transform-01 to-transform) (transform-01 from-transform) (transform-02 to-transform) (transform-02 from-transform) (transform-03 to-transform) (transform-03 from-transform) (transform-10 to-transform) (transform-10 from-transform) (transform-11 to-transform) (transform-11 from-transform) (transform-12 to-transform) (transform-12 from-transform) (transform-13 to-transform) (transform-13 from-transform) (transform-20 to-transform) (transform-20 from-transform) (transform-21 to-transform) (transform-21 from-transform) (transform-22 to-transform) (transform-22 from-transform) (transform-23 to-transform) (transform-23 from-transform) (transform-30 to-transform) (transform-30 from-transform) (transform-31 to-transform) (transform-31 from-transform) (transform-32 to-transform) (transform-32 from-transform) (transform-33 to-transform) (transform-33 from-transform))) ;;;from Zetalisp math:invert-matrix (DEFUN INVERT-TRANSFORM (transform into-transform &AUX (INTO-MATRIX (make-array '(4 4))) (DIM 4)) "Computes the inverse of TRANSFORM using the Gauss-Jordan algorithm with partial pivoting." ; (SETQ DIM (ARRAY-DIMENSION MATRIX 0)) ; (IF INTO-MATRIX ; (OR (AND (EQ DIM (ARRAY-DIMENSION INTO-MATRIX 0)) ; (EQ DIM (ARRAY-DIMENSION INTO-MATRIX 1))) ; (ERROR "INTO-MATRIX is not correct for the inverse of MATRIX")) ; (SETQ INTO-MATRIX (MAKE-ARRAY (LIST DIM DIM) ; :ELEMENT-TYPE (LET ((TEM (ARRAY-ELEMENT-TYPE MATRIX))) ; (IF (SUBTYPEP TEM 'INTEGER) T TEM))))) (setf (aref into-matrix 0 0) (transform-00 transform)) (setf (aref into-matrix 0 1) (transform-01 transform)) (setf (aref into-matrix 0 2) (transform-02 transform)) (setf (aref into-matrix 0 3) (transform-03 transform)) (setf (aref into-matrix 1 0) (transform-10 transform)) (setf (aref into-matrix 1 1) (transform-11 transform)) (setf (aref into-matrix 1 2) (transform-12 transform)) (setf (aref into-matrix 1 3) (transform-13 transform)) (setf (aref into-matrix 2 0) (transform-20 transform)) (setf (aref into-matrix 2 1) (transform-21 transform)) (setf (aref into-matrix 2 2) (transform-22 transform)) (setf (aref into-matrix 2 3) (transform-23 transform)) (setf (aref into-matrix 3 0) (transform-30 transform)) (setf (aref into-matrix 3 1) (transform-31 transform)) (setf (aref into-matrix 3 2) (transform-32 transform)) (setf (aref into-matrix 3 3) (transform-33 transform)) ;(DOTIMES (I (ARRAY-LENGTH INTO-MATRIX)) ; (SETF (AR-1-FORCE INTO-MATRIX I) ; (+ 0.0s0 (AR-1-FORCE INTO-MATRIX I)))) (LET ((COLS (MAKE-ARRAY DIM)) (TEM (MAKE-ARRAY (LIST DIM DIM))) (COLS-USED (MAKE-ARRAY DIM :ELEMENT-TYPE 'BIT :INITIAL-ELEMENT 0))) (DO ((I 0 (1+ I)) (J)) ((>= I DIM)) ;; Find the greatest element in this row in an unused column (SETQ J (DO ((J 0 (1+ J)) (MAX 0) POS TEM1) ((>= J DIM) (AND (ZEROP MAX) (ERROR "The matrix is singular.")) POS) (AND (ZEROP (AREF COLS-USED J)) (> (SETQ TEM1 (ABS (AREF INTO-MATRIX I J))) MAX) (SETQ MAX TEM1 POS J)))) (SETF (AREF COLS I) J) (SETF (AREF COLS-USED J) 1) ;; Pivot about I,J (DO ((K 0 (1+ K)) (ELEM-I-J (AREF INTO-MATRIX I J))) ((>= K DIM)) (DO ((L 0 (1+ L)) (ELEM-K-J (AREF INTO-MATRIX K J)) (ELEM)) ((>= L DIM)) (SETQ ELEM (AREF INTO-MATRIX K L)) (SETF (AREF TEM K L) (IF (= K I) ;Same row? (IF (= L J) ;Corner itself? (/ ELEM) (/ ELEM ELEM-I-J)) (IF (= L J) ;Same column? (- (/ ELEM ELEM-I-J)) (- ELEM (/ (* ELEM-K-J (AREF INTO-MATRIX I L)) ELEM-I-J))))))) (dotimes (i dim) (dotimes (j dim) (setf (aref into-matrix i j) (aref tem i j))))) ;; And finally permute (DOTIMES (I DIM) (DO ((J 0 (1+ J)) (K (AREF COLS I))) ((>= J DIM)) (SETF (AREF TEM K J) (AREF INTO-MATRIX I J)))) (DOTIMES (I DIM) (DO ((K 0 (1+ K)) (J (AREF COLS I))) ((>= K DIM)) (SETF (AREF INTO-MATRIX K I) (AREF TEM K J))))) (setf (transform-00 into-transform) (aref into-matrix 0 0)) (setf (transform-01 into-transform) (aref into-matrix 0 1)) (setf (transform-02 into-transform) (aref into-matrix 0 2)) (setf (transform-03 into-transform) (aref into-matrix 0 3)) (setf (transform-10 into-transform) (aref into-matrix 1 0)) (setf (transform-11 into-transform) (aref into-matrix 1 1)) (setf (transform-12 into-transform) (aref into-matrix 1 2)) (setf (transform-13 into-transform) (aref into-matrix 1 3)) (setf (transform-20 into-transform) (aref into-matrix 2 0)) (setf (transform-21 into-transform) (aref into-matrix 2 1)) (setf (transform-22 into-transform) (aref into-matrix 2 2)) (setf (transform-23 into-transform) (aref into-matrix 2 3)) (setf (transform-30 into-transform) (aref into-matrix 3 0)) (setf (transform-31 into-transform) (aref into-matrix 3 1)) (setf (transform-32 into-transform) (aref into-matrix 3 2)) (setf (transform-33 into-transform) (aref into-matrix 3 3)) into-transform)