vector.lisp

   1 (defpackage :vector
   2   (:use :cl)
   3   (:export
   4    #:vec2 #:make-vec2 #:vec2-x #:vec2-y
   5    #:norm2 #:x #:y #:z
   6    #:vec #:make-vec #:v #:vec-x #:vec-y #:vec-z
   7    #:v. #:v+ #:v- #:v* #:norm #:normalize
   8    #:cross
   9    #:m #:rotation-matrix #:m*
  10    #:vec-i #:make-vec-i #:vec-i-x #:vec-i-y #:vec-i-z
  11    #:v.-i #:v+-i #:v--i #:v*-i #:norm-i))
  12
  13 (in-package :vector)
  14
  15 ;;;; double-float 2 vector
  16 (deftype vec2 ()
  17   `(simple-array double-float (2)))
  18
  19 (defstruct (vec2 (:type (vector double-float)))
  20   (x 0d0) (y 0d0))
  21
  22 (defun norm2 (vec2)
  23   (declare (vec2 vec2)
  24            (values double-float &optional))
  25   (let ((x (vec2-x vec2))
  26         (y (vec2-y vec2)))
  27     (sqrt (+ (* x x) (* y y)))))
  28
  29 ;;;; double-float 3 vector
  30
  31 (deftype vec ()
  32   `(simple-array double-float (3)))
  33
  34 (defstruct (vec (:type (vector double-float)))
  35   (x 0d0) (y 0d0) (z 0d0))
  36
  37 (deftype mat ()
  38   `(simple-array double-float (3 3)))
  39
  40 (defun v (&optional (x 0d0) (y 0d0) (z 0d0))
  41   (declare (double-float x y z)
  42            (values vec &optional))
  43   (make-array 3
  44               :element-type 'double-float
  45               :initial-contents (list x y z)))
  46
  47 (defun v. (a b)
  48   "Dot product between two vectors."
  49   (declare (vec a b)
  50            (values double-float &optional))
  51   (let ((sum 0d0))
  52     (declare (double-float sum))
  53     (dotimes (i 3)
  54       (incf sum (* (aref a i)
  55                    (aref b i))))
  56     sum))
  57 (defun cross (a b)
  58   (declare (vec a b)
  59            (values vec &optional))
  60   (v (- (* (aref a 1) (aref b 2))
  61         (* (aref a 2) (aref b 1)))
  62      (- (* (aref a 2) (aref b 0))
  63         (* (aref a 0) (aref b 2)))
  64      (- (* (aref a 0) (aref b 1))
  65         (* (aref a 1) (aref b 0)))))
  66 #+nil
  67 (cross (v 1d0)
  68        (v 0d0 1d0))
  69
  70 (declaim (ftype (function (vec vec)
  71                           (values vec &optional))
  72                 v- v+))
  73 (defmacro v-op (op a b)
  74   "Subtracting and adding vectors."
  75   `(let ((result (v)))
  76      (dotimes (i 3)
  77        (setf (aref result i) (,op (aref ,a i)
  78                                   (aref ,b i))))
  79      result))
  80
  81 (defun v+ (a b)
  82   "Add two vectors."
  83   (v-op + a b))
  84
  85 (defun v- (a b)
  86   "Subtract two vectors."
  87   (v-op - a b))
  88
  89 #+nil (defun  v* (a scalar)
  90   "Multiply vector with scalar."
  91   (declare (double-float scalar)
  92            (vec a)
  93            (values vec &optional))
  94   (let* ((result (v)))
  95     (dotimes (i 3)
  96       (setf (aref result i) (* scalar (aref a i))))
  97     result))
  98
  99 (defun  %v* (a scalar)
 100   "Multiply vector with scalar."
 101   (declare (double-float scalar)
 102            (vec a)
 103            (values vec &optional))
 104   (let* ((result (v)))
 105     (dotimes (i 3)
 106       (setf (aref result i) (* scalar (aref a i))))
 107     result))
 108
 109 (defmacro v* (a scalar)
 110   (if (numberp scalar)
 111       `(%v* ,a (load-time-value (* 1d0 ,scalar)))
 112       `(%v* ,a ,scalar)))
 113 #+nil
 114 (v* (v 1d0) 4)
 115
 116
 117
 118 (declaim (ftype (function (vec)
 119                           (values double-float &optional))
 120                 norm))
 121 (defun norm (a)
 122   "Length of a vector."
 123   (let ((l2 (v. a a)))
 124     (declare (type (double-float 0d0) l2)) ;; Otherwise warning with complex-double
 125     (sqrt l2)))
 126
 127
 128 (declaim (ftype (function (vec)
 129                           (values vec &optional))
 130                 normalize))
 131 (defun normalize (a)
 132   "Rescale vector to unit length."
 133   (let ((len (norm a)))
 134     (if (eq len 0d0)
 135         (v 0d0 0d0 1d0)
 136         (v* a (/ len)))))
 137
 138
 139 ;;;; 3x3 Matrix
 140
 141 (declaim (ftype (function (double-float double-float double-float
 142                                         double-float double-float double-float
 143                                         double-float double-float double-float)
 144                           (values mat &optional))
 145                 m))
 146 (defun m (a b c d e f g h i)
 147   (make-array '(3 3)
 148               :element-type 'double-float
 149               :initial-contents (list (list a b c) (list d e f) (list g h i))))
 150
 151 (defun rotation-matrix (angle vect)
 152   "Create matrix that rotates by ANGLE radians around the direction
 153  VECT. VECT must be normalized."
 154   (declare ((double-float 0d0 #.(* 2d0 pi)) angle)
 155            (vec vect)
 156            (values mat &optional))
 157   (let* ((u (aref vect 0))
 158          (v (aref vect 1))
 159          (w (aref vect 2))
 160          (c (cos angle))
 161          (s (sin angle))
 162          (1-c (- 1 c))
 163          (su (* s u))
 164          (sv (* s v))
 165          (sw (* s w)))
 166     (m (+ c (* 1-c u u))
 167        (+ (* 1-c u v) sw)
 168        (- (* 1-c u w) sv)
 169
 170        (- (* 1-c u v) sw)
 171        (+ c (* 1-c v v))
 172        (+ (* 1-c v w) su)
 173
 174        (+ (* 1-c u w) sv)
 175        (- (* 1-c v w) su)
 176        (+ c (* 1-c w w)))))
 177 #+nil
 178 (rotation-matrix (/ pi 2) (v 0d0 0d0 1d0))
 179
 180 (defun m* (matrix vect)
 181   "Multiply MATRIX with VECT. Copies 4th component w from VECT into
 182 result."
 183   (declare (mat matrix)
 184            (vec vect)
 185            (values vec &optional))
 186   (let ((res (v)))
 187     (dotimes (i 3)
 188       (dotimes (j 3)
 189         (incf (aref res i)
 190               (* (aref matrix i j) (aref vect j)))))
 191     res))
 192 #+nil
 193 (m* (rotation-matrix (/ pi 2) (v 0d0 0d0 1d0)) (v 1d0))
 194
 195
 196
 197
 198 ;;;; Integer vectors
 199
 200
 201 (deftype vec-i ()
 202   `(simple-array fixnum (3)))
 203
 204 (defstruct (vec-i (:type (vector fixnum)))
 205   (x 0) (y 0) (z 0))
 206
 207 (defun v.-i (a b)
 208   (declare (vec-i a b)
 209            (values fixnum &optional))
 210   (+ (* (vec-i-x a) (vec-i-x b))
 211      (* (vec-i-y a) (vec-i-y b))
 212      (* (vec-i-z a) (vec-i-z b))))
 213
 214 (declaim (ftype (function (vec-i vec-i)
 215                           (values vec-i &optional))
 216                 v--i v+-i))
 217 (defun v--i (a b)
 218   (make-vec-i :x (- (vec-i-x a) (vec-i-x b))
 219               :y (- (vec-i-y a) (vec-i-y b))
 220               :z (- (vec-i-z a) (vec-i-z b))))
 221 (defun v+-i (a b)
 222   (make-vec-i :x (+ (vec-i-x a) (vec-i-x b))
 223               :y (+ (vec-i-y a) (vec-i-y b))
 224               :z (+ (vec-i-z a) (vec-i-z b))))
 225
 226 (defun norm-i (a)
 227   (declare (vec-i a)
 228            (values double-float &optional))
 229   (sqrt (* 1d0 (v.-i a a))))
 230
 231 (defun  v*-i (a scalar)
 232   "Multiply vector with scalar."
 233   (declare (fixnum scalar)
 234            (vec-i a)
 235            (values vec-i &optional))
 236   (let* ((result (make-vec-i)))
 237     (dotimes (i 3)
 238       (setf (aref result i) (* scalar (aref a i))))
 239     result))