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-norm*
  33         ((q quaternion (x y z w)))
  34   (:return single-float
  35                    (sqrt (+ (* x x) (* y y) (* z z) (* w w)))))
  36
  37
  38
  39 (def-tuple-op quaternion-unitize*
  40         ((q quaternion (x y z w)))
  41   "Ensure a quaternion is a unit"
  42   (:return quaternion
  43                    (let ((1/mag
  44                                   (the single-float
  45                                         (/ 1.0 (sqrt (+ (* x x) (* y y) (* z z) (* w w)))))))
  46                          (quaternion-values* (* x 1/mag) (* y 1/mag) (* z 1/mag) (* w 1/mag)))))
  47
  48 (def-tuple-op quaternion-sum*
  49         ((q0 quaternion (x0 y0 z0 w0))
  50          (q1 quaternion (x1 y1 z1 w1)))
  51   "Sum the components of two quaternions"
  52   (:return quaternion
  53                    (quaternion-values* (+ x0 x1) (+ y0 y1) (+ z0 z1) (+ w0 w1))))
  54
  55 (def-tuple-op quaternion-dot*
  56         ((quaternion-lhs quaternion (x0 y0 z0 w0))
  57          (quaternion-rhs quaternion (x1 y1 z1 w1)))
  58   "Dot product of two quaternions."
  59   (:return single-float
  60                    (+ (* x0 x1) (* y0 y1) (* z0 z1) (* w0 w1))))
  61
  62 (def-tuple-op quaternion-inverse*
  63         ((q quarternion (x y z w)))
  64   "Inverse of quaternion"
  65   (:return quaternion
  66                    (let* ((mag
  67                                    (the single-float
  68                                          (sqrt (+ (* x x) (* y y) (* z z) (* w w)))))
  69                                   (1/mag2
  70                                    (the single-float (/ 1.0 (* mag mag)))))
  71                          (declare (single-float mag2))
  72                          (quaternion-values* (/ x mag2) (/ y mag2) (/ z mag2) (/ w mag2)))))
  73
  74
  75 (def-tuple-op quaternion-product*
  76         ((q-lhs quaternion (x1 y1 z1 w1))
  77          (q-rhs quaternion (x2 y2 z2 w2)))
  78   "Multiple of two quaternions"
  79   (:return quaternion
  80                    (quaternion-values* (- (+ (* w1 x2) (* x1 w2) (* y1 z2)) (* z1 y2))
  81                                                            (- (+ (* w1 y2) (* y1 w2) (* z1 x2)) (* x1 z2))
  82                                                            (- (+ (* w1 z2) (* x1 y2) (* z1 w2)) (* y1 x2))
  83                                                            (- (* w1 w2) (* x1 x2) (* y1 y2) (* z1 z2)))))
  84
  85 (def-tuple-op quaternion-matrix33*
  86         ((q quaternion (x y z w)))
  87   "Convert a quaternion to a 3x3 rotation matrix."
  88   (:return matrix33
  89                    (matrix33-values*
  90                         (- 1 (* 2 y y) (* 2 z z))   (- (* 2 x y) (* 2 w z))   (+ (* 2 x z) (* 2 w y))
  91                         (+   (* 2 x y) (* 2 w z))   (- 1 (* 2 x x) (* 2 z z)) (- (* 2 y z) (* 2 w x))
  92                         (-   (* 2 x z) (* 2 w y))   (+  (* 2 y z) (* 2 w x))  (- 1 (* 2 x x) (* 2 y y)))))
  93
  94
  95 (def-tuple-op angle-axis-quaternion*
  96         ((aa angle-axis (x y z a)))
  97   "Convert an angle-axis tuple to a quaternion tuple"
  98   (:return quaternion
  99                    (quaternion-values* (* x (sin (* 0.5 a)))
 100                                                            (* y (sin (* 0.5 a)))
 101                                                            (* z (sin (* 0.5 a)))
 102                                                            (cos (* 0.5 a)))))
 103
 104
 105
 106 (def-tuple-op quaternion-transform-vector3d*
 107         ((vector vector3d (vx vy vz))
 108          (quat quaternion (qx qy qz qw)))
 109   "Transform a 3d vector with a quaternion"
 110   (:return vector3d
 111                    (with-quaternion*
 112                            (quaternion-product*
 113                                 (quaternion-product*
 114                                  (quaternion-values* qx qy qz qw)
 115                                  (quaternion-values* vx vy vz 0.0))
 116                                 (quaternion-conjugate (quaternion-values* qx qy qz qw)))
 117                            (rx ry rz rw)
 118                          (vector3d-values* rx ry rz))))
 119
 120
 121 (def-tuple-op vector3d-quaternion*
 122         ((vector vector3d (vx vy vz)))
 123   "Convert a 3d vector into q auqt for angular velocity purposes"
 124   (:return quaternion
 125                    (quaternion-values* vx vy vz 0.0)))
 126
 127
 128