source download quatlib.lsp
Module: quatlib.lsp
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
- Quat: exp,log functionVersion: 0.6 - Fixed mkTransRotDq issue with translation/rotation; Added trans/rot example
- Dual Quat: preperation rotation dq, DLB, NLERP,
- ScLERP(experimental), forward/invers rotation+translation DQ
- Vec: Some basic vector utils
Author: Heiko Schroeter, Bremen 2016
This module provides Quaternion calculus for 2/3D operations.
Rotation, Multiplication, Division, Add, Sub etc.
Converting routines from xyz coord to and from quaternions included.
Simple lat/lon conversion included.
Interpolation routines for dual quaternions included.
This module is intended to be a (dual)quaternion scratchpad utility.
It was written for clarity not speed.
Theoretical background:
Wikipedia-Quaternion
Wikipedia-Rotation
EuclideanSpace-Quats
EuclideanSpace-Transforms
The fabulous gnu octave with quaternion calculus:
Octave-quaternion Octave
Include this file at the beginning of each file prforming quaternion calculus either one of the following lines:(load "/usr/local/share/newlisp/quatlib.lsp") ; or as a shorter alternative (module "quatlib.lsp")§
Vec:Vec
syntax: (Vec:Vec a b c d ...)
Basic functor
Vector class. Only helper for Quat calculus
Vectors are just simple list of nums i.e. (Vec a b c d e f ...)
parameter: [a - b ...] numbers
return: Vector object i.e. (Vec a b c d ...)
§
Vec:vecScale
syntax: (Vec:vecScale vec scalar)
Multiply Vector by scalar
parameter: vec,scalar - vector,num
return: Vector = scalar*(Vec a b c d ...)
§
Vec:vecAdd
syntax: (Vec:vecAdd vec1 vec2)
Add two vectors of same length
parameter: vec1,2 - vectors
return: Vector = vec1 + vec2
§
Vec:vecSub
syntax: (Vec:vecSub vec1 vec2)
Sub two vectors of same length
parameter: vec1,2 - vectors
return: Vector = vec1 - vec2
§
Vec:vecDot
syntax: (Vec:vecDot vec1 vec2)
Dot product of vectors
parameter: vec1,2 - vectors
return: num = vec1.vec2 i.e. (a1a2+b1b2+c1c2 + ...)
§
Vec:vecNorm
syntax: (Vec:vecNorm vec1 vec2)
Norm of vector i.e. sqrt(vec1*vec2)
Param: vec1,2
return: num = sqrt(vec1.vec2)
§
Vec:vecSum
syntax: (Vec:vecSum vec1)
Internal sum of vector
Param: vec1
return: num = (a + b + c + ...)
§
Quats:rad2deg
syntax: (Quats:rad2deg ang)
parameter: angle - in Radians
return: angle in Degree
§
Quats:deg2rad
syntax: (Quats:deg2rad ang)
parameter: angle - in Degree
return: angle in Radians
§
Quats:Quats
syntax: (Quats:Quats w ix jy kz)
Quaternion Functor
parameter: - w ix jy kz , default 0
return: (list Quats w x y z)
Usually w=0 and xyz are the 3D (or vector U) coords of point in space.
q = w + xi + yj + zk§
Quats:getValues
syntax: (Quats:getValues w ix jy kz)
Get numerical Values of Quaternion
parameter: - w ix jy kz
return: (w x y z)
§
Quats:conjugate
syntax: (Quats:conjugate q)
parameter: 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§
Quats:norm
syntax: (Quats:norm q)
parameter: 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)§
Quats:select
syntax: (Quats:select q)
Selects the real part w of quaternion.
parameter: 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§
Quats:scale
syntax: (Quats:scale q scale)
Quaternion Scale
parameter: q,scale - (Quats:Quats w ix jy kz), num(scale)
return: Scalar multiplied quats object i.e. scalar*q
Example:q = n * q1§
Quats:add
syntax: (Quats:add q1 q2)
Quaternion Add
parameter: 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§
Quats:sub
syntax: (Quats:sub q1 q2)
Quaternion Sub
parameter: 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§
Quats:mulscalar
syntax: (Quats:mulscalar q a)
Quaternion Scalar Multiplication = Scale
parameter: q,a - (Quats:Quats w ix jy kz), num(a)
return: Scalar multiplied quaternion.
Example:q = a * q§
Quats:divscalar
syntax: (Quats:divscalar q a)
Quaternion Scalar Division
parameter: q,a - (Quats:Quats w ix jy kz), num(a)
return: Scalar divided quaternion.
Example:q = q / a§
Quats:inverse
syntax: (Quats:inverse q)
Inverse Quaternion
parameter: 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)§
Quats:mul
syntax: (Quats:mul q1 q2)
Quaternion Multiplication
parameter: 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;§
Quats:dot
syntax: (Quats:dot q1 q2)
Quaternion Dot Product
parameter: 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)§
Quats:unit
syntax: (Quats:unit q)
Unit Quaternion
parameter: q - (Quats:Quats w ix jy kz)
return: q of unit length N(q)=1
§
Quats:q2m
syntax: (Quats:q2m q)
Convert Quaternion to Matrice
parameter: q - (Quats:Quats w ix jy kz)
return: 4x4 matrice of quaternion
§
Quats:getU
syntax: (Quats:getU q)
Get U vector of Quaternion
parameter: 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§
Quats:unit3Dvec
syntax: (Quats:unit3Dvec q)
Extract Unit Vector from Quaternion
parameter: 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§
Quats:exp
syntax: (Quats:exp q)
exponential Quaternion
parameter: 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§
Quats:log
syntax: (Quats:log q)
Natural logarithm of Quaternion
parameter: 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§
Quats:parq
syntax: (Quats:parq q planeq)
Quaternions parallel plane
parameter: 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§
Quats:perpq
syntax: (Quats:perpq q planeq)
Quaternions perpendicular plane
parameter: 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§
Quats:reflq
syntax: (Quats:reflq q planeq)
Quaternions reflection at plane
parameter: 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§
Quats:avec2q
syntax: (Quats:avec2q avector)
Generate rotation Quaternion from vector with angle i.e. (angle x y z):
parameter: avector - (list a x y z)
return: rotq
Euclideanspace-Explanation
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 !§
Quats:vec2q
syntax: (Quats:vec2q xyz)
Quaternion vec2quaternion
parameter: (list - x y z)
return: 3D Vector as Quaternion
vector (x y z) -> q(0 + ix + jy + kz)§
Quats:rotq
syntax: (Quats:rotq q rq)
Rotate Quaternion (Sandwich Operation) qnew = rq*q*rq'
parameter: 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§
Quats:xyz2ecf
syntax: (Quats:xyz2ecf xyz)
Convert xyz coord to ECEF coord system
parameter: 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
ECEF, ECF or conventional terrestrial coordinate system. see https://en.wikipedia.org/wiki/Geographic_coordinate_system . y -> ex, z -> ey, x -> ez§
Quats:ecf2xyz
syntax: (Quats:ecf2xyz ecf)
Convert ECEF to xyz coords
parameter: 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>§
Quats:latlon2xyz
syntax: (Quats:latlon2xyz tuple)
Convert lattitude/longitude tuple to xyz coords
parameter: lat - lon tuple i.e. (list lat lon) in Radians
return: (list x y z) vector with unit length
Wikipedia Geographic coordinate system
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.§
Quats:xyz2latlon
syntax: (Quats:xyz2latlon xyz R)
Convert xyz coords to lat/lon tuple
parameter: vector - xyz (unit length), R radius of sphere
return: (list lat lon) in radians
Wikipedia Geographic coordinate system
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.§
Quats:demo
syntax: (Quats:demo)
Param: none
return: nil
Euclideanspace-transform§
DQuats:DQuats
syntax: (DQuats:DQuats w i j k e ei ej ek)
Dual Quaternions
parameter: - 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:
Wikipedia Dual Quaternions
Euclideanspace DQ
Euclideanspace example
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
§
DQuats:DQuatsFQ
syntax: (DQuats:DQuatsFQ q1 q2)
Create DQ from 2 Quaternions
parameter: 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§
DQuats:DQuatsFV
syntax: (DQuats:DQuatsFV q1 3Dvec)
Create DQ from Quaternion and 3D-Vector
parameter: 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)))
§
DQuats:getTranslation
syntax: (DQuats:getTranslation dq)
Get Translation i.e. imaginary dual part of DQ
parameter: dq - w i j k e ei ej ek
return: (ei ej ek) of dual part
§
DQuats:getRotation
syntax: (DQuats:getRotation dq)
Get rotational part of DQ
parameter: dq - w i j k e ei ej ek
return: (Quats w i j k)
§
DQuats:getReal
syntax: (DQuats:getReal dq)
Get real part of DQ
parameter: dq - w i j k e ei ej ek
return: (Quats w i j k)
§
DQuats:getDual
syntax: (DQuats:getDual dq)
Get dual part of DQ
parameter: dq - w i j k e ei ej ek
return: (Quats e ei ej ek)
§
DQuats:add
syntax: (DQuats:add dq1 dq2)
Dual quaternion addition
parameter: 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))§
DQuats:sub
syntax: (DQuats:sub dq1 dq2)
Dual quaternion subtraction
parameter: 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))§
DQuats:split
syntax: (DQuats:split dq)
Split DQ in two Quaternions
parameter: dq - (DQuats:DQuats w i j k e ei ej ek)
return: (list QuaternionRealPart QuaternionDualPart)
§
DQuats:mul
syntax: (DQuats:mul dq1 dq2)
Dual quaternion multiplication
parameter: dq1,dq2 - (DQuats:DQuats w i j k e ei ej ek)
return: multiplied dq
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§
DQuats:dqmul
syntax: (DQuats:dqmul dq1 dq2)
Multiply DQs by Quaternion multiplication
parameter: 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§
DQuats:conjugateRev
syntax: (DQuats:conjugateRev dq)
Conjugate DQ for reversing multiplicands, case (1)
parameter: 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§
DQuats:conjugateTr
syntax: (DQuats:conjugateTr dq)
Conjugate DQ for Translation/Rotation, case (3)
parameter: dq - (DQuats:DQuats w i j k e ei ej ek)
return: conj w -i -j -k -e ei ej ek
§
DQuats:conjugate
syntax: (DQuats:conjugate dq)
Same as conjugateTr, case (3), for shorter writing
parameter: dq - (DQuats:DQuats w i j k e ei ej ek)
return: conjugate of unit dq
§
DQuats:conjugate_via_quats
syntax: (DQuats:conjugate_via_quats dq)
Conjugation via Quaternions. Should be the same as conjugateRev.
parameter: 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.§
DQuats:rotq
syntax: (DQuats:rotq dq rdq)
DQ "sandwich" operation
parameter: 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)§
DQuats:mkRotTransDq
syntax: (DQuats:mkRotTransDq type)
Create a translation/rotation DQ
parameter: (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)
SetupRotationDQ
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§
DQuats:mkInvRotTransDq
syntax: (DQuats:mkInvRotTransDq type)
Invert rotation/translation
parameter: (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 !§
DQuats:scale
syntax: (DQuats:scale dq r)
parameter: dq,r - (DQuats:DQuats w i j k e ei ej ek) (num r)
return: DQuats * r
Dual quaternion scale by scalar§
DQuats:mulscalar
syntax: (DQuats:mulscalar dq r)
parameter: dq,r - (DQuats:DQuats w i j k e ei ej ek) (num r)
return: DQuats * r
Dual quaternion multiplication by scalar; same as scale§
DQuats:divscalar
syntax: (DQuats:divscalar dq r)
parameter: dq,r - (DQuats:DQuats w i j k e ei ej ek) (num r)
return: DQuats/r
Dual quaternion divide by scalar§
DQuats:norm
syntax: (DQuats:norm dq)
Norm of DQ = sqrt(w^2 + e^2)
parameter: dq - (DQuats:DQuats w i j k e ei ej ek)
return: magnitude of DQuat (i.e. sqrt of added real parts)
§
DQuats:unit
syntax: (DQuats:unit dq)
Unit Dual Quaternion
NormalizeDQ
"Not too difficult. Of interest for computer graphics are only unit dual quaternions,
i.e. ||Q|| = 1. This leads to:
QQ' = (R, D)(R*, D*) = (RR*, RD* + DR*) = (1, 0)
Q = dual quaternion. R = real part, D = dual part. You see, for unit dual quaternions
the dual part vanishes. You need only to calculate the magnitude for the real part.
So the problem is reduced to calculating the magnitude of a simple quaternion.
And that is calculated analogous as it is done for complex numbers:
||R|| = sqrt(r1^2+r2^2+r3^2+r4^2)
(r1 - r4 are the components of the 4D vector R)
Now just divide the R/||R|| and D/||R|| and you have your normalized Q."
parameter: 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§
DQuats:norm1
syntax: (DQuats:norm1 dq)
Compute norm via two Quaternions
parameter: 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)§
DQuats:invers
syntax: (DQuats:invers dq)
Inverse of a Dual Quaternion
parameter: dq - (:unit (DQuats:DQuats w i j k e ei ej ek))
return: invers of unit dq
See also Answer by Fred Klingener
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)}§
DQuats:DLB
syntax: (DQuats:DLB dq2 t)
DLB interpolation for t between 0 ... 1 i.e. from (self) -> dq2
parameter: 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||§
DQuats:NLERP
syntax: (DQuats:NLERP dq2 t)
NLERP interpolation for t between 0 ... 1 i.e. from (self) -> dq2
parameter: 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||§
DQuats:ScLERP
syntax: (DQuats:ScLERP dq2 t)
ScLERP interpolation for t (Experimental !)
parameter: 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§
DQuats:demo
syntax: (DQuats:demo)
Param: none
return: nil
See also Dual Quats on Euclideanspace§
DQuats:example1
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)
eof- ∂ -
generated with newLISP and newLISPdoc