floss/stoe
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fix the typed returned by quaternion constructor

Browse source
This commit is contained in:
Renaud Casenave-Péré 2015-12-27 14:19:03 +01:00
parent 0aef2509d5
commit 00fa5fab7a
1 changed files with 20 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

32
maths/quaternion.lisp
View file

@ -23,20 +23,21 @@
(defun from-attribs (x y z w)
(let ((q (make-quaternion x y z w)))
(normalize q)))
(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)))
(normalize (make-quaternion (v* v sin) cos))))
(qnormalize (make-quaternion (v* v sin) cos))))
(defmacro quat (&rest attribs)
(let ((len (length attribs)))
(assert (or (= len 2) (= len 4)))
(assert (or (= len 0) (= len 2) (= len 4)))
(case len
(4 `(from-attribs ,@attribs))
(2 `(from-axis-and-angle ,@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)))
@ -56,12 +57,19 @@
(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)
(normalize (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)))
(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)))