summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDavid Thompson <dthompson2@worcester.edu>2014-08-28 18:59:11 -0400
committerDavid Thompson <dthompson2@worcester.edu>2014-08-28 18:59:11 -0400
commitd4774ab5c12fcb5f2358fbc7b2fa162df86a8486 (patch)
tree27721a25d7cfd702dcdd31fc4235c964038886f2
parent7e737414099825359fe66fa6bade3bd62df29cdb (diff)
Add quaternion module.
* Makefile.am (SOURCES): Add 'sly/quaternion.scm'. * sly/quaternion.scm: New file.
-rw-r--r--Makefile.am1
-rw-r--r--sly/quaternion.scm131
2 files changed, 132 insertions, 0 deletions
diff --git a/Makefile.am b/Makefile.am
index 8b9c75c..14adfa5 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -38,6 +38,7 @@ SOURCES = \
sly/math.scm \
sly/mesh.scm \
sly/mouse.scm \
+ sly/quaternion.scm \
sly/rect.scm \
sly/repl.scm \
sly/scene.scm \
diff --git a/sly/quaternion.scm b/sly/quaternion.scm
new file mode 100644
index 0000000..68dd660
--- /dev/null
+++ b/sly/quaternion.scm
@@ -0,0 +1,131 @@
+;;; Sly
+;;; Copyright (C) 2013, 2014 David Thompson <dthompson2@worcester.edu>
+;;;
+;;; This program is free software: you can redistribute it and/or
+;;; modify it under the terms of the GNU General Public License as
+;;; published by the Free Software Foundation, either version 3 of the
+;;; License, or (at your option) any later version.
+;;;
+;;; This program is distributed in the hope that it will be useful,
+;;; but WITHOUT ANY WARRANTY; without even the implied warranty of
+;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+;;; General Public License for more details.
+;;;
+;;; You should have received a copy of the GNU General Public License
+;;; along with this program. If not, see
+;;; <http://www.gnu.org/licenses/>.
+
+;;; Commentary:
+;;
+;; Useful representation of 3D rotations.
+;;
+;;; Code:
+
+(define-module (sly quaternion)
+ #:use-module (ice-9 match)
+ #:use-module (srfi srfi-1)
+ #:use-module (srfi srfi-9)
+ #:use-module (sly math)
+ #:use-module (sly transform)
+ #:use-module (sly vector)
+ #:export (make-quaternion
+ quaternion?
+ quaternion-w quaternion-x quaternion-y quaternion-z
+ identity-quaternion null-quaternion
+ quaternion* quaternion-slerp
+ quaternion-magnitude quaternion-normalize
+ vector->quaternion axis-angle->quaternion
+ quaternion->vector quaternion->transform))
+
+(define-record-type <quaternion>
+ (make-quaternion w x y z)
+ quaternion?
+ (w quaternion-w)
+ (x quaternion-x)
+ (y quaternion-y)
+ (z quaternion-z))
+
+(define identity-quaternion (make-quaternion 1 0 0 0))
+(define null-quaternion (make-quaternion 0 0 0 0))
+
+(define (quaternion* . quaternions)
+ "Return the product of all QUATERNIONS. If called without
+arguments, 'identity-quaternion' is returned."
+ (reduce (lambda args
+ (match args
+ ((($ <quaternion> w1 x1 y1 z1) ($ <quaternion> w2 x2 y2 z2))
+ (make-quaternion
+ (- (* w1 w2) (* x1 x2) (* y1 y2) (* z1 z2))
+ (+ (* w1 x2) (* x1 w2) (* y1 z2) (- (* z1 y2)))
+ (+ (* w1 y2) (* y1 w2) (* z1 x2) (- (* x1 z2)))
+ (+ (* w1 z2) (* z1 w2) (* x1 y2) (- (* y1 x2)))))))
+ identity-quaternion
+ quaternions))
+
+(define (quaternion-slerp q1 q2 delta)
+ "Perform a spherical linear interpolation of the quaternions Q1 and
+Q2 and blending factor DELTA."
+ (let* ((q1 (quaternion->vector q1))
+ (q2 (quaternion->vector q2))
+ (dot (clamp -1 1 (vdot q1 q2)))
+ (theta (* (acos dot) delta))
+ (q3 (normalize (v- q2 (v* q1 dot)))))
+ (vector->quaternion
+ (v+ (v* q1 (cos theta)) (v* q3 (sin theta))))))
+
+(define (quaternion-magnitude q)
+ "Return the magnitude of the quaternion Q."
+ (sqrt
+ (+ (square (quaternion-w q))
+ (square (quaternion-x q))
+ (square (quaternion-y q))
+ (square (quaternion-z q)))))
+
+(define (quaternion-normalize q)
+ "Return the normalized form of the quaternion Q."
+ (let ((m (quaternion-magnitude q)))
+ (if (zero? m)
+ (make-quaternion 0 0 0 0)
+ (make-quaternion (/ (quaternion-w q) m)
+ (/ (quaternion-x q) m)
+ (/ (quaternion-y q) m)
+ (/ (quaternion-z q) m)))))
+
+(define (axis-angle->quaternion axis theta)
+ "Convert the rotation of THETA radians about AXIS to a quaternion.
+AXIS must be a 3D vector."
+ (let* ((cos (cos (/ theta 2)))
+ (sin (sin (/ theta 2))))
+ (match axis
+ (#(x y z)
+ (make-quaternion cos (* x sin) (* y sin) (* z sin))))))
+
+(define (quaternion->vector q)
+ "Convert the quaternion Q into a 4D vector."
+ (match q
+ (($ <quaternion> w x y z)
+ (vector w x y z))))
+
+(define (vector->quaternion v)
+ "Convert the 4D vector V into a quaternion."
+ (match v
+ (#(w x y z)
+ (make-quaternion w x y z))))
+
+(define (quaternion->transform q)
+ "Convert the quaternion Q into a 4x4 transformation matrix."
+ (match q
+ (($ <quaternion> w x y z)
+ (make-transform
+ (- 1 (* 2 (square y)) (* 2 (square z)))
+ (- (* 2 x y) (* 2 w z))
+ (+ (* 2 x z) (* 2 w y))
+ 0
+ (+ (* 2 x y) (* 2 w z))
+ (- 1 (* 2 (square x)) (* 2 (square z)))
+ (- (* 2 y z) (* 2 w x))
+ 0
+ (- (* 2 x z) (* 2 w y))
+ (+ (* 2 y z) (* 2 w x))
+ (- 1 (* 2 (square x)) (* 2 (square y)))
+ 0 0 0 0 1))))