;; @module quatlib.lsp
;; @description Quaternion calculus
;; @version 0.1 - initial release
;; @version 0.2 - added various functions (invers,conjugate,norm et al) for DQuats
;; @version 0.3 - cleaned up inline documentation
;; @version 0.4 - bug fix DQuats:divscalar
;; @version 0.5
;; <blockquote> - Quat: exp,log function<br>
;; - Dual Quat: preperation rotation dq, DLB, NLERP,<br>
;; - ScLERP(experimental), forward/invers rotation+translation DQ<br>
;; - Vec: Some basic vector utils</blockquote>
;; @version 0.6 - Fixed mkTransRotDq issue with translation/rotation; Added trans/rot example
;; @author Heiko Schroeter, Bremen 2016
;;
;; This module provides Quaternion calculus for 2/3D operations.<br>
;; Rotation, Multiplication, Division, Add, Sub etc.<br>
;; Converting routines from xyz coord to and from quaternions included.<br>
;; Simple lat/lon conversion included.<br>
;; Interpolation routines for dual quaternions included.<br>
;; This module is intended to be a (dual)quaternion scratchpad utility.<br>
;; It was written for clarity not speed.
;;
;; Theoretical background:
;;
;; @link https://en.wikipedia.org/wiki/Quaternion Wikipedia-Quaternion <br>
;; @link https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation Wikipedia-Rotation <br>
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm EuclideanSpace-Quats <br>
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm EuclideanSpace-Transforms <br>
;; The fabulous gnu octave with quaternion calculus:<br>
;; @link http://octave.sourceforge.net/quaternion Octave-quaternion
;; @link https://www.gnu.org/software/octave Octave
;;
;; Include this file at the beginning of each file prforming quaternion calculus
;; either one of the following lines:
;; <pre>
;; (load "/usr/local/share/newlisp/quatlib.lsp")
;; ; or as a shorter alternative
;; (module "quatlib.lsp")
;; </pre>
(set-locale "POSIX")
(new Class 'Vec)
;; @syntax (Vec:Vec a b c d ...)
;; Basic functor<br>
;; Vector class. Only helper for Quat calculus<br>
;; Vectors are just simple list of nums i.e. (Vec a b c d e f ...)
;;
;; @param [a b ...] numbers
;; @return Vector object i.e. (Vec a b c d ...)
(define (Vec:Vec)
(if (for-all number? (first (args)))
(flat (list Vec (first (args))))
nil))
;; @syntax (Vec:vecScale vec scalar)
;; Multiply Vector by scalar
;;
;; @param vec,scalar vector,num
;; @return Vector = scalar*(Vec a b c d ...)
(define (Vec:vecScale vector scale)
(push Vec (map (fn(x) (mul x scale)) (rest vector))))
;; @syntax (Vec:vecAdd vec1 vec2)
;; Add two vectors of same length
;;
;; @param vec1,2 vectors
;; @return Vector = vec1 + vec2
(define (Vec:vecAdd vec1 vec2)
(push Vec (map add (rest vec1) (rest vec2))))
;; @syntax (Vec:vecSub vec1 vec2)
;; Sub two vectors of same length
;;
;; @param vec1,2 vectors
;; @return Vector = vec1 - vec2
(define (Vec:vecSub vec1 vec2)
(push Vec (map sub (rest vec1) (rest vec2))))
;; @syntax (Vec:vecDot vec1 vec2)
;; Dot product of vectors
;;
;; @param vec1,2 vectors
;; @return num = vec1.vec2 i.e. (a1a2+b1b2+c1c2 + ...)
(define (Vec:vecDot vec1 vec2)
(apply add (map mul (rest vec1) (rest vec2))))
;; @syntax (Vec:vecNorm vec1 vec2)
;; Norm of vector i.e. sqrt(vec1*vec2)
;;
;; @param vec1,2
;; @return num = sqrt(vec1.vec2)
(define (Vec:vecNorm vec1 vec2)
(sqrt (Vec:vecDot vec1 vec2)))
;; @syntax (Vec:vecSum vec1)
;; Internal sum of vector
;;
;; @param vec1
;; @return num = (a + b + c + ...)
(define (Vec:vecSum vec1)
(apply add (rest vec1)))
(new Class 'Quats)
(set 'Quats:PI (mul 4 (atan 1))
'Quats:epsilon 1e-4)
;; @syntax (Quats:rad2deg ang)
;; @param angle in Radians
;; @return angle in Degree
(define (Quats:rad2deg ang)
(mul 180 (div ang Quats:PI)))
;; @syntax (Quats:deg2rad ang)
;; @param angle in Degree
;; @return angle in Radians
(define (Quats:deg2rad ang)
(mul Quats:PI (div ang 180)))
;; @syntax (Quats:Quats <w ix jy kz>)
;; Quaternion Functor
;;
;; @param <w ix jy kz> , default 0
;; @return (list Quats w x y z)
;; <pre>
;; Usually w=0 and xyz are the 3D (or vector U) coords of point in space.<br>
;; q = w + xi + yj + zk
;; </pre>
(define (Quats:Quats (w 0) (x 0) (y 0) (z 0))
(if (for-all number? (list w x y z))
(list Quats w x y z)
nil))
;; @syntax (Quats:getValues <w ix jy kz>)
;; Get numerical Values of Quaternion
;;
;; @param <w ix jy kz>
;; @return (w x y z)
(define (Quats:getValues)
(rest (self)))
;; @syntax (Quats:conjugate q)
;; @param q (Quats:Quats <w ix jy kz>)
;; @return The conjugated Quats object (Quats w -x -y -z)
;; @example
;; Quaternion conjugate:
;; q = w - xi - yj - zk
;; for unit quaternions:
;; q * conj(q) = 1
(define(Quats:conjugate)
(list Quats
(self 1)
(sub (self 2))
(sub (self 3))
(sub (self 4))))
;; @syntax (Quats:norm q)
;; @param q (Quats:Quats <w ix jy kz>)
;; @return The norm of the Quats object i.e. sqrt(w^2+x^2+y^2+z^2)
;; @example
;; Quaternion Norm:
;; N(q*) = N(q)
;; N(pq) = N(p)N(q)
;; N(q)=N(w+xi+yj+zk)=sqrt(w^2+x^2+y^2+z^2)
(define (Quats:norm)
(sqrt (ssq (rest (self)))))
;; @syntax (Quats:select q)
;; Selects the real part w of quaternion.
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return The "real part" of the Quats object i.e. w
;; @example
;; Quaternion Select:
;; W(q)=W(w+xi+yj+zk)=w
;; W(q)=(q+q*)/2
(define (Quats:select q)
(self 1))
;; @syntax (Quats:scale q scale)
;; Quaternion Scale
;;
;; @param q,scale (Quats:Quats <w ix jy kz>), num(scale)
;; @return Scalar multiplied quats object i.e. scalar*q
;; @example
;; q = n * q1
(define (Quats:scale n)
(push Quats
(map (fn (x)(mul n x)) (rest (self)))))
;; @syntax (Quats:add q1 q2)
;; Quaternion Add
;;
;; @param q1,q2 (Quats:Quats <w ix jy kz>)
;; @return Added q1,q2 i.e.q=(w1+w2)+i(x1+x2)+j(y1+y2)+k(z1+z2)
;; @example
;; q = q1 + q2
(define (Quats:add q2)
(push Quats (map add
(rest (self))
(rest q2))))
;; @syntax (Quats:sub q1 q2)
;; Quaternion Sub
;;
;; @param q1,q2 (Quats:Quats <w ix jy kz>)
;; @return q1-q2 i.e. q = (w1-w2)+i(x1-x2)+j(y1-y2)+k(z1-z2)
;; @example
;; q = q1 - q2
(define (Quats:sub q2)
(push Quats (map sub
(rest (self))
(rest q2))))
;; @syntax (Quats:mulscalar q a)
;; Quaternion Scalar Multiplication = Scale
;;
;; @param q,a (Quats:Quats <w ix jy kz>), num(a)
;; @return Scalar multiplied quaternion.
;; @example
;; q = a * q
(define (Quats:mulscalar a)
(:scale (self) a))
;; @syntax (Quats:divscalar q a)
;; Quaternion Scalar Division
;;
;; @param q,a (Quats:Quats <w ix jy kz>), num(a)
;; @return Scalar divided quaternion.
;; @example
;; q = q / a
(define (Quats:divscalar a)
(if (!= 0.0 a)
(:scale (self) (div a))
nil))
;; @syntax (Quats:inverse q)
;; Inverse Quaternion
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return Inverted quaternion.
;; @example
;; qq^-1 = q^-1q = 1
;; i.e. (:mul (:inverse q1) q1) = (Quats 1 0 0 0)
;; Definition:
;; q^-1 = conj(q) / ssq(q)
;; i.e. (w + i x + j y + k z)^-1 = (w - i x - j y - k z) / (w^2 + x^2 + y^2 + z^2)
;; q1 = 1 + 2i + 3j - 4k
;; (:mul q1 (:inverse q1))
;; ==> (Quats 1 0 -5.5511e-17 0)
(define (Quats:inverse)
(:divscalar (:conjugate (self))
(ssq (rest (self)))))
;; @syntax (Quats:mul q1 q2)
;; Quaternion Multiplication
;;
;; @param q1,q2 (Quats:Quats <w ix jy kz>)
;; @return q = q1 * q2
;; @example
;; w = -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w;
;; x = q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x;
;; y = -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y;
;; z = q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z;
(define (Quats:mul q2)
(list Quats
(add (mul (self 1) (nth 1 q2)) ;; w
(mul -1 (self 2) (nth 2 q2))
(mul -1 (self 3) (nth 3 q2))
(mul -1 (self 4) (nth 4 q2)))
(add (mul (self 1) (nth 2 q2)) ;; x
(mul (self 2) (nth 1 q2))
(mul (self 3) (nth 4 q2))
(mul -1 (self 4) (nth 3 q2)))
(add (mul (self 1) (nth 3 q2)) ;; y
(mul -1 (self 2) (nth 4 q2))
(mul (self 3) (nth 1 q2))
(mul (self 4) (nth 2 q2)))
(add (mul (self 1) (nth 4 q2)) ;; z
(mul (self 2) (nth 3 q2))
(mul -1 (self 3) (nth 2 q2))
(mul (self 4) (nth 1 q2)))))
;; @syntax (Quats:dot q1 q2)
;; Quaternion Dot Product
;;
;; @param q1,2 (Quats:Quats <w ix jy kz>)
;; @return q = q1 dot q2
;; @example
;; Dot product of q1 and q2.
;; q1 dot q2 = w0w1 + x0x1 + y0y1 + z0z1
;; = W(q1 q2*)
;; (:dot q1 q1) = (:norm q1)
(define (Quats:dot q2)
(apply add (map mul
(rest (self))
(rest q2))))
;; @syntax (Quats:unit q)
;; Unit Quaternion
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return q of unit length N(q)=1
(define (Quats:unit)
(:divscalar (self)
(:norm (self))))
;; @syntax (Quats:q2m q)
;; Convert Quaternion to Matrice
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return 4x4 matrice of quaternion
(define (Quats:q2m)
(let ((a (self 1))
(b (self 2))
(c (self 3))
(d (self 4)))
(array 4 4
(list a b c d
(sub b) a (sub d) c
(sub c) d a (sub b)
(sub d) (sub c) b a))))
;; @syntax (Quats:getU q)
;; Get U vector of Quaternion
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return xyz vector U of quaternion.
;; @example
;; U = 3D vec part i.e. x y z of quaternion
(define (Quats:getU)
(slice (self) 2 3))
;; @syntax (Quats:unit3Dvec q)
;; Extract Unit Vector from Quaternion
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return w + (x y z)/N(x y z)
;; @example
;; Helper function for Quats:exp and Quats:log
;; unitq = cos(t) + U sin(t)
;; U = 3D vec, length = 1
(define (Quats:unit3Dvec)
(letn ((ttq (:unit (self)))
(costheta (:select ttq))
(sintheta (sin (acos costheta)))
(u (map (fn(x) (div x sintheta))(slice ttq 2 3)))
(normu (ssq u)))
;; (println ttq " -- " u " -- " normu)
(map (fn(x)(div x normu)) u)))
;; @syntax (Quats:exp q)
;; exponential Quaternion
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return exponential of q
;; @example
;; exp(q) = e^w * { cos(||v||) + v/||v|| sin(||v||) }
;; v = vector part of q i.e. ix,jy,kz
;; ||v|| =(sqrt(ix^2 + jy^2 + kz^2))
;; q = 1 + 2i + 3j - 4k
;; exp(q) = 1.694 - 0.7896i - 1.184j + 1.579k
(define (Quats:exp)
(letn ((vecu (:getU (self)))
(normu (sqrt (ssq vecu)))
(cosu (cos normu))
(sinu (sin normu))
(ea (exp (:select (self)))))
(if (!= 0 normu)
(:mulscalar (list Quats cosu
(mul sinu (div (vecu 0) normu))
(mul sinu (div (vecu 1) normu))
(mul sinu (div (vecu 2) normu)))
ea)
nil)))
;; @syntax (Quats:log q)
;; Natural logarithm of Quaternion
;;
;; @param q (Quats:Quats <w ix jy kz>)
;; @return natural log of quaternion
;; @example
;; ln(quaternion)
;; ln(q) = (ln ||q||, u/||u||*arccos(s/||q||)) ; u=unit 3D vec; s = w/||q||
;; q = 1 + 2i + 3j - 4k
;; log(q) = 1.701 + 0.5152i + 0.7728j - 1.03k
(define (Quats:log)
(letn ((u (:unit3Dvec (self)))
(normq (:norm (self)))
(sq (div (self 1) normq))
(accos (acos sq)))
(append (list Quats (log normq))
(map (fn(x)(mul x accos)) u))))
;; @syntax (Quats:parq q planeq)
;; Quaternions parallel plane
;;
;; @param q,planeq (Quats:Quats <w ix jy kz>)
;; @return Quaternion of parallel component of plane
;; @example
;; (set 'myq (vec2q '(2 3 4)))
;; (:parq myq (:unit (Quats:Quats 0 0 0 1)))
;;
;; Pout = 0.5 * (Pin + q*Pin*q)
;; x y stay the same, z=0 when projection is on xy plane
;; q(0 + 2i + 3j + 4k) pq(0 + 0i + 0j +1k) -> 0 + 2i + 3j +4k
(define (Quats:parq planeq)
(let ((q1 (self)))
(:mulscalar
(:add q1 (:mul (:mul planeq q1) planeq))
0.5)))
;; @syntax (Quats:perpq q planeq)
;; Quaternions perpendicular plane
;;
;; @param q,planeq (Quats:Quats <w ix jy kz>)
;; @return Quaternion of perpendicular component of plane
;; @example
;; (set 'myq (vec2q '(2 3 4)))
;; (:perpq myq (:unit (Quats:Quats 0 0 0 1)))
;;
;; Pout = 0.5 * (Pin - q*Pin*q)
;; x y = 0, z stays perpendicular on xy-plane
;; q(0 + 2i + 3j + 1k) pq(0 + 0i + 0j +1k) -> 0 + 0 + 0 + 4k
(define (Quats:perpq planeq)
(let ((q1 (self)))
(:mulscalar
(:sub q1 (:mul (:mul planeq q1) planeq))
0.5)))
;; @syntax (Quats:reflq q planeq)
;; Quaternions reflection at plane
;;
;; @param q,planeq (Quats:Quats <w ix jy kz>)
;; @return Quaternion of reflected component of plane
;; @example
;; (set 'myq (vec2q '(2 3 4)))
;; (:reflq myq (:unit (Quats:Quats 0 0 0 1)))
;;
;; Pout = q*Pin*q
;; reflection in xy plane -> xy stay the same, z negated
;; q(0 + 2i + 3j + 1k) pq(0 + 0i + 0j +1k) -> 0 + 2i + 3j - 4k
(define (Quats:reflq planeq)
(:mul (:mul planeq (self)) planeq))
;; @syntax (Quats:avec2q avector)
;; Generate rotation Quaternion from vector
;; with angle i.e. (angle x y z):
;;
;; @param avector (list a x y z)
;; @return rotq
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm Euclideanspace-Explanation <br>
;; @example
;; Rotate 45 degrees around y-axis
;; (:rotq (Quats:vec2q '(1 0 0))
;; (Quats:avec2q (Quats:deg2rad 90) 0 1 0)
;; ;; ==> (Quats 0 0,7071 0 -0,7071)
;; Rotate 45 degrees around z-axis
;; (:rotq (Quats:vec2q '(1 0 0))
;; (Quats:avec2q (Quats:deg2rad 90) 0 0 1)
;; ;; ==> (Quats 0 0,7071 0,7071 0)
;; Pls note that quaternion takes angle/2 !
(define (Quats:avec2q a x y z)
(letn ((a2 (div a 2))
(cosa2 (cos a2))
(sina2 (sin a2)))
(Quats:Quats cosa2
(mul x sina2)
(mul y sina2)
(mul z sina2))))
; @syntax (Quats:q2avec q)
; Get Vector Part of Quaternion
;
; @param q (Quats:Quats <a w ix jy kz>)
; @return angle (degrees), xyz vector renormalized.
; @example
; Does not work for rotations of 0,180 and 360 Degrees.
; (:q2avec (Quats:avec2q (Quats:deg2rad 45) 1 0 0))
; -> (45 1 0 0)
; (:q2avec (Quats:avec2q (Quats:deg2rad -56.3) 12 0.3 -4))
; -> (56.3 -12 -0.3 4)
; (define (Quats:q2avec)
; (letn ((theta2 (mul 2 (acos (nth 1 (self)))))
; (sina2 (sin theta2))
; (sq2 (sqrt 2))
; (result nil))
; ;; (println "q: " (self) " theta2: " theta2)
; (if (> (abs sina2) Quats:epsilon)
; (push (Quats:rad2deg theta2)
; (map (fn(x)
; (mul 2 (nth 1 (self))
; (div x sina2 )))
; (:getU (self))))
; nil)))
;; @syntax (Quats:vec2q xyz)
;; Quaternion vec2quaternion
;;
;; @param (list x y z)
;; @return 3D Vector as Quaternion
;; <pre>
;; vector (x y z) -> q(0 + ix + jy + kz)
;; </pre>
(define (Quats:vec2q vec)
(append '(Quats 0) vec))
;; @syntax (Quats:rotq q rq)
;; Rotate Quaternion ('Sandwich Operation') qnew = rq*q*rq'
;;
;; @param q,unit(rq) (Quats:Quats <w ix jy kz>)
;; @return Rotated q = rq*q1*(conjugate rq)
;; @example
;; i.e. q(0 + 1i + j0 + k0); rq(0.7 + i0 + j0 +0.7k) ==> 0 + 0i + 1j + 0k
;;
;; (set 'myq (vec2q '(1 0 0)))
;; (:rotq myq (:unit (Quats:Quats 0.7 0 0 0.7)))
;;
;; Rotation quaternion is a quaternion of unit length.
;; Defines the point and axis around at which the rotation is perforemd.
;; Quaternion rotation is around (x,y,z) = (0,0,0).
;; Q2 = p*Q1*p' ; p=unit(rotation quaternion)
;; Q2 output quaternion
;; Q1 input quaternion
;; (:q2v (letn ((rq (:unit (Quats 1 0 0 1)))
;; (q1 (Quats 0 5 0 0)))
;; (:mul (:mul rq q1) (:conjugate rq))))
;; You can setup the rotation quaternion from xyz coords with:
;; rotq = cos(t/2) + i(x*sin(t/2)) + j(y*sin(t/2)) + k(z*sin(t/2))
;; t = rot angle; x,y,z coord of rot vector
(define (Quats:rotq rq)
(:mul (:mul rq (self)) (:conjugate rq)))
;; @syntax (Quats:xyz2ecf xyz)
;; Convert xyz coord to ECEF coord system
;;
;; @param xyz vector of "normal" xyz coord. i.e. x right pointing, y up, z towards front
;; @return ecf vector (list ex ey ez), i.e. x to front, y right, z upwards
;; <pre>
;; ECEF, ECF or conventional terrestrial coordinate system.
;; see https://en.wikipedia.org/wiki/Geographic_coordinate_system .
;; y -> ex, z -> ey, x -> ez
;; </pre>
(define (Quats:xyz2ecf xyz)
(rotate xyz 1))
;; @syntax (Quats:ecf2xyz ecf)
;; Convert ECEF to xyz coords
;;
;; @param ecf vector , x to front, y right , z upwards
;; @return xyz vector,
;; @example
;; ECF to xyz coord system conversion.
;; ey -> x, ez -> y, ex -> z
;; </pre>
(define (Quats:ecf2xyz ecf)
(rotate ecf -1))
;; @syntax (Quats:latlon2xyz tuple)
;; Convert lattitude/longitude tuple to xyz coords
;;
;; @param lat lon tuple i.e. (list lat lon) in Radians
;; @return (list x y z) vector with unit length
;; @link https://en.wikipedia.org/wiki/Geographic_coordinate_system Wikipedia Geographic coordinate system <br>
;; @example
;; (Quats:latlon2xyz (list (Quats:deg2rad 8) (Quats:deg2rad 52)))
;;
;; Helper Functions for simple Lat/Lon calculus:
;; Multiply result by radius of sphere (earth e.g. 6371km).
;; Simple convertion from lat/lon (Radians) to xyz coords.
;; x = R * cos(lat) * cos(lon)
;; y = R * cos(lat) * sin(lon)
;; z = R * sin(lat)
;; Definitions: x = direction towards viewer
;; y = direction right
;; z = direction up
;; Also known as the ECEF, ECF,
;; or conventional terrestrial coordinate system.
(define (Quats:latlon2xyz tuple)
(list (cos (first tuple)) (cos (last tuple))
(cos (first tuple)) (sin (last tuple))
(sin (first tuple))))
;; @syntax (Quats:xyz2latlon xyz R)
;; Convert xyz coords to lat/lon tuple
;;
;; @param vector xyz (unit length), R radius of sphere
;; @return (list lat lon) in radians
;; @link https://en.wikipedia.org/wiki/Geographic_coordinate_system Wikipedia Geographic coordinate system <br>
;; @example
;; (Quats:xyz2latlon (list 1 1 1) 1)
;;
;; Simple conversion from xyz to lat/lon (Radians)
;; lat = asin(z / R)
;; lon = atan2(y, x)
;; xyz in ECEF, ECF, or conventional terrestrial coordinate system.
(define (Quats:xyz2latlon xyzlst R)
(if (and R (!= 0.0 R))
(list (asin (div (nth 2 xyzlst) R))
(atan2 (nth 0 xyzlst) (nth 1 xyzlst)))
nil))
;; @syntax (Quats:demo)
;; @param none
;; @return nil
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm Euclideanspace-transform
(define (Quats:demo)
(let ((myq (Quats:vec2q '(2 3 4)))
(myrq (Quats:vec2q '(1 0 0)))
(xyq (:unit (Quats 0 0 0 1)))
(rotq (:unit (Quats 1 0 0 1))))
(pretty-print 80 " " "%2.1g")
(println "PARQ: Parallel component of x-y plane. ")
(println " q(0 + 2i + 3j + 4k) * pq(0 + 0i + 0j + 1k)")
(println " -> 0 + 2i + 3j + 0k")
(println " calculated: " (:parq myq xyq))
(println "PERPQ: Perpendicular component of x-y plane.")
(println " q(0 + 2i + 3j + 4k) pq(0 + 0i + 0j + 1k")
(println " -> 0 + 0i + 0j + 4k")
(println " calculated: " (:perpq myq xyq))
(println "REFLQ: Reflection in x-y plane.")
(println " q(0 + 2i + 3j + 4k) pq(0 + 0i + 0j + 1k)")
(println " -> 0 + 2i + 3j - 4k")
(println " " (:reflq myq xyq))
(println "ROTQ: Rotate around z-axis")
(println " q(0 + 1i + 0j + 0k) pq(0.7 + 0i + 0j + 0.7k)")
(println " -> 0 + 0i + 1j + 0k")
(println " calculated: " (:rotq myrq rotq))
(println "Rotate vec '(1 0 0) 45 degrees around '(0 0 1) ")
(println "-> 0 + 0,7i + 0,7j + 0k")
(println " calculated: " (:rotq (Quats:vec2q '(1 0 0))
(Quats:avec2rotq (Quats:deg2rad 45) 0 0 1))))
nil)
(new Class 'DQuats)
;; @syntax (DQuats:DQuats <w i j k e ei ej ek>)
;; Dual Quaternions
;;
;; @param <w i j k e ei ej ek> , default 0
;; @return (DQuats w x y z e ei ej ek)
;;
;; Dual quaternion theory see also:<br>
;; @link https://en.wikipedia.org/wiki/Dual_quaternion Wikipedia Dual Quaternions <br>
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/index.htm Euclideanspace DQ <br>
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/example/index.htm Euclideanspace example <br>
;; <pre>
;; Dual quaternion Functor:
;; dq = w + i + j + k + εw + εi + εj + εk, where ε != 0 and e^2 = 0.
;; The 1, i, j and k terms together can represent
;; rotation (as a normalised quaternion).
;; The 1ε, iε, jε and kε terms together can represent displacement
;; (where 1ε =0 and (iε, jε, kε) is a vector equal to half the displacement).
;; Transform/Rotation: P2 = Q*P1*Q' ("Sandwich" as with quaternions)
;; P1 = vector representing the initial position of a point being transformed.
;; Q = the dual quaternion representing the transform
;; Q' = the 3rd type of conjugate
;; P2 = the result which is a vector representing the final position of the point.
;; Different writing of Dual Quaternions:
;; dq = w + xi + yj + zk + e + eix0 + ejy0 +ekz0
;; w and e = real numbers; i,j,k,ei,ej,ek = imaginary numbers
;; Usually w=0 and xyz are the 3D (or vector U) coords of point in space.
;; x0,y0,z0 translational offset
;; </pre>
;;
;; @syntax (DQuats:DQuatsFQ q1 q2)
;; Create DQ from 2 Quaternions
;;
;; @param q1,2 <w i j k e ei ej ek>
;; @return (DQuats (unit(w x y z) e ei ej ek))
;; Create Dual Quat from two Quaternions
(define (DQuats:DQuatsFQ q1 q2)
(flat
(list DQuats
(:getValues q1)
(:getValues q2))))
;; @syntax (DQuats:DQuatsFV q1 3Dvec)
;; Create DQ from Quaternion and 3D-Vector
;;
;; @param q1 <w i j k e ei ej ek>; 3Dvec = (x y z)
;; @return (DQuats (unit(w i j k) (x/2 y/2 z/2)))
(define (DQuats:DQuatsFV q1 vec)
(:DQuatsFQ q1
(append (list Quats 0) vec)))
;; @syntax (DQuats:getTranslation dq)
;; Get Translation i.e. imaginary dual part of DQ
;;
;; @param dq <w i j k e ei ej ek>
;; @return (ei ej ek) of dual part
(define (DQuats:getTranslation)
(let ((quatlst (:split (self))))
(:getU (last quatlst))))
;; @syntax (DQuats:getRotation dq)
;; Get rotational part of DQ
;;
;; @param dq <w i j k e ei ej ek>
;; @return (Quats w i j k)
(define (DQuats:getRotation)
(first (:split (self))))
;; @syntax (DQuats:getReal dq)
;; Get real part of DQ
;;
;; @param dq <w i j k e ei ej ek>
;; @return (Quats w i j k)
(define (DQuats:getReal)
(first (:split (self))))
;; @syntax (DQuats:getDual dq)
;; Get dual part of DQ
;;
;; @param dq <w i j k e ei ej ek>
;; @return (Quats e ei ej ek)
(define (DQuats:getDual)
(last (:split (self))))
;; @syntax (DQuats:add dq1 dq2)
;; Dual quaternion addition
;;
;; @param dq1,dq2 (DQuats:DQuats <w i j k e ei ej ek>)
;; @return The added dual quaternion object
;; @example
;; (list DQuats (w0+w1) (x0+x1) (y0+y1) (z0+z1)
;; (e0+e1) (ei0+ei1) (ej0+ej1) (ek0+ek1))
(define (DQuats:add dq2)
(push DQuats (map add
(rest (self))
(rest dq2))))
;; @syntax (DQuats:sub dq1 dq2)
;; Dual quaternion subtraction
;;
;; @param dq1,dq2 (DQuats:DQuats <w i j k e ei ej ek>)
;; @return The subtracted dual quaternion object
;; @example
;; (list DQuats (w0-w1) (x0-x1) (y0-y1) (z0-z1)
;; (e0-e1) (ei0-ei1) (ej0-ej1) (ek0-ek1))
(define (DQuats:sub dq2)
(push DQuats (map sub
(rest (self))
(rest dq2))))
;; @syntax (DQuats:split dq)
;; Split DQ in two Quaternions
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return (list QuaternionRealPart QuaternionDualPart)
(define (DQuats:split)
(list (list Quats (self 1) (self 2) (self 3) (self 4))
(list Quats (self 5) (self 6) (self 7) (self 8))))
;; @syntax (DQuats:mul dq1 dq2)
;; Dual quaternion multiplication
;;
;; @param dq1,dq2 (DQuats:DQuats <w i j k e ei ej ek>)
;; @return multiplied dq
;; <pre>
;; dq = w + i + j + k + εw + εi + εj + εk
;; The 1, i, j and k terms together can represent
;; rotation (as a normalised quaternion).
;; The 1ε, iε, jε and kε terms together can represent displacement
;; (where 1ε =0 and (iε, jε, kε) is a vector equal to half the displacement).
;; Transform: P2 = Q*P1*Q'
;; P1 = vector representing the initial position of a point being transformed.
;; Q = the dual quaternion representing the transform
;; Q' = the 3rd type of conjugate
;; P2 = the result which is a vector representing the final position of the point.
;;
;; Multiplication Table for Dual Quaternions which can form rotation+translation
;;
;; a*b | b.1 b.i b.j b.k b.ε b.εi b.εj b.εk
;; _________________________________________________________________
;; a.1 | 1 i j k ε εi εj εk
;; a.i | i -1 k -j εi -ε -εk εj
;; a.j | j -k -1 i εj εk -ε -εi
;; a.k | k j -i -1 εk -εj εi -ε
;; a.e | ε -εi -εj -εk 0 0 0 0
;; a.ei | εi ε -εk εj 0 0 0 0
;; a.ej | εj εk ε -εi 0 0 0 0
;; a.ek | εk -εj εi ε 0 0 0 0
;; ==>
;; w2 = a1b1 - aibi - ajbj - akbk
;; i2 = a1bi + aib1 + ajbk - akbj
;; j2 = a1bj - aibk + ajb1 + akbi
;; k2 = a1bk + aibj - ajbi + akb1
;; ε2 = aeb1 + a1be - aeibi - aejbj - aekbk - aibei - ajbej - akbek
;; εi2 = aeib1 + aebi - aekbj + aejbk + aibe + a1bei - akbej + ajbek
;; εj2 = aejb1 + aekbi - aeibk + aebj + ajbe + akbei + a1bej - aibek
;; εk2 = aekb1 - aejbi + aeibj + aebk + akbe - ajbei + aibej + a1bek
;; </pre>
(define (DQuats:mul dq2)
(let ((a1 (self 1))(ai (self 2))(aj (self 3))(ak (self 4))
(ae (self 5))(aei (self 6))(aej (self 7))(aek (self 8))
(b1 (nth 1 dq2))(bi (nth 2 dq2))(bj (nth 3 dq2))(bk (nth 4 dq2))
(be (nth 5 dq2))(bei (nth 6 dq2))(bej (nth 7 dq2))(bek (nth 8 dq2)))
(list DQuats
(add (mul a1 b1) (mul -1 ai bi) (mul -1 aj bj) (mul -1 ak bk)) ;; w
(add (mul a1 bi) (mul ai b1) (mul aj bk) (mul -1 ak bj)) ;; i
(add (mul a1 bj) (mul -1 ai bk) (mul aj b1) (mul ak bi)) ;; j
(add (mul a1 bk) (mul ai bj) (mul -1 aj bi) (mul ak b1)) ;; k
(add (mul ae b1) (mul a1 be) (mul -1 aei bi) (mul -1 aej bj )
(mul -1 aek bk) (mul -1 ai bei) (mul -1 aj bej) (mul -1 ak bek)) ;; e
(add (mul aei b1) (mul ae bi) (mul -1 aek bj) (mul aej bk )
(mul ai be) (mul a1 bei) (mul -1 ak bej) (mul aj bek)) ;; ei
(add (mul aej b1) (mul aek bi) (mul -1 aei bk) (mul ae bj )
(mul aj be) (mul ak bei) (mul a1 bej) (mul -1 ai bek)) ;; ej
(add (mul aek b1) (mul -1 aej bi) (mul aei bj) (mul ae bk )
(mul ak be) (mul -1 aj bei) (mul ai bej) (mul a1 bek))))) ;; ek
;; @syntax (DQuats:dqmul dq1 dq2)
;; Multiply DQs by Quaternion multiplication
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return multiplied <w i j k e ei ej ek>
;; @example
;; Multiplication of dual quaternions via quaternion multiplication
;; and addition. Slower than DQuats:mul but cleaner algorithm.
;; May be useful for cross checking.
;; dq1 = q0 + q1e
;; dq2 = q2 + q3e
;; dq1 + dq2 = (q0+dq1e)(q2+q3e)
;; = q0q2 + (q0q3 + q1q2)e
(define (DQuats:dqmul q2)
(letn ((sq1 (:split (self)))
(sq2 (:split q2)))
(flat (list DQuats
(rest (:mul (first sq1) (first sq2)))
(rest (:add (:mul (first sq1) (last sq2))
(:mul (last sq1) (first sq2))))))))
;; @syntax (DQuats:conjugateRev dq)
;; Conjugate DQ for reversing multiplicands, case (1)
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return conj <w -i -j -k e -ei -ej -ek>
;; @example
;; case (1), reversing multiplicands
;; The dual quaternion conjugate is essentially an extension of the
;; quaternion conjugate, and is given by:
;; conj(dq) = conj(realPart) + e*(conj(dualPart))
;; Conjugate Dual Quaternion
;; There are multiple definitions for the conjugate of a dual quaternion:
;; Q† = r† + ε d†
;; Q† = r - ε d
;; Q† = r† - ε d†
;; where:
;; Q = the dual quaternion
;; Q† = the conjugate of the dual quaternion
;; r = a quaternion
;; d = another quaternion which forms the dual part of the dual quaternion.
;; The type of conjugate that we use depends on what we want it to do:
;; TYPE OF CONJUGATE USE OF THIS TYPE
;; case (1) Q† = r† + ε d† reversing multiplicands
;; (Q1 Q2)† = Q2† Q1†
;; case (2) Q† = r - ε d -
;; case (3) Q† = r† - ε d† translation
;; Pout = Q * Pin * Q†
;; case (1) dq = w + i + j + k + εw + εi + εj + εk
;; dq' = w - i - j - k + εw - εi - εj - εk
;; dq * dq' = (w + εw); real number
;; case (3) dq = w + i + j + k + εw + εi + εj + εk
;; dq' = w - i - j - k - εw + εi + εj + εk
(define (DQuats:conjugateRev)
(list (self 0)
(self 1) (sub (self 2)) (sub (self 3)) (sub (self 4))
(self 5) (sub (self 6)) (sub (self 7)) (sub (self 8))))
;; @syntax (DQuats:conjugateTr dq)
;; Conjugate DQ for Translation/Rotation, case (3)
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return conj <w -i -j -k -e ei ej ek>
(define (DQuats:conjugateTr)
(list DQuats
(self 1) (sub (self 2)) (sub (self 3)) (sub (self 4))
(sub (self 5)) (self 6) (self 7) (self 8)))
;; @syntax (DQuats:conjugate dq)
;; Same as conjugateTr, case (3), for shorter writing
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return conjugate of unit dq
(define (DQuats:conjugate)
(:conjugateTr (self)))
;; @syntax (DQuats:conjugate_via_quats dq)
;; Conjugation via Quaternions. Should be the same as conjugateRev.
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return conj <w -i -j -k e -ei -ej -ek>
;; @example
;; conjugate_via_quats as reference only.
(define (DQuats:conjugate_via_quats)
(letn ((sq (:split (self))) ;; split into quaternions
(q1 (first sq))
(q2 (last sq)))
(DQuats:DQuatsFQ (:conjugate q1) (:conjugate q2))))
;; @syntax (DQuats:rotq dq rdq)
;; DQ "sandwich" operation
;;
;; @param dq,rdq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return rotated/translated (DQuats w i j k e ei ej ek)
;; @example
;; Dual quaternion rotation i.e. dual quaternion 'Sandwich' operation.
;; dq = unit(rdq) * DQin * unit(rdq)^-1
;;
;; 1) setup rdq
;; (set 'drq (:mkRotTransDq (DQuats:DQuatsFQ '(Quats 0 1 0 0) '(Quats 0 4 2 6)) 4)) ;; trans then rotate
;; 2) setupq dq
;; (set 'dq1 (DQuats:DQuatsFQ '(Quats 1 0 0 0) '(Quats 0 3 4 5)))
;; 3) move the whole lot
;; (:rotq dq1 drq) --> (DQuats 1 0 0 0 0 7 -6 -11)
(define (DQuats:rotq rdq)
(:mul (:mul (:unit rdq) (self)) (:conjugateTr (:unit rdq))))g
;; @syntax (DQuats:mkRotTransDq type)
;; Create a translation/rotation DQ
;;
;; @param (self) (DQuats:DQuats <w i j k e ei ej ek>), type int
;; @return rotate/translate DQ (DQuats w i j k e ei ej ek)
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/index.htm SetupRotationDQ<br>
;; @example
;; Combining Transforms:
;; 1) Pure Rotation
;; Q = r
;; 2) Pure Translation
;; Q = 1 + ε½t
;; 3) Translation then Rotation
;; Q = (1 + ε½t)*r
;; = r + ε½t r
;; 4) Rotation then Translation
;; Q = r*(1 + ε½t)
;; = r + ε½r t
;; 5) Rotation about a point
;; Q = (1 + ε½c)*r*(1 - ε½c)
;; = (1 + ε½c)*(r - ε½r c)
;; = r - ε½r c + ε½c r
;;
;; (set 'dq1 (DQuats:DQuatsFQ '(Quats 1 0 0 0) '(Quats 0 3 4 5)))
;; (set 'drq (DQuats:DQuatsFQ '(Quats 0 1 0 0) '(Quats 0 4 2 6)))
;; (:rotq dq1 (:mkRotTransDq drq 4)) --> (DQuats 1 0 0 0 0 7 -6 -11) ;; translate then rotate
(define (DQuats:mkRotTransDq type)
(case type
(1 (DQuats:DQuatsFQ (:getReal (self)) '(Quats 0 0 0 0))) ;; pure rotation
(2 (DQuats:DQuatsFQ '(Quats 1 0 0 0) (:mulscalar (:getDual (self)) 0.5))) ;; pure translation
(3 (DQuats:DQuatsFQ (:getReal (self)) (:mul (:mulscalar (:getDual (self)) 0.5) (:getReal (self))))) ;;TransRot
(4 (DQuats:DQuatsFQ (:getReal (self)) (:mul (:mulscalar (:getReal (self)) 0.5) (:getDual (self))))) ;;RotTrans
(5 (DQuats:DQuatsFQ (:getReal (self)) ;; Rotation about a point
(:add
(:mul (:mulscalar (:getReal (self)) -0.5) (:getDual (self)))
(:mul (:mulscalar (:getDual (self)) 0.5) (:getReal (self))))))
(true nil)))
;; @syntax (DQuats:mkInvRotTransDq type)
;; Invert rotation/translation
;;
;; @param (self) (DQuats:DQuats <w i j k e ei ej ek>), type int
;; @return invers rotate/translate DQ (DQuats w i j k e ei ej ek)
;; @example
;; To invert the quaternion 'r' we use its conjugate 'r†',
;; to invert the translation 't' we use '-t'. With combined transforms we must
;; also reverse the order that they are combined
;; (don't forget that order is important for translations and their corresponding dual quaternion).
;; The inverse of common transforms are:
;; Q Q-1
;; Pure Rotation r r†
;; Pure Translation 1 + ε½t 1 - ε½t
;; Translation then Rotation r + ε½t r r† - ε½r† t
;; Rotation then Translation r + ε½r t r† - ε½t r†
;; Rotation about a point 'c' r - ε½r c + ε½c r r† + ε½r† c - ε½c r†
;;
;; (set 'dq1 (DQuats:DQuatsFQ '(Quats 1 0 0 0) '(Quats 0 3 4 5)))
;; (set 'drq (DQuats:DQuatsFQ '(Quats 0 1 0 0) '(Quats 0 4 2 6)))
;; (:rotq (:rotq dq1 (:mkRotTransDq drq 4)) (:mkInvRotTransDq drq 3)) == dq1
;; ! Pls note that you have to inverse the operation of rotation resp. translation as well !
(define (DQuats:mkInvRotTransDq type)
(case type
(1 (DQuats:DQuatsFQ (:conjugate (:getReal (self))) '(Quats 0 0 0 0))) ;; pure rotation
(2 (DQuats:DQuatsFQ '(Quats 1 0 0 0) (:mulscalar (:getDual (self)) -0.5))) ;; pure translation
(3 (DQuats:DQuatsFQ (:conjugate (:getReal (self)))
(:mul (:mulscalar (:conjugate (:getDual (self))) -0.5)
(:getReal (self))))) ;;TransRot
(4 (DQuats:DQuatsFQ (:conjugate (:getReal (self)))
(:mul (:mulscalar (:getReal (self)) -0.5)
(:conjugate (:getDual (self)))))) ;;RotTrans
(5 (DQuats:DQuatsFQ (:conjugate (:getReal (self))) ;; Rotation about a point
(:add
(:mul (:mulscalar (:conjugate (:getReal (self))) 0.5) (:getDual (self)))
(:mul (:mulscalar (:getDual (self)) -0.5) (:conjugate (:getReal (self)))))))
(true nil)))
;; @syntax (DQuats:scale dq r)
;; @param dq,r (DQuats:DQuats <w i j k e ei ej ek>) (num r)
;; @return DQuats * r
;; <pre>Dual quaternion scale by scalar</pre>
(define (DQuats:scale r)
(push DQuats (map (fn(x)(mul x r)) (rest (self)))))
;; @syntax (DQuats:mulscalar dq r)
;; @param dq,r (DQuats:DQuats <w i j k e ei ej ek>) (num r)
;; @return DQuats * r
;; <pre>Dual quaternion multiplication by scalar; same as scale</pre>
(define (DQuats:mulscalar r)
(:scale (self) r))
;; @syntax (DQuats:divscalar dq r)
;; @param dq,r (DQuats:DQuats <w i j k e ei ej ek>) (num r)
;; @return DQuats/r
;; <pre>Dual quaternion divide by scalar</pre>
(define (DQuats:divscalar r)
(if (!= 0.0 r)
(:mulscalar (self) (div r))
nil))
;; @syntax (DQuats:norm dq)
;; Norm of DQ = sqrt(w^2 + e^2)
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return magnitude of DQuat (i.e. sqrt of added real parts)
(define (DQuats:norm)
(let ((mag2 (:mul (self) (:conjugateTr (self)))))
(sqrt
(add
(abs (nth 1 mag2))
(abs (nth 5 mag2))))))
;; @syntax (DQuats:unit dq)
;; Unit Dual Quaternion
;;
;; @link http://stackoverflow.com/questions/23174899/properly-normalizing-a-dual-quaternion NormalizeDQ<br>
;; "Not too difficult. Of interest for <b><i>computer graphics</i></b> are only <b><i>unit</i></b> dual quaternions,<br>
;; i.e. ||Q|| = 1. This leads to:<br>
;; QQ' = (R, D)(R*, D*) = (RR*, RD* + DR*) = (1, 0)<br>
;; Q = dual quaternion. R = real part, D = dual part. You see, for <b><i>unit</i></b> dual quaternions<br>
;; the dual part vanishes. You need only to calculate the magnitude for the real part.<br>
;; So the problem is reduced to calculating the magnitude of a simple quaternion.<br>
;; And that is calculated analogous as it is done for complex numbers:<br>
;; ||R|| = sqrt(r1^2+r2^2+r3^2+r4^2)<br>
;; (r1 - r4 are the components of the 4D vector R)<br>
;; Now just divide the R/||R|| and D/||R|| and you have your normalized Q."<br>
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return DQuats of unit magnitude
;; @example
;; ||dq^|| = ||dq^ * conj(dq^)|| = 1
;; dq^ = unit dq
(define (DQuats:unit)
(:divscalar (self) (:norm (self))))
; ;; @syntax (DQuats:unit1 dq)
; ;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
; ;; @return DQuats of unit magnitude
; ;; <pre>
; ;; ||dq^|| = dq/sqrt(ssq(Real part of dq))
; ;; dq^ = unit dq
; ;; </pre>
; (define (DQuats:unit1)
; (:divscalar (self) ;; divide dual quat
; (sqrt ;; by sqrt of norm of real part of dual quat
; (:norm
; (first
; (:split (self)))))))
;; @syntax (DQuats:norm1 dq)
;; Compute norm via two Quaternions
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>)
;; @return (list) Dual number which is the norm of a dq; i.e. a=a0+e*a1
;; @example
;; (set 'p1 (DQuats:DQuats 1 12 13 14 2 3 4 5))
;; (:norm1 (:mul (:unit p1) (:conjugateTr (:unit p1)))) ==> (1 7.619e-19) ~ (1,0)
;;
;; Norm of Dual Quaternion:
;; ||D|| = ||D0|| + e[(conj(D0)*D1 + conj(D1)*D0)/(2*||D0||)]
;; where ||D0|| is the quaternion norm of the real part.
;; Definition: |Â| = sqrt(Â Â*), a dual number called the magnitude of the
;; dual quaternion. Dual quaternions with |Â| = 1 are unit dual quaternions.
;; Dual quaternions of magnitude 1 are used to represent spatial Euclidean
;; displacements. Notice that the requirement that  Â* = 1,
;; introduces two algebraic constraints on the components of Â, that is:
;; Â Â* = (D0,D1)(D0*,D1*) = (D0D0*,D0D1*+D1D0*) = (1,0)
(define (DQuats:norm1)
(letn ((sdq (:split (self)))) ;; dual quat
(list (:norm (first sdq))
(:norm (last sdq)))))
;; @syntax (DQuats:invers dq)
;; Inverse of a Dual Quaternion
;;
;; @param dq (:unit (DQuats:DQuats <w i j k e ei ej ek>))
;; @return invers of unit dq
;; See also @link https://math.stackexchange.com/questions/761124/dual-quaternion-inverse <em>Answer by Fred Klingener</em>
;;
;; @example
;; (set 'p1 (DQuats:DQuats 1 12 13 14 2 3 4 5))
;; ;; According to 0.) (U + ε V) (U0 + ε V0) = 1:
;; (:norm (:mul (:unit p1) (:inverse (:unit p1)))) ==> (1 1.388e-17) ~ (1,0)
;;
;; Attention: This holds only for unit dual quaternions !
;; Dual quaternion inverse Q^-1 = (U0 + ε V0)
;; 0.) (U + ε V) (U0 + ε V0) = 1 ; for unit dual quaternions !
;; 1.) U U0 = 1, U0 = U* ==> invers==conjugate only for unit quaternions !
;; 2.) V U0 + U V0 = 0 = V U* + U V0,
;; V0 = - U* V U*
;; So the invers of Dual Quaternion is:
;; Q^-1 = (U* - ε U* V U*) ;; {U* = conj(U)}
(define (DQuats:inverse)
(letn ((usdq (:split (self)))
(u (first usdq))
(v (last usdq))
(uconj (:conjugate u)))
(flat
(push DQuats (list
(rest uconj) ;; U part
(rest ;; εV part
(:mul (:mulscalar uconj -1) (:mul v uconj))
))))))
;; @syntax (DQuats:DLB dq2 t)
;; DLB interpolation for t between 0 ... 1 i.e. from (self) -> dq2
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>); (float) t
;; @return interpolated dq between (self) and dq2 according to t
;; @example
;; dq = d1*(1-t)+d2*t / ||d1*(1-t) + d2*t||
(define (DQuats:DLB dwq2 t)
(letn ((tq1 (:mulscalar (self) (sub 1 t)))
(tq2 (:mulscalar dwq2 t))
(qsum (:add tq1 tq2)))
(:divscalar qsum (:norm qsum))))
;; @syntax (DQuats:NLERP dq2 t)
;; NLERP interpolation for t between 0 ... 1 i.e. from (self) -> dq2
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>); (float) t
;; @return interpolated dq between (self) and dq2 according to t
;; @example
;; d1 + (d2-d1)*t / ||d1 + (d2-d1)*t||
(define (DQuats:NLERP dwq2 t)
(let ((nlerp (:add (self)
(:mulscalar (:sub dwq2 (self)) t))))
(:divscalar nlerp (:norm nlerp))))
;; @syntax (DQuats:ScLERP dq2 t)
;; ScLERP interpolation for t (Experimental !)
;;
;; @param dq (DQuats:DQuats <w i j k e ei ej ek>); (float) t
;; @return interpolated dq between (self) and dq2 according to t
;; @example
;; q = qa(qa^-1 qb)^t
(define (DQuats:ScLERP dq2 t)
;#; Attention: Very Bad Lisp style ahead ....
;#; Example C-Class from Ben Kenwright, Oct 2012
;#; "Dual-Quaternions: From Classical Mechanics to Computer Graphics and Beyond"
;#; Ben Kenwright www.xbdev.net, bkenwright@xbdev.net
(letn ((lst1 (:split (self)))
(lst2 (:split dq2))
(dot (:dot (first lst1) (first lst2))))
(when (< dot 0)
(setq dq2 (DQuats:conjugateRev dq2)))
(setq diff (:mul (DQuats:conjugateRev (self)) dq2))
(setq vr (push Vec (slice diff 2 3)))
(setq vd (push Vec (slice diff 6 4)))
(when (!= 0 (Vec:vecSum vr))
(begin
(setq invr (div (Vec:vecNorm vr vr)))
(setq angle (mul 2 (acos (diff 1))))
(setq pitch (mul -2 invr (diff 5)))
(setq direction (Vec:vecScale vr invr))
(setq moment (Vec:vecSub
vd
(Vec:vecScale direction (mul pitch (diff 1)))))
(setq angle (mul t angle))
(setq pitch (mul t pitch))
(setq sinAngle (sin (div angle 2)))
(setq cosAngle (cos (div angle 2)))
(setq qReal (flat (list Quats cosAngle (rest (Vec:vecScale direction sinAngle)))))
(setq qDual (flat (list Quats
(mul -0.5 sinAngle pitch)
(rest (Vec:vecAdd (Vec:vecScale moment sinAngle)
(Vec:vecScale direction (mul pitch 0.5 cosAngle)))))))
(:mul (self) (DQuats:DQuatsFQ qReal qDual))))))
;; @syntax (DQuats:demo)
;; @param none
;; @return nil
;; @link http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/index.htm <em>See also Dual Quats on Euclideanspace</em>
(define (DQuats:demo)
(pretty-print 80 " " "%1.4g")
(let ((p1 (DQuats:DQuats 1 0 0 0 0 3 4 5))
(dq (DQuats:DQuats 1 0 0 0 0 2 1 3))
(rq (DQuats:DQuats 0 1 0 0 0 0 0 0))
(dtr (DQuats:DQuats 0 1 0 0 -2 0 -3 1))
(qpc (DQuats:DQuats 1 0 0 0 0 0 0 50))
(qch (DQuats:DQuats 0.9659 0 0.2588 0 0 0 0 0)))
(println
"Examples taken from:
http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/example/index.htm
So if we are initially at point (x=3, y=4, z=5) this will be represented by the Dual Quaternion:
P1 = 1 + 3 iε + 4 jε + 5 kε
A pure displacement will be represented by the dual quaternion:
(1 + x/2 iε + y/2 jε + z/2 kε)
We now want to displace the initial point by (x=4, y=2, z=6) this will be represented by the Dual Quaternion:
q = (1 + 2 iε + 1 jε + 3 kε)
So to combine these, to give the resulting position, we use P2 = q * P1 * q' to give:
p2 = (1 + 2 iε + 1 jε + 3 kε)*(1 + 3 iε + 4 jε + 5 kε)*(1 + 2 iε + 1 jε + 3 kε)
multiplying out the first two terms using the above multiplication table we get:
P2 = (1 + 5 iε + 5 jε + 8 kε)*(1 + 2 iε + 1 jε + 3 kε)
P2 = (1 + 7 iε + 6 jε + 11 kε)")
(println "==>" (:mul (:mul dq p1) (:conjugateTr dq)) )
(println)
(println
"If the displacement is zero then iε = jε = kε = 0 and the rotation is
represented by the normalised quaternion as explained on this page.
Applying a rotation of point (3,4,5) by 180° around the x axis is given by:
P2 = (0 + i)*(1 + 3iε + 4jε + 5 kε)*(0 - i)
P2 = ( i - 3ε - 5jε + 4kε)*(0 - i)
P2 = 1 + 3iε - 4jε - 5 kε")
(println "==>" (:mul (:mul rq p1) (:conjugateTr rq)))
(println)
(println "Press a key to continue ... ")
(read-key)
(println)
(println
"Starting from the previous position: (1 + 3iε + 4jε + 5 kε)
and both displace by (x=4, y=2, z=6) and applying a rotation
of 180° around the x axis represented by: (0 + i)
Therefore:
Q = (0 + i) (1 + 2 iε + 1 jε + 3 kε)
Q = (i - 2 ε - 3 jε + 1 kε)
So applying the transform gives:
P2 = (i - 2 ε - 3 jε + 1 kε)*(1 + 3 iε + 4 jε + 5 kε)*(-i + 2 ε -3 jε + 1 kε)
P2 = (i - 5 iε - 8 jε + 5 kε)*(-i + 2 ε -3 jε + 1 kε)
P2 = 1 + 7 iε - 6 jε - 11 kε")
(println "==>" (:mul (:mul dtr p1) (:conjugateTr dtr)))
(println "-----------------------------------------------------------------------------")
(println "
Example by Amy de Buitléir
Calculate the scene inside a container according to home coordinates.
http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/example/index.htm
Ex: We have a prop inside a container, which is inside a scene.
The prop is at (0, 0, 100), specified in the container's co-ordinate system.
The container is at (0, 0, 0), specified in the scene's co-ordinate system.
The container is also rotated +30° around the y-axis.
{Note: I have shown the coordinates differently from usual (z down, y toward viewer).}
Q: What is the location of the prop, specified in the scene's co-ordinate
system? ")
(println)
(println "Press a key to continue ...")
(read-key)
(println)
(println
"First, we need to build a dual quaternion to represent the state
(location and rotation) of the prop wrt (with respect to) the container.
We'll call this dual quaternion Q(prop,container).
The position of the prop with respect to the container can be represented by
the quaternion:
t(prop,container) = (0, 0, 0, 100)
The real part of the dual quaternion Q(prop,container) is 1 because prop isn't rotated.
The dual part is:
dual part = (1/2)(real)t = (1/2)(1)(0, 0, 0, 100) = (0, 0, 0, 50)
Q(prop,container) = 1 + ε(0, 0, 0, 50)
Next, we need to build a dual quaternion to represent the state of the container wrt the scene.
t(container,home) = 0 because the container is not translated.
The container is rotated 30 around the y-axis, so
sx2 = sz2 = 0 sy2 = sin(30/2) = 0.2588
cx2 = cz2 = 1 cy2 = cos(30/2) = 0.9659
real part = (0.9659, 0, 0.2588, 0)
dual part = (1/2)(real)t = 0
Q(container,home) = (0.9659, 0, 0.2588, 0) + ε0 = (0.9659, 0, 0.2588, 0)
Now we're ready to calculate the state of the prop wrt the scene:
t(prop,home) = 2*real†*dual
= 2(0.9659, 0, 0.2588, 0)†(0, -12.94, 0, 48.295)
= 2(0.9659, 0, -0.2588, 0)(0, -12.94, 0, 48.295)
= 2(0, -25.0, 0, 43.3)
= (0, -50, 0, 86.6)")
(let ((splitDQ (:split (:mul qpc qch))))
(println " Calculated ==> " (:mulscalar
(:mul
(:conjugate (first splitDQ))
(last splitDQ))
2)))
(println "Press a key to continue ...")
(read-key)
(println)
(println
"We can double-check this using an inverse transformation. We'll use the last
quaternion to calculate the state of the prop wrt the container, and verify
that it agrees with the original quaternion.
Q(prop,container) = Q(prop,home) * Q(home,container)
= Q(prop,home) * Q(container,home)†
= [(0.9659, 0, 0.2588, 0) + ε(0, -12.94, 0, 48.295)] (0.9659, 0, 0.2588, 0)†
= [(0.9659, 0, 0.2588, 0) + ε(0, -12.94, 0, 48.295)] (0.9659, 0, -0.2588, 0)
= 1 + ε(0, -12.94, 0, 48.295)(0.9659, 0, -0.2588, 0)
= 1 + ε(0, 0, 0, 50)
Which agrees with the original calculation.")
(println)
(println "Calculated ==> " (let ((qph (DQuats:DQuats 0.9659 0 0.2588 0 0 -12.94 0 48.295))
(qhc (DQuats:DQuats 0.9659 0 0.2588 0 0 0 0 0)))
(:mul qph (:conjugateTr qhc)))))
nil)
;; @syntax (DQuats:example1)
;; Example of Rotatation and Translation
;; @param none
;; @return nil
;; @example
;; DQ1: (DQuats 1 0 0 0 0 0 100 0)
;; Rotq: (DQuats 0.9239 0 0 0.3827 0 0 15 0) i.e. +45° (z-axis) and dy = +15
;; 1) Rotate only:
;; --> (DQuats 1 0 0 0 0 -70.71 70.71 0)
;; 2) Translate only:
;; --> (DQuats 1 0 0 0 0 0 115 0)
;; 3) Translate then rotate:
;; --> (DQuats 1 0 0 0 0 -70.71 85.71 0)
;; 4) Rotate then translate
;; --> (DQuats 1 0 0 0 0 -81.32 81.32 0)
;; 5) Rotate around point
;; --> (DQuats 1 0 0 0 0 -60.1 75.1 0)
(define (DQuats:example1)
(let ((drq (list DQuats
(cos (Quats:deg2rad (div 45 2))) 0 0 (sin (Quats:deg2rad (div 45 2)))
0 0 15 0))
(dq1 '(DQuats 1 0 0 0 0 0 100 0))
(pp (pretty-print)))
(pretty-print 80 " " "%1.4g")
(println "DQ1: " dq1)
(println "Rotq: " drq " i.e. +45° (z-axis) and dy = +15")
(println)
(println "1) Rotate only: ")
(println " --> " (:rotq dq1 (:mkRotTransDq drq 1)))
(println "2) Translate only: ")
(println " --> " (:rotq dq1 (:mkRotTransDq drq 2)))
(println "3) Translate then rotate: ")
(println " --> " (:rotq dq1 (:mkRotTransDq drq 3)))
(println "4) Rotate then translate ")
(println " --> " (:rotq dq1 (:mkRotTransDq drq 4)))
(println "5) Rotate around point ")
(println " --> " (:rotq dq1 (:mkRotTransDq drq 5)))
(pretty-print 80 " " "%1.15g"))
nil)
;;
;; eof
syntax highlighting with newLISP and newLISPdoc