vis-ftp.cs.umass.edu/vis-ftp.cs.umass.edu.tar/vis-ftp.cs.umass.edu/pub/Software/ASCENDER/ascendMar8.tar/UMass/Epipolar/vectors.lisp

Lisp/Scheme | 1995-07-20 | 19KB | 472 lines

;;; File VECTORS.LISP  - simple vector and matrix manipulation
;;; Bob Collins, University of Massachusetts
;;; general contents:
;;;   cross product,  dot product,  computing vector length,
;;;   scaling, adding, and subtracting vectors
;;;   normalizing vectors, plane and line normals
;;;   matrix multiplication
;;;   determinant and minors of 3x3 matrices
;;;
;;; converted from the lisp machine version -- Bob Collins, 8/11/94
;;;   added sequence-type function to replace type-of for the result type
;;;     of mapping over vectors (this was needed for clisp, lucid was OK).
;;;   wrote la::transpose to replace the nonexistant math::matrix-transpose
;;;   wrote la::listarray to replace the nonexistant listarray

(in-package 'linear-algebra :nicknames '("LA"))

(export '(cross-product dot-product vector-length
          normalize normalize2 normalize3 normalize-line normalize-plane
      scale-vector vector-subtract vector-add
      v+ v- vs vdot vcross vnorm vmag vmag2
      matmult matplus matminus scale-matrix transpose
      m* m+ m- ms mt
      compute-minors3 compute-minor3 cofactors3
      cofactors-mat3 adjoint3 inverse3
      matrix-with-columns diag
      solve-2-by-2
      unit-normal unit-vector-angle))

(defmacro sequence-type (thing)
  `(let ((evalthing ,thing))
     (if (consp evalthing) 'list (type-of evalthing))))

(defun listarray (arr)
  (ecase (length (array-dimensions arr))
    (1 (map 'list #'identity arr))
    (2 (let ((res nil))
      (dotimes (i (array-dimension arr 0))
        (dotimes (j (array-dimension arr 1))
           (push (aref arr i j) res)))
      (nreverse res)))))

(defun vectorize (thing)
  "  if thing is a sequence, everything is OK.  It can also be an array, in
  which case a listarray is done to it - which gives obvious results in the
  case of a matrix consisting of a single row or column, less obvious in
  other cases. If it is a number, then a list of one number is return."
  (cond
    ((or (vectorp thing) (listp thing)) thing)
    ((arrayp thing) (listarray thing))
    ((numberp thing) (list thing))
    (t (error "argument ~s was not an array, vector, list or number" thing))))

(defun cross-product3 (x1 y1 z1 x2 y2 z2)
  "Returns the cross product of two 3-dimensional vectors."
  (list (- (* y1 z2) (* z1 y2))
    (- (* x2 z1) (* x1 z2))
    (- (* x1 y2) (* y1 x2))))

(defun cross-product (vector1 vector2)
  "  Returns a new vector which is the cross product of 2 vectors.
  They should both have either 2 or 3 elements.  The new vector has
  exactly 3 elements."
  (setf vector1 (vectorize vector1) vector2 (vectorize vector2))
  (multiple-value-bind (x1 y1 z1) (values-list vector1)
    (multiple-value-bind (x2 y2 z2) (values-list vector2)
      (cross-product3 x1 y1 (or z1 0) x2 y2 (or z2 0)))))

(defun abs-max (sequence)
  "Returns the largest element magnitude."
  (reduce #'(lambda (x y) (max x (abs y)))
      sequence
      :initial-value (abs (elt sequence 0))
      :start 1))

(defun dot-product (vector1 vector2)
  "  Returns the dot product of vector1 and vector2, both of which must be sequences.
  If both vectors are not the same length, the shorter is treated as if it were padded
  with zeros at the end."
  ;;note: the following is more accurate in some cases than the obvious implementation
  (setf vector1 (vectorize vector1) vector2 (vectorize vector2))
  (let ((result 0)
    (max1 (abs-max vector1))
    (max2 (abs-max vector2)))
    (if (or (zerop max1) (zerop max2))
    0
    (progn
      (map nil #'(lambda (n1 n2) (incf result (* (/ n1 max1) (/ n2 max2)))) vector1 vector2)
      (* result max1 max2)))))

(defun dot-product3 (x1 y1 z1 x2 y2 z2)
  "Returns the dot product of two 3-dimensional vectors."
  (dot-product (list x1 y1 z1) (list x2 y2 z2)))

(defun squared-vector-length (&rest vector)
  "Returns the square of the vector length; basically dotproduct(vector,vector)."
  (unless (numberp (elt vector 0)) (setf vector (vectorize (car vector))))
  (dot-product vector vector))

(defun vector-length (&rest vector)
  "Returns vector length of vector = sqrt(dotproduct(vector,vector))"
  (unless (numberp (elt vector 0)) (setf vector (vectorize (car vector))))
  (sqrt (dot-product vector vector)))

(defun scale-vector (k &rest vector)
  "Returns a new vector scaled by k."
  (unless (numberp (elt vector 0)) (setf vector (car vector)))
  (setf vector (vectorize vector))
  (map (sequence-type vector)
       #'(lambda (n) (* k n))
       vector))

(defun vector-subtract (vector1 vector2)
  "subtracts vector2 from vector1 and returns the result"
  (setf vector1 (vectorize vector1) vector2 (vectorize vector2))
  (map (sequence-type vector1)
       #'-
       vector1
       vector2))

(defun vector-add (vector1 vector2)
  "adds two vectors and returns the result"
  (setf vector1 (vectorize vector1) vector2 (vectorize vector2))
  (map (sequence-type vector1)
       #'+
       vector1
       vector2))

(defun normalize (&rest vector)
  "Returns a new vector, scaled to have length 1."
  (unless (numberp (elt vector 0)) (setf vector (vectorize (car vector))))
  (let ((max (abs-max vector)))
    (if (zerop max)
    (progn (cerror "return ~s" "vector of length 0 cannot be normalized" vector)
           vector)
    (let* ((newvec (map 'list #'(lambda (x) (/ x max)) vector))
           (len-squared (dot-product newvec newvec)))
      (map (sequence-type vector)
           #'(lambda (n) (* (signum n) (sqrt (/ (* n n) len-squared))))
           newvec)))))


(defun v+ (&rest vectors)
  "  Adds a list of vector together, if they have compatible dimensions."
  (when vectors
    (setf vectors (mapcar #'vectorize vectors))
    (apply #'map (sequence-type (car vectors)) #'+ vectors)))


(defun v- (&rest vectors)
  "  Subtracts a list of vectors from the first, or unary minus if only one."
  (when vectors
    (if (= 1 (length vectors))
    (scale-vector -1 (car vectors))
    (let ((vectors (mapcar #'vectorize vectors)))
      (apply #'map (sequence-type (car vectors)) #'- vectors)))))

(defun vs (k vec)
  "  Return a new vector scaled by the given amount."
  (scale-vector k vec))

(defun vdot (vec1 vec2)
  "  Returns the dot product of vector1 and vector2, both of which must be sequences.
  If both vectors are not the same length, the shorter is treated as if it were padded
  with zeros at the end."
  (dot-product vec1 vec2))

(defun vcross (vec1 vec2)
  "  Returns a new vector which is the cross product of 2 vectors.
  They should both have either 2 or 3 elements.  The new vector has
  exactly 3 elements."
  (cross-product vec1 vec2))

(defun vnorm (vec)
  "Returns a new vector, scaled to have length 1."
  (normalize vec))

(defun vmag (vector)
  "Returns vector magnitude = length of vector = sqrt(dotproduct(vector,vector))"
  (vector-length vector))

(defun vmag2 (vector)
  "Returns the square of the vector magnitude; basically dotproduct(vector,vector)."
  (squared-vector-length vector))

(defun normalize2 (&rest vector)
  "Returns a new vector, scaled so that the sum of the squares of the first two elements is 1."
  (unless (numberp (elt vector 0)) (setf vector (vectorize (car vector))))
  (let ((n1 (elt vector 0))
    (n2 (elt vector 1)))
    (unless (and n1 n2) (error "vector must have at least two elements"))
    (let ((len-squared (+ (* n1 n1) (* n2 n2))))
      (if (zerop len-squared)
      (progn (cerror "return ~s" "vector cannot be normalized by 0" vector)
           vector)
      (map (sequence-type vector)
           #'(lambda (n) (* (signum n) (sqrt (/ (* n n) len-squared))))
           vector)))))

(defun normalize3 (&rest vector)
  "Returns a new vector, scaled so that the sum of the squares of the first three elements is 1."
  (unless (numberp (elt vector 0)) (setf vector (vectorize (car vector))))
  (let ((n1 (elt vector 0))
    (n2 (elt vector 1))
    (n3 (elt vector 2)))
    (unless (and n1 n2 n3) (error "vector must have at least three elements"))
    (let ((len-squared (+ (* n1 n1) (* n2 n2) (* n3 n3))))
      (if (zerop len-squared)
      (progn (cerror "return ~s" "vector cannot be normalized by 0" vector)
         vector)
      (map (sequence-type vector)
           #'(lambda (n) (* (signum n) (sqrt (/ (* n n) len-squared))))
           vector)))))

(defun normalize-line (vec)
  (unless (= (length vec) 3) (error "line must have 3 parameters"))
  (normalize2 vec))

(defun normalize-plane (vec)
  (unless (= (length vec) 4) (error "plane must have 4 parameters"))
  (normalize3 vec))

(defun matricize (thing)
  "  if thing is a matrix, everything is OK.  If it is a vector or list,
  it is treated as a column vector and the appropriate Nx1 array is returned.
  If it is a number, then a 1X1 matrix is returned."
  (cond
    ((and (arrayp thing) (not (vectorp thing))) thing)
    ((or (vectorp thing) (listp thing))
        (let ((arr (make-array (list (length thing) 1))))
      (dotimes (i (length thing) arr)
        (setf (aref arr i 0) (elt thing i)))))
    ((numberp thing) (make-array '(1 1) :initial-element thing))
    (t (error "argument ~s was not an array, vector, list or number" thing))))

(defun column-vector (vec)
  (matricize vec))

(defun matmult (a b)
  "  Multiplies two dimensional matrices a and b having size (lxm) and (mxn)
  respectively. The resulting matrix has size (lxn)."
  (setf a (matricize a) b (matricize b))
  (let ((l (array-dimension a 0))
    (m (array-dimension a 1))
    (m2 (array-dimension b 0))
    (n (array-dimension b 1)))
    (unless (= m m2) (error "matrices have incompatible dimensions"))
    (let ((result (make-array (list l n) :element-type (array-element-type a))))
      (dotimes (i l)
    (dotimes (j n)
      (let ((sum 0))
        (dotimes (k m)
          (incf sum (* (aref a i k) (aref b k j))))
        (setf (aref result i j) sum))))
      result)))

(defun matplus (a b)
  "  Adds two matrices together."
  (setf a (matricize a) b (matricize b))
  (let ((l (array-dimension a 0))
    (m (array-dimension a 1)))
    (unless (and (= l (array-dimension b 0)) (= m (array-dimension b 1)))
      (error "matrices have incompatible dimensions"))
    (let ((result (make-array (list l m) :element-type (array-element-type a))))
      (dotimes (i l)
    (dotimes (j m)
      (setf (aref result i j) (+ (aref a i j) (aref b i j)))))
      result)))

(defun matminus (a b)
  "  Subtracts second matrix from first."
  (setf a (matricize a) b (matricize b))
  (let ((l (array-dimension a 0))
    (m (array-dimension a 1)))
    (unless (and (= l (array-dimension b 0)) (= m (array-dimension b 1)))
      (error "matrices have incompatible dimensions"))
    (let ((result (make-array (list l m) :element-type (array-element-type a))))
      (dotimes (i l)
    (dotimes (j m)
      (setf (aref result i j) (- (aref a i j) (aref b i j)))))
      result)))

(defun scale-matrix (mat scale)
  "  Return a new matrix scaled by the given amount."
  (let ((l (array-dimension mat 0))
    (m (array-dimension mat 1)))
    (let ((result (make-array (list l m) :element-type (array-element-type mat))))
      (dotimes (i l)
    (dotimes (j m)
      (setf (aref result i j) (* (aref mat i j) scale))))
      result)))


(defun transpose (mat)
  "  Returns the transpose of the given matrix"
  (multiple-value-bind (r c) (values-list (array-dimensions mat))
    (let ((arr (make-array (list c r))))
      (dotimes (i r arr)
        (dotimes (j c)
          (setf (aref arr j i) (aref mat i j)))))))


(defun m* (&rest matrices)
  "  Multiplies a list of matrices together, if they have compatible dimensions."
  (reduce #'matmult (cdr matrices) :initial-value (car matrices)))

(defun m+ (&rest matrices)
  "  Adds a list of matrices together, if they have compatible dimensions."
  (reduce #'matplus (cdr matrices) :initial-value (car matrices)))

(defun m- (&rest matrices)
  "  Subtracts a list of matrices from the first, or unary minus if only one given."
  (if (= 1 (length matrices))
      (scale-matrix (car matrices) -1)
      (reduce #'matminus (cdr matrices) :initial-value (car matrices))))

(defun ms (k mat)
  "  Return a new matrix scaled by the given amount."
  (scale-matrix mat k))

(defun mt (mat)
  "  Returns the transpose of the given matrix."
  (transpose (matricize mat)))

(defun determinant3 (a11 a12 a13 a21 a22 a23 a31 a32 a33)
  "Computes the determinant of the 3x3 matrix with elements aij, i=row, j=col."
  (- (+ (* a11 a22 a33) (* a21 a32 a13) (* a31 a12 a23))
     (+ (* a31 a22 a13) (* a21 a12 a33) (* a11 a32 a23))))

(defun determinant2 (a11 a12 a21 a22)
  "Computes the determinant of the 2X2 matrix with elements aij, i=row, j=col."
  (- (* a11 a22) (* a21 a12)))

(defun compute-minors3 (a11 a12 a13 a21 a22 a23 a31 a32 a33)
  "Finds rank2 minors of 3X3 matrix aij.  Returns as a list of
  9 minors (... mij ...)  where mij is minor without rowi, colj."
  (list
    (determinant2 a22 a23 a32 a33)
    (determinant2 a21 a23 a31 a33)
    (determinant2 a21 a22 a31 a32)
    (determinant2 a12 a13 a32 a33)
    (determinant2 a11 a13 a31 a33)
    (determinant2 a11 a12 a31 a32)
    (determinant2 a12 a13 a22 a23)
    (determinant2 a11 a13 a21 a23)
    (determinant2 a11 a12 a21 a22)))

(defun compute-minor3 (matrix row col)
  "  Finds rank2 minor of 3X3 MATRIX (a list of nine elements in row-col order)
  formed by leaving out row ROW and column COL."
  (multiple-value-bind (a11 a12 a13 a21 a22 a23 a31 a32 a33)
      (values-list matrix)
    (ecase row
      (1 (ecase col
       (1 (determinant2 a22 a23 a32 a33))
       (2 (determinant2 a21 a23 a31 a33))
       (3 (determinant2 a21 a22 a31 a32))))
      (2 (ecase col
       (1 (determinant2 a12 a13 a32 a33))
       (2 (determinant2 a11 a13 a31 a33))
       (3 (determinant2 a11 a12 a31 a32))))
      (3 (ecase col
       (1 (determinant2 a12 a13 a22 a23))
       (2 (determinant2 a11 a13 a21 a23))
       (3 (determinant2 a11 a12 a21 a22)))))))

(defun cofactors3 (a11 a12 a13 a21 a22 a23 a31 a32 a33)
  "Finds cofactors of 3X3 matrix aij.  Returns as a list of
  9 cofactors (... cij ...)  where cij is cofactor of rowi, colj."
  (list
    (determinant2 a22 a23 a32 a33)
    (- (determinant2 a21 a23 a31 a33))
    (determinant2 a21 a22 a31 a32)
    (- (determinant2 a12 a13 a32 a33))
    (determinant2 a11 a13 a31 a33)
    (- (determinant2 a11 a12 a31 a32))
    (determinant2 a12 a13 a22 a23)
    (- (determinant2 a11 a13 a21 a23))
    (determinant2 a11 a12 a21 a22)))

(defun cofactors-mat3 (mat &optional (cofmat (make-array '(3 3) :element-type (array-element-type mat))))
  "Finds cofactors of 3X3 matrix MAT, and returns a new matrix where element i,j is cofactor of rowi, colj."
  (multiple-value-bind (a11 a12 a13 a21 a22 a23 a31 a32 a33) (values-list (listarray mat))
    (setf (aref cofmat 0 0) (determinant2 a22 a23 a32 a33))
    (setf (aref cofmat 0 1) (- (determinant2 a21 a23 a31 a33)))
    (setf (aref cofmat 0 2) (determinant2 a21 a22 a31 a32))
    (setf (aref cofmat 1 0) (- (determinant2 a12 a13 a32 a33)))
    (setf (aref cofmat 1 1) (determinant2 a11 a13 a31 a33))
    (setf (aref cofmat 1 2) (- (determinant2 a11 a12 a31 a32)))
    (setf (aref cofmat 2 0) (determinant2 a12 a13 a22 a23))
    (setf (aref cofmat 2 1) (- (determinant2 a11 a13 a21 a23)))
    (setf (aref cofmat 2 2) (determinant2 a11 a12 a21 a22))
    cofmat))

(defun adjoint3 (mat &optional (adj-mat (make-array '(3 3) :element-type (array-element-type mat))))
  "Finds transpose of cofactor matrix of 3X3 matrix MAT, which is the inverse of MAT up to a scale factor"
  (multiple-value-bind (a11 a12 a13 a21 a22 a23 a31 a32 a33) (values-list (listarray mat))
    (setf (aref adj-mat 0 0) (determinant2 a22 a23 a32 a33))
    (setf (aref adj-mat 1 0) (- (determinant2 a21 a23 a31 a33)))
    (setf (aref adj-mat 2 0) (determinant2 a21 a22 a31 a32))
    (setf (aref adj-mat 0 1) (- (determinant2 a12 a13 a32 a33)))
    (setf (aref adj-mat 1 1) (determinant2 a11 a13 a31 a33))
    (setf (aref adj-mat 2 1) (- (determinant2 a11 a12 a31 a32)))
    (setf (aref adj-mat 0 2) (determinant2 a12 a13 a22 a23))
    (setf (aref adj-mat 1 2) (- (determinant2 a11 a13 a21 a23)))
    (setf (aref adj-mat 2 2) (determinant2 a11 a12 a21 a22))
    adj-mat))

(defun inverse3 (mat &optional (inv-mat (make-array '(3 3) :element-type (array-element-type mat))))
  "Returns inverse of the given 3x3 matrix (or nil if singular)."
  (let ((adj (adjoint3 mat inv-mat)))
    (let ((scale (+ (* (aref mat 0 0) (aref adj 0 0))
            (* (aref mat 0 1) (aref adj 1 0))
            (* (aref mat 0 2) (aref adj 2 0)))))
      (if (zerop scale)
      nil
      (ms (/ scale) adj)))))

(defun matrix-with-columns (&rest column-list)
  "Return a matrix having the given columns."
  (let ((mat (make-array (list (length (car column-list)) (length column-list))))
    (numrows (length (car column-list)))
    (col 0))
    (dolist (vec column-list mat)
      (dotimes (row numrows)
    (setf (aref mat row col) (elt vec row)))
      (incf col))))

(defun diag (&rest element-list)
  "Return a diagonal matrix containing the given elements"
  (let ((num (length element-list))
    (i 0))
    (let ((mat (make-array (list num num) :element-type (type-of (car element-list))
               :initial-element (coerce 0 (type-of (car element-list))))))
      (dolist (element element-list mat)
    (setf (aref mat i i) element)
    (incf i)))))

(defun solve-2-by-2 (a11 a12 a21 a22 b1 b2)
  "  Solves 2X2 system of equations
    | a11 a12 |  | X |   | b1 |
    | a21 a22 |  | Y | = | b2 |
  for X and Y and returns them as the list (X Y)."
  (let ((denom (- (* a11 a22) (* a12 a21))))
    (list (/ (- (* b1 a22) (* b2 a12)) denom)
      (/ (- (* b2 a11) (* b1 a21)) denom))))

(defun unit-normal (vector1 vector2)
  "Returns a unit vector perpendicular to the two given vectors"
  (normalize (cross-product vector1 vector2)))

(defun unit-vector-angle (x y z refx refy refz normx normy normz)
  "Returns a angle in radians between [0 and 2pi) representing the angle
  between the two unit vectors <x,y,z> and <refx,refy,refz>.  Norm is a
  unit normal vector perpendicular to the plane containing the two other
  vectors.  The angle returned is measured from the reference vector ref
  to the given vector <x,y,z> counterclockwise around the axis norm.
  Thus an angle r means that to a viewer at the origin whose line of
  sight lies along vector <normx,normy,normz>, the vector <refx,refy,refz>
  would have to be rotated counterclockwise about the origin r radians
  to bring it into correspondence with vector <x,y,z>."
  (let ((dp (dot-product3 refx refy refz x y z))
    (sp (determinant3 normx normy normz refx refy refz x y z)))
    ;;note: dp = dot product of ref and xyz = cos angle between them
    ;;      sp = scalar triple product = norm *dot* (ref *cross* xyz)
    ;;         = sin of the angle between ref and xyz
    (let ((theta (acos (max -1.0 (min 1.0 dp)))))  ;acos returns results in range [0,pi)
      (when (and (plusp sp) (plusp theta)) (setf theta (- (* 2.0 pi) theta)))
      theta)))