diff options
| -rw-r--r-- | chickadee/math/matrix.scm | 36 |
1 files changed, 21 insertions, 15 deletions
diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm index dfc3cc1..6931a2c 100644 --- a/chickadee/math/matrix.scm +++ b/chickadee/math/matrix.scm @@ -589,22 +589,28 @@ clipping plane NEAR and FAR." (define (matrix4-rotate! matrix q) "Return a new rotation matrix for the quaternion Q." - (let ((x (quaternion-x q)) - (y (quaternion-y q)) - (z (quaternion-z q)) - (w (quaternion-w q))) + ;; Math based on this StackOverflow answer: + ;; https://stackoverflow.com/a/1556470 + ;; + ;; sqrt elimination thanks to this comment on the above answer: + ;; https://stackoverflow.com/questions/1556260/convert-quaternion-rotation-to-rotation-matrix#comment74466994_1556470 + (let* ((x (quaternion-x q)) + (y (quaternion-y q)) + (z (quaternion-z q)) + (w (quaternion-w q)) + (n (/ 2.0 (+ (* x x) (* y y) (* z z) (* w w))))) (init-matrix4 matrix - (- 1.0 (* 2.0 (* y y)) (* 2.0 (* z z))) - (- (* 2.0 x y) (* 2.0 w z)) - (+ (* 2.0 x z) (* 2.0 w y)) - 0 - (+ (* 2.0 x y) (* 2.0 w z)) - (- 1.0 (* 2.0 (* x x)) (* 2.0 (* z z))) - (- (* 2.0 y z) (* 2.0 w x)) - 0 - (- (* 2.0 x z) (* 2.0 w y)) - (+ (* 2.0 y z) (* 2.0 w x)) - (- 1.0 (* 2.0 (* x x)) (* 2.0 (* y y))) + (- 1.0 (* n y y) (* n z z)) + (- (* n x y) (* n z w)) + (+ (* n x z) (* n y w)) + 0.0 + (+ (* n x y) (* n z w)) + (- 1.0 (* n x x) (* n z z)) + (- (* n y z) (* n x w)) + 0.0 + (- (* n x z) (* n y w)) + (+ (* n y z) (* n x w)) + (- 1.0 (* n x x) (* n y y)) 0.0 0.0 0.0 |