vector.lisp

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