quaternion.lisp

   1 (in-package :cl-tuples)
   2
   3 (def-tuple-type quaternion
   4   :tuple-element-type fast-float
   5   :initial-element 0.0f0
   6   :elements (x y z w))
   7
   8 (export-tuple-operations quaternion)
   9
  10 (def-tuple-type angle-axis
  11   :tuple-element-type fast-float
  12   :initial-element 0.0f0
  13   :elements (x y z a))
  14
  15 (export-tuple-operations angle-axis)
  16
  17 (def-tuple-op angle-axis-matrix33*
  18     ((aa angle-axis (x y z a)))
  19   (:return matrix33
  20            (matrix33-map*
  21             (+)
  22             (identity-matrix33*)
  23             (matrix33-scale*
  24              (sin a)
  25              #1=(matrix33-values*
  26                 0.0f0 (- z) y
  27                 z 0.0f0 (- x)
  28                 (- y) x 0.0f0))
  29             (matrix33-scale*
  30              (- 1 (cos a))
  31              (matrix33-product*
  32               #1# #1#)))))
  33
  34 (def-tuple-op matrix33-trace*
  35     ((m matrix33 #.(tuple-elements 'matrix33)))
  36   (:return fast-float
  37            (+ e00 e11 e22)))
  38
  39 (def-tuple-op vector3d-angle-axis*
  40     ((axis vector3d (x y z))
  41      (angle fast-float))
  42   (:return angle-axis
  43            (angle-axis-values* x y z angle)))
  44
  45 ;; TODO: the matrix has to be a rotation matrix, but then we can also
  46 ;; restrict the values to their respective ranges, which eliminates the
  47 ;; warning about the empty COND case and yields better compilation
  48 ;; results.  the question is:  are these correct?
  49 (def-tuple-op matrix33-angle-axis*
  50     ((m matrix33 #.(tuple-elements 'matrix33)))
  51   (:return angle-axis
  52            (let* ((trace (matrix33-trace* m))
  53                   (angle (acos (/ (1- trace) 2))))
  54              (declare (type (single-float -1.0f0 3.0f0) trace)
  55                       (type (single-float 0.0f0 #.fast-pi)))
  56              (cond
  57                ((= angle 0.0f0)
  58                 (angle-axis-key-values))
  59                ((< 0.0f0 angle fast-pi)
  60                 (vector3d-angle-axis*
  61                  (vector3d-normal*
  62                   (vector3d-values*
  63                    (- e21 e12)
  64                    (- e02 e20)
  65                    (- e10 e01)))
  66                  angle))
  67                (T                       ; (= angle #1#)
  68                 (vector3d-angle-axis*
  69                  (flet ((u0 ()
  70                           (let* ((u0 (/ (the single-float (sqrt (1+ (- e00 e11 e22)))) 2))
  71                                  (2u0 (* 2 u0)))
  72                             (vector3d-values* u0 (/ e01 2u0) (/ e02 2u0))))
  73                         (u1 ()
  74                           (let* ((u1 (/ (the single-float (sqrt (1+ (- e11 e00 e22)))) 2))
  75                                  (2u1 (* 2 u1)))
  76                             (vector3d-values* (/ e01 2u1) u1 (/ e12 2u1))))
  77                         (u2 ()
  78                           (let* ((u2 (/ (the single-float (sqrt (1+ (- e22 e00 e11)))) 2))
  79                                  (2u2 (* 2 u2)))
  80                             (vector3d-values* (/ e02 2u2) (/ e12 2u2) u2))))
  81                    (if (> e00 e11)
  82                        (if (> e00 e22)
  83                            (u0)
  84                            (if (> e22 e11)
  85                                (u2)
  86                                (u1)))
  87                        (if (> e22 e11)
  88                            (u2)
  89                            (u1))))
  90                  angle))))))
  91
  92 ;; need conjugate, angle-axis conversion, slerp
  93
  94 (def-tuple-op quaternion-conjugate*
  95     ((q quaternion (x y z w)))
  96   (quaternion-values* (- x) (- y) (- z) w))
  97
  98 (def-tuple-op quaternion-scale*
  99     ((q quaternion (x y z w))
 100      (s fast-float))
 101   "Multiply a quat by a scalar"
 102   (:return quaternion
 103            (quaternion-map*
 104             (*) (quaternion-key-values :initial-element s) q)))
 105
 106 (def-tuple-op quaternion-dot*
 107     ((q0 quaternion)
 108      (q1 quaternion))
 109   "Dot product of two quaternions."
 110   (:return fast-float
 111            (quaternion-reduce*
 112             (+) (quaternion-map* (*) q0 q1))))
 113
 114 (def-tuple-op quaternion-mag-square*
 115     ((q quaternion))
 116   (:return fast-float
 117            (quaternion-dot* q q)))
 118
 119 (def-tuple-op quaternion-mag*
 120     ((q quaternion))
 121   (:return fast-float
 122            (sqrt (quaternion-mag-square* q))))
 123
 124 (def-tuple-op quaternion-normalize*
 125     ((q quaternion))
 126   "Ensure a quaternion is a unit"
 127   (:return quaternion
 128            (quaternion-scale*
 129             q (/ (quaternion-mag* q)))))
 130
 131 (def-tuple-op quaternion-sum*
 132     ((q0 quaternion)
 133      (q1 quaternion))
 134   "Sum the components of two quaternions"
 135   (:return quaternion
 136            (quaternion-map* (+) q0 q1)))
 137
 138 (def-tuple-op quaternion-inverse*
 139     ((q quaternion))
 140   "Inverse of quaternion"
 141   (:return quaternion
 142            (quaternion-scale*
 143             q (/ (quaternion-mag-square* q)))))
 144
 145 (def-tuple-op quaternion-power*
 146     ((q quaternion (x y z w))
 147      (times fast-float))
 148   (:return quaternion
 149            (let* ((angle (acos w))
 150                   (sin (sin angle)))
 151              (if (= angle 0)
 152                  q
 153                  (let ((factor (* angle times (/ sin))))
 154                    (quaternion-values*
 155                     (sin (* x factor))
 156                     (sin (* y factor))
 157                     (sin (* z factor))
 158                     (cos (* times angle))))))))
 159
 160 (def-tuple-op quaternion-product*
 161         ((q-lhs quaternion (x1 y1 z1 w1))
 162          (q-rhs quaternion (x2 y2 z2 w2)))
 163   "Multiple of two quaternions"
 164   (:return quaternion
 165                    (quaternion-values* (- (+ (* w1 x2) (* x1 w2) (* y1 z2)) (* z1 y2))
 166                                                            (- (+ (* w1 y2) (* y1 w2) (* z1 x2)) (* x1 z2))
 167                                                            (- (+ (* w1 z2) (* x1 y2) (* z1 w2)) (* y1 x2))
 168                                                            (- (* w1 w2) (* x1 x2) (* y1 y2) (* z1 z2)))))
 169
 170 (def-tuple-op quaternion-matrix33*
 171         ((q quaternion (x y z w)))
 172   "Convert a quaternion to a 3x3 rotation matrix."
 173   (:return matrix33
 174                    (matrix33-values*
 175                         (- 1 (* 2 y y) (* 2 z z))   (- (* 2 x y) (* 2 w z))   (+ (* 2 x z) (* 2 w y))
 176                         (+   (* 2 x y) (* 2 w z))   (- 1 (* 2 x x) (* 2 z z)) (- (* 2 y z) (* 2 w x))
 177                         (-   (* 2 x z) (* 2 w y))   (+  (* 2 y z) (* 2 w x))  (- 1 (* 2 x x) (* 2 y y)))))
 178
 179 (def-tuple-op matrix33-quaternion*
 180     ((m matrix33 :default))
 181   (:return quaternion
 182            (let ((trace (matrix33-trace* m)))
 183              (if (> trace 0)
 184                  (let* ((w (/ (sqrt (1+ trace)) 2))
 185                         (4w (* 4 w)))
 186                    (quaternion-values*
 187                     (/ (- e21 e12) 4w)
 188                     (/ (- e20 e02) 4w)
 189                     (/ (- e01 e10) 4w)
 190                     w))
 191                  (flet ((e00 ()
 192                           (let* ((x (/ (sqrt (1+ (- e00 e11 e22))) 2))
 193                                  (4x (* 4 x)))
 194                             (quaternion-values*
 195                              x
 196                              (/ (+ e01 e10) 4x)
 197                              (/ (+ e02 e20) 4x)
 198                              (/ (- e21 e21) 4x))))
 199                         (e11 ()
 200                           (let* ((y (/ (sqrt (1+ (- e11 e00 e22))) 2))
 201                                   (4y (* 4 y)))
 202                              (quaternion-values*
 203                               (/ (- e01 e10) 4y)
 204                               y
 205                               (/ (+ e12 e21) 4y)
 206                               (/ (- e20 e02) 4y))))
 207                         (e22 ()
 208                           (let* ((z (/ (sqrt (1+ (- e22 e00 e11))) 2))
 209                                  (4z (* 4 z)))
 210                             (quaternion-values*
 211                              (/ (+ e02 e20) 4z)
 212                              (/ (+ e12 e21) 4z)
 213                              z
 214                              (/ (- e01 e10) 4z)))))
 215                    (if (> e00 e11)
 216                        (if (> e00 e22)
 217                            (e00)
 218                            (if (> e22 e11)
 219                                (e22)
 220                                (e11)))
 221                        (if (> e22 e11)
 222                            (e22)
 223                            (e11))))))))
 224
 225 (def-tuple-op angle-axis-quaternion*
 226     ((aa angle-axis (x y z a)))
 227   "Convert an angle-axis tuple to a quaternion tuple"
 228   (:return quaternion
 229            (let* ((a/2 (* 0.5f0 a))
 230                   (sin-angle (sin a/2)))
 231              (quaternion-values*
 232               (* x sin-angle)
 233               (* y sin-angle)
 234               (* z sin-angle)
 235               (cos a/2)))))
 236
 237 (def-tuple-op quaternion-angle-axis*
 238     ((q quaternion (x y z w)))
 239   "Convert an quaternion tuple to an angle-axis tuple.  If the angle is
 240 zero, the axis isn't defined, so the unit x vector is returned instead."
 241   (:return angle-axis
 242            ;; either test for one, disable traps, or catch the exception
 243            (if (or (= w 1.0f0) (= w -1.0f0))
 244                (angle-axis-key-values x 1.0f0)
 245                (let ((angle (* 2 (acos w))))
 246                  (vector3d-angle-axis*
 247                   (vector3d-scale*
 248                    (vector3d-values* x y z)
 249                    (/ (sqrt (- 1 (* w w)))))
 250                   angle)))))
 251
 252 (def-tuple-op quaternion-transform-vector3d*
 253     ((vector vector3d (vx vy vz))
 254      (quat quaternion (qx qy qz qw)))
 255   "Transform a 3d vector with a quaternion"
 256   (:return vector3d
 257            (with-quaternion*
 258                (quaternion-product*
 259                 (quaternion-product*
 260                  quat
 261                  (vector3d-quaternion* vector))
 262                 (quaternion-conjugate* quat))
 263                (rx ry rz rw)
 264              (vector3d-values* rx ry rz))))
 265
 266 (def-tuple-op vector3d-quaternion*
 267     ((vector vector3d (vx vy vz)))
 268   "Convert a 3d vector into q auqt for angular velocity purposes"
 269   (:return quaternion
 270            (quaternion-values* vx vy vz 0.0f0)))