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