quaternion.lisp

   1
   2 (in-package :cl-tuples)
   3
   4 (def-tuple-type quaternion
   5         :tuple-element-type fast-float
   6         :initial-element 0.0f0
   7         :elements (x y z w))
   8
   9 (export-tuple-operations quaternion)
  10
  11 (def-tuple-type angle-axis
  12         :tuple-element-type fast-float
  13         :initial-element 0.0f0
  14         :elements (x y z a))
  15
  16 (export-tuple-operations angle-axis)
  17
  18 ;; need conjugate, angle-axis conversion, slerp
  19
  20 (def-tuple-op quaternion-conjugate*
  21         ((q quaternion (x y z w)))
  22   (quaternion-values* (- x) (- y) (- z) w))
  23
  24 (def-tuple-op quaternion-scale*
  25         ((q quaternion (x y z w))
  26          (s single-float))
  27   "Multiply a quat by a scalar"
  28   (:return quaternion
  29                    (quaternion-values*
  30                         (* s x) (* s y) (* s z) (* s  w))))
  31
  32 (def-tuple-op quaternion-mag-square*
  33     ((q quaternion (x y z w)))
  34   (:return single-float
  35            (+ (* x x) (* y y) (* z z) (* w w))))
  36
  37 (def-tuple-op quaternion-mag*
  38     ((q quaternion (x y z w)))
  39   (:return single-float
  40            (sqrt (quaternion-mag-square* q))))
  41
  42 (def-tuple-op quaternion-normalize*
  43     ((q quaternion (x y z w)))
  44   "Ensure a quaternion is a unit"
  45   (:return quaternion
  46            (quaternion-scale*
  47             (quaternion-values* x y z w)
  48             (/ 1f0 (quaternion-mag*
  49                     (quaternion-values* x y z w))))))
  50
  51 (def-tuple-op quaternion-sum*
  52         ((q0 quaternion (x0 y0 z0 w0))
  53          (q1 quaternion (x1 y1 z1 w1)))
  54   "Sum the components of two quaternions"
  55   (:return quaternion
  56                    (quaternion-values* (+ x0 x1) (+ y0 y1) (+ z0 z1) (+ w0 w1))))
  57
  58 (def-tuple-op quaternion-dot*
  59         ((quaternion-lhs quaternion (x0 y0 z0 w0))
  60          (quaternion-rhs quaternion (x1 y1 z1 w1)))
  61   "Dot product of two quaternions."
  62   (:return single-float
  63                    (+ (* x0 x1) (* y0 y1) (* z0 z1) (* w0 w1))))
  64
  65 (def-tuple-op quaternion-inverse*
  66         ((q quaternion (x y z w)))
  67   "Inverse of quaternion"
  68   (:return quaternion
  69                    (let* ((mag
  70                                    (the single-float
  71                                          (sqrt (+ (* x x) (* y y) (* z z) (* w w)))))
  72                                   (1/mag2
  73                                    (the single-float (/ 1.0 (* mag mag)))))
  74                          (declare (single-float 1/mag2))
  75                          (quaternion-values* (* x 1/mag2) (* y 1/mag2) (* z 1/mag2) (* w 1/mag2)))))
  76
  77
  78 (def-tuple-op quaternion-product*
  79         ((q-lhs quaternion (x1 y1 z1 w1))
  80          (q-rhs quaternion (x2 y2 z2 w2)))
  81   "Multiple of two quaternions"
  82   (:return quaternion
  83                    (quaternion-values* (- (+ (* w1 x2) (* x1 w2) (* y1 z2)) (* z1 y2))
  84                                                            (- (+ (* w1 y2) (* y1 w2) (* z1 x2)) (* x1 z2))
  85                                                            (- (+ (* w1 z2) (* x1 y2) (* z1 w2)) (* y1 x2))
  86                                                            (- (* w1 w2) (* x1 x2) (* y1 y2) (* z1 z2)))))
  87
  88 (def-tuple-op quaternion-matrix33*
  89         ((q quaternion (x y z w)))
  90   "Convert a quaternion to a 3x3 rotation matrix."
  91   (:return matrix33
  92                    (matrix33-values*
  93                         (- 1 (* 2 y y) (* 2 z z))   (- (* 2 x y) (* 2 w z))   (+ (* 2 x z) (* 2 w y))
  94                         (+   (* 2 x y) (* 2 w z))   (- 1 (* 2 x x) (* 2 z z)) (- (* 2 y z) (* 2 w x))
  95                         (-   (* 2 x z) (* 2 w y))   (+  (* 2 y z) (* 2 w x))  (- 1 (* 2 x x) (* 2 y y)))))
  96
  97 (def-tuple-op angle-axis-quaternion*
  98     ((aa angle-axis (x y z a)))
  99   "Convert an angle-axis tuple to a quaternion tuple"
 100   (:return quaternion
 101            (let* ((a/2 (* 0.5 a))
 102                   (sin-angle (sin a/2)))
 103              (quaternion-values*
 104               (* x sin-angle)
 105               (* y sin-angle)
 106               (* z sin-angle)
 107               (cos a/2)))))
 108
 109 (def-tuple-op quaternion-transform-vector3d*
 110     ((vector vector3d (vx vy vz))
 111      (quat quaternion (qx qy qz qw)))
 112   "Transform a 3d vector with a quaternion"
 113   (:return vector3d
 114            (with-quaternion*
 115                (quaternion-product*
 116                 (quaternion-product*
 117                  quat
 118                  (vector3d-quaternion* vector))
 119                 (quaternion-conjugate* quat))
 120                (rx ry rz rw)
 121              (vector3d-values* rx ry rz))))
 122
 123 (def-tuple-op vector3d-quaternion*
 124     ((vector vector3d (vx vy vz)))
 125   "Convert a 3d vector into q auqt for angular velocity purposes"
 126   (:return quaternion
 127            (quaternion-values* vx vy vz 0.0)))