75 lines
2.5 KiB
Common Lisp
75 lines
2.5 KiB
Common Lisp
#|
|
|
This file is a part of stoe project.
|
|
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
|
|#
|
|
|
|
(uiop:define-package :stoe/maths/quaternion
|
|
(:use :cl
|
|
:stoe/maths/types
|
|
:stoe/maths/vector
|
|
:stoe/maths/matrix)
|
|
(:export #:quat #:conjug #:q* #:quat-to-mat3 #:quat-to-mat4)
|
|
(:import-from :stoe/maths/vector
|
|
#:fill-vector))
|
|
(in-package :stoe/maths/quaternion)
|
|
|
|
(defun make-quaternion (&rest attribs)
|
|
(let ((q (make-instance 'quaternion
|
|
:array (make-array '(4) :element-type 'single-float))))
|
|
(loop with i = 0
|
|
for attr in attribs
|
|
do (setf i (fill-vector q attr i)))
|
|
q))
|
|
|
|
(defun from-attribs (x y z w)
|
|
(let ((q (make-quaternion x y z w)))
|
|
(qnormalize q)))
|
|
|
|
(defun from-axis-and-angle (vec angle)
|
|
(let ((v (normalize vec))
|
|
(sin (coerce (sin (/ angle 2)) 'single-float))
|
|
(cos (coerce (cos (/ angle 2)) 'single-float)))
|
|
(qnormalize (make-quaternion (v* v sin) cos))))
|
|
|
|
(defmacro quat (&rest attribs)
|
|
(let ((len (length attribs)))
|
|
(assert (or (= len 0) (= len 2) (= len 4)))
|
|
(case len
|
|
(4 `(from-attribs ,@attribs))
|
|
(2 `(from-axis-and-angle ,@attribs))
|
|
(0 `(from-attribs 0.0 0.0 0.0 1.0)))))
|
|
|
|
(defun conjug (quat)
|
|
(quat (- (x quat)) (- (y quat)) (- (z quat)) (w quat)))
|
|
|
|
(defun quat-to-mat3 (quat)
|
|
(with-swizzle (x y z w) quat
|
|
(let ((2xx (* 2 x x)) (2yy (* 2 y y)) (2zz (* 2 z z))
|
|
(2xy (* 2 x y)) (2xz (* 2 x z)) (2xw (* 2 x w))
|
|
(2yz (* 2 y z)) (2yw (* 2 y w))
|
|
(2zw (* 2 z w)))
|
|
(mat (- 1 2yy 2zz) (- 2xy 2zw) (+ 2xz 2yw)
|
|
(+ 2xy 2zw) (- 1 2xx 2zz) (- 2yz 2xw)
|
|
(- 2xz 2yw) (+ 2yz 2xw) (- 1 2xx 2yy)))))
|
|
|
|
(defun quat-to-mat4 (quat)
|
|
(let ((mat (mat4 (quat-to-mat3 quat))))
|
|
(setf (mref mat 3 3) 1.0)
|
|
mat))
|
|
|
|
(defun qnormalize (q)
|
|
(let ((len (vlength q))
|
|
(quat (make-quaternion 0.0 0.0 0.0 0.0)))
|
|
(loop for i from 0 below 4
|
|
do (setf (vref quat i) (/ (vref q i) len)))
|
|
quat))
|
|
|
|
(defun q* (&rest q-list)
|
|
(qnormalize (reduce (lambda (q1 q2)
|
|
(with-swizzle ((ax x) (ay y) (az z) (aw w)) q1
|
|
(with-swizzle ((bx x) (by y) (bz z) (bw w)) q2
|
|
(quat (- (+ (* aw bx) (* ax bw) (* ay bz)) (* az by))
|
|
(- (+ (* aw by) (* ay bw) (* az bx)) (* ax bz))
|
|
(- (+ (* aw bz) (* az bw) (* ax by)) (* ay bx))
|
|
(- (* aw bw) (* ax bx) (* ay by) (* az bz))))))
|
|
q-list)))
|