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