\ Quaternions and matrices of rotation
\ Forth Scientific Library Algorithm #56
\
\ Copyright 1998 Pierre Henri Michel., Abbat.
\ Anyone may use this code freely, as long as this notice is preserved.
CR
.( Quaternions and matrices of rotation ) CR
.( by Pierre Abbat Version 1.1 20001102 ) CR
\
\ What are quaternions?
\
\ Quaternions are an extension of complex numbers. There are three
\ linearly independent (over the reals) numbers, i, j, and k, whose
\ square is -1. Mathematicians use i and electrical engineers use j,
\ but no one as far as I know uses k, except in quaternions. A
\ quaternion is the sum of four real numbers multiplied by 1, i, j,
\ and k respectively.
\
\ What is the algebraic structure of quaternions?
\
\ Quaternions add termwise. Multiplication is a bit complicated, and
\ unlike the complex numbers it is not commutative. The distributive
\ and associative laws hold, and every nonzero quaternion has a
\ multiplicative inverse. Thus the quaternions form a noncommutative
\ division ring.
\
\ To multiply two quaternions, consider them as the sum of a scalar
\ and a vector in 3-space. The product of two scalars is a scalar,
\ the product of a vector and a scalar is a vector, and the product
\ of two vectors is their cross product minus their dot product.
\
\ What are quaternions used for?
\
\ Quaternions are used to represent rotations. The mapping is two-to-one;
\ for every rotation there are two unit-norm quaternions, which are
\ additive inverses, which represent it.
\
\ A rotation about an axis is represented by a quaternion whose scalar part
\ is the cosine of half the angle and whose vector part is along the axis
\ by the sine of half the angle. These are called Euler parameters.
\ http://mathworld.wolfram.com/EulerParameters.html
\
\ If you turn a sphere once around an axis, the quaternion representing
\ that rotation is -1. If you turn it twice around an axis, the quaternion
\ is +1. Thus, a double rotation is topologically deformable to no rotation,
\ whereas a single rotation is not. This is why, in centrifuges with feed lines,
\ the feed line turns once when the centrifuge turns twice.
\
\ Conventions
\
\ The coordinates are oriented as follows:
\ Bow +x Stern -x
\ Starboard +y Port -y
\ Keel +z Mast -z
\ This is the convention used for airplanes. The bottom is positive z,
\ which to me is upside down.
\ http://www.monmouth.com/~jsd/how/htm/motion.html#fig_axes
\
\ If the scalar component of a quaternion is positive, and the vector component
\ is nonzero, and the thumb of a right hand forming the dez of ten in American
\ Sign Language is pointed in the direction of the vector component, the quaternion
\ denotes a rotation less than a straight angle in the direction of the curled fingers.
\
\ A matrix of rotation denotes that rotation in which the vector (x y z) is
\ x'
\ dropped in the top of it and (y') comes out the side.
\ z'
\
\ If the norm of a quaternion is not 1 and it is interpreted as a rotation,
\ the rotation is accompanied by stretching or shrinking by the norm squared.
\
\ If an object is in the orientation q2 and is then rotated by q1 in
\ world coordinates (for example a ship lists from a north wind),
\ the resulting orientation is q1 q2 q*. If it is rotated by q1 in
\ object coordinates (bzw. it lists to port), it is q2 q1 q*. The same
\ order of operations applies to matrices as well.
\
\ The following words are provided for the user:
\
\ Q ( -- ; f: w x y z )
\ Puts a quaternion literal on the floating-point stack.
\ This word is state-smart.
\
\ QDUP ( f: q - q q )
\ Duplicates a quaternion.
\
\ Q+ ( f: q q - q )
\ Adds two quaternions.
\
\ Q- ( f: q q - q )
\ Subtracts two quaternions.
\
\ QDROP ( f: q )
\ Drops a quaternion.
\
\ Q* ( f: q q - q )
\ Multiplies two quaternions.
\
\ Q/ ( f: q q - q )
\ Divides two quaternions.
\
\ Q. ( f: q )
\ Outputs a quaternion to the console.
\
\ F>Q ( f: f - q )
\ Converts a real number to a quaternion with vector part (0,0,0).
\
\ Q>F ( f: q - f )
\ Returns the real part of a quaternion.
\
\ V>Q ( f: x y z - q )
\ Converts a vector to a quaternion with real part 0.
\
\ Q>V ( f: q - x y z )
\ Returns the vector part of a quaternion.
\
\ NORMAL ( f: q ; - ? )
\ Returns true if the norm of the quaternion is close to 1.
\
\ NORM ( f: q - f )
\ Returns the norm of a quaternion.
\
\ NORMALIZE ( f: q - q )
\ Divides a quaternion by its norm.
\
\ QINV ( f: q - q )
\ Computes the multiplicative inverse of a quaternion.
\
\ QCONJ ( f: q - q )
\ Computes the conjugate of a quaternion.
\
\ QF* ( f: q f - q )
\ Multiplies a quaternion by a real number. Equivalent to F>Q Q* but faster.
\
\ QVARIABLE ( -- )
\ Creates a quaternion variable.
\
\ Q@ ( addr ; f: - q )
\ Fetches a quaternion variable.
\
\ Q! ( addr ; f: q )
\ Stores a quaternion variable.
\
\ DEGREES ( f: f - f )
\ Converts degrees to radians.
\
\ ROLLQ ( f: angle - q )
\ PITCHQ ( f: angle - q )
\ YAWQ ( f: angle - q )
\ Convert an angle in radians to a quaternion representing a rotation about an axis.
\ These can be used to construct a quaternion corresponding to a rotation
\ given in terms of Euler angles. There are at least three different Euler-angle
\ conventions, so I shall leave the Euler-angle words to you.
\
\ }}QUAT>MATRIX ( mat{{ ; f: q )
\ Converts a quaternion representing a rotation to a 3*3 matrix
\ representing the same rotation.
\
\ }}MATRIX>QUAT ( mat{{ ; f: - q )
\ Converts a matrix representing a rotation to a quaternion
\ representing the same rotation. If the matrix is not orthogonal
\ or has a nonpositive determinant, the result will be garbage.
\
\ }}MAT*VEC ( mat{{ ; f: x y z - x' y' z' )
\ Multiplies the matrix by the vector.
\
\ QROTATE ( f: q x y z - x' y' z' )
\ Rotates the vector (x y z) by the quaternion q. The result is the
\ same as converting it to a matrix and using }}MAT*VEC. The formula
\ is explained on Laura Downs' website.
\ http://http.cs.berkeley.edu/~laura/cs184/quat/quaternion.html
\
\ ANS Forth Program Label
\
\ This is an ANS Forth program with environmental dependencies
\ requiring the Floating-Point word set.
\
\ Nonstandard and non-core, non-float words:
\
\ \ CORE EXT
\ NEEDS Includes a file if it hasn't been included already.
\ ANEW -- Creates a marker, if it doesn't exist, or forgets
\ everything after it, if it does.
\ TRUE CORE EXT
\ TO CORE EXT
\ FPICK Copies a number from somewhere in the FP stack.
\ FSQRT FLOATING EXT
\ 4DUP 2OVER 2OVER
\ PI 3.1415926535897932384E0
\ FSINCOS FLOATING EXT
\ FCOS FLOATING EXT
\ [IF] TOOLS EXT
\ [THEN] TOOLS EXT
\ S" (interpretive) FILE
\
\ A version of IcosaTest uses many nonstandard words to draw
\ the icosahedron on the screen in Win32Forth. All other compilers
\ ignore the section.
\
\ Environmental dependencies:
\
\ This module assumes that the floating-point stack
\ is separate from the integer stack.
\
\ This module assumes that the floating-point stack is at least 13 floats deep.
\ IcosaTest requires a stack 17 floats deep.
\
\ Ambiguous conditions:
\
\ Attempting to normalize 0 is an ambiguous condition. It will cause
\ division of 0 by 0, which in Win32Forth returns -NAN.
\
\ Calculating the norm of or normalizing a quaternion, where the
\ norm squared exceeds the range of floating-point numbers,
\ is an ambiguous condition. Win32Forth returns infinite for the norm
\ and normalizes such a quaternion to 0.
\
\ Normalizing a quaternion, all of whose elements, when squared,
\ cause floating-point underflow, is an ambiguous condition.
\ Win32Forth returns a quaternion with some or all elements
\ equal to infinity.
\
\ Converting a quaternion whose norm squared exceeds the range of
\ floating-point numbers to a matrix is an ambiguous condition.
\ Win32Forth will return a matrix which may contain one or more
\ elements equal to infinity.
\
\ Converting a matrix which does not represent a rotation, possibly
\ accompanied by a positive stretching, to a quaternion is an
\ ambiguous condition. It may cause attempting to take the square
\ root of a negative number. Win32Forth will ignore and continue.
\
\ Using the word Q followed by fewer than four words, or four words
\ at least one of which is not a valid floating-point number,
\ is an ambiguous condition. Win32Forth aborts with the message:
\ "Error: invalid floating point number".
\
\ Terminal facilities required: Minimal.
\
\ The system is still standard after loading this module.
\
\ NEEDS FSL_Util
MARKER -quaternions-
( Quaternions are stored on the floating-point stack with the real term
on the bottom. )
4 FLOATS CONSTANT QUATERNION ( for declaring arrays: 10 QUATERNION
ARRAY q{ )
: QDUP ( f: q - q q )
3 FPICK 3 FPICK 3 FPICK 3 FPICK ;
: QSWAP ( f: q1 q2 - q2 q1 )
FRAME| a b c d e f g h |
d c b a h g f e |FRAME ;
: QOVER ( f: q1 q2 - q1 q2 q1 )
7 FPICK 7 FPICK 7 FPICK 7 FPICK ;
: Q+ ( f: q q - q )
FRAME| a b c d e f g h |
d h F+ c g F+ b f F+ a e F+
|FRAME ;
: Q- ( f: q q - q )
FRAME| a b c d e f g h |
h d F- g c F- f b F- e a F-
|FRAME ;
: QNEGATE ( f: q - -q )
FRAME| a b c d | d FNEGATE c FNEGATE b FNEGATE a FNEGATE |FRAME ;
: resp* ( f: a b c d e f g h - a b c d e f g h ae bf cg dh )
4 0 DO
3 FPICK 8 FPICK F*
LOOP ;
: QDROP ( f: q ) F2DROP F2DROP ;
: Q* ( f: q q - q )
FRAME| a b c d |
d c b a resp* F+ F+ F- &e F! ( 1 ) QDROP
b a d c resp* F+ F- F- &g F! ( j ) QDROP
c d a b resp* F- F+ F+ &f F! ( i ) QDROP
a b c d resp* F- F- F+ &h F! ( k ) QDROP QDROP
e f g h |FRAME ;
: Q. ( f: q )
FRAME| a b c d |
d F. c F. ." i " b F. ." j " a F. ." k "
|FRAME ;
: Q ( -- ; runtime or interpreted f: f f f f )
POSTPONE % POSTPONE % POSTPONE % POSTPONE % ;
IMMEDIATE
: F>Q ( f: f - q )
0E0 FDUP FDUP ;
: Q>F ( f: q - f )
F2DROP FDROP ;
: V>Q ( f: x y z - q )
FRAME| a b c | 0E0 c b a |FRAME ;
: Q>V ( f: q - x y z )
FRAME| a b c d | c b a |FRAME ;
( Quaternion normalizing )
FVARIABLE FTEMP
: FSQR ( f: f - f ) FDUP F* ;
: NORMSQ ( q - f ) QDUP RESP* F+ F+ F+ FTEMP F!
QDROP QDROP FTEMP F@ ;
: NORM ( q - f ) NORMSQ FSQRT ;
: NORMAL ( f: q ; - ? )
( Note: In some versions of Win32Forth, F~ returns incorrect results. )
NORMSQ 1E0 1E-6 F~ ;
: QF* ( q f - q*f )
FRAME| a b c d e |
e a F* d a F* c a F* b a F* |FRAME ;
: NORMALIZE ( q - q ) QDUP NORM 1E0 FSWAP F/ QF* ;
: QCONJ ( q - q )
FROT FNEGATE FROT FNEGATE FROT FNEGATE ;
: QINV ( q - q )
QCONJ QDUP NORMSQ 1E0 FSWAP F/ QF* ;
: Q/ ( q q - q ) QINV Q* ;
( Quaternions )
: QVARIABLE CREATE 4 FLOATS ALLOT ;
: Q@ ( addr - ; f: - q )
3 FLOATS +
4 0 DO
DUP F@ FLOAT -
LOOP DROP ;
: Q! ( f: q ; addr )
4 0 DO
DUP F! FLOAT+
LOOP DROP ;
: DEGREES ( f: f - f )
PI F* 180E0 F/ ;
: ROLLQ ( f: angle - q )
( Lists to starboard )
F2/ FSINCOS FSWAP 0E0 FDUP ;
: PITCHQ ( f: angle - q )
( Bow goes up )
ROLLQ -FROT ;
: YAWQ ( f: angle - q )
( Hard astarboard )
ROLLQ FROT ;
: }}QUAT>MATRIX ( mat{{ - ; f: q - )
( Converts a quaternion to a matrix of rotation.
The matrix should be 3*3 float. )
FRAME| a b c d | ( note d is the real component )
a a f* &e f!
b b f* &f f!
c c f* &g f!
d d f* &h f!
h g f+ f f- e f- DUP 0 0 }} f!
h g f- f f+ e f- DUP 1 1 }} f! ( These terms mean the object stays in
place. )
h g f- f f- e f+ DUP 2 2 }} f!
a b f* f2* &e f!
c d f* f2* &f f!
e f f- DUP 1 2 }} f! ( The object rotates about the x-axis. )
e f f+ DUP 2 1 }} f!
a c f* f2* &e f!
b d f* f2* &f f!
e f f- DUP 2 0 }} f! ( The object rotates about the y-axis. )
e f f+ DUP 0 2 }} f!
c b f* f2* &e f!
a d f* f2* &f f!
e f f- DUP 0 1 }} f! ( The object rotates about the z-axis. )
e f f+ 1 0 }} f!
|FRAME ;
: }}DET3 ( mat{{ - ; f: - f )
( Computes the determinant of a 3*3 matrix. )
DUP 0 0 }} F@ DUP 1 1 }} F@ DUP 2 2 }} F@ F* F*
DUP 0 1 }} F@ DUP 1 2 }} F@ DUP 2 0 }} F@ F* F* F+
DUP 0 2 }} F@ DUP 1 0 }} F@ DUP 2 1 }} F@ F* F* F+
DUP 0 0 }} F@ DUP 1 2 }} F@ DUP 2 1 }} F@ F* F* F-
DUP 0 2 }} F@ DUP 1 1 }} F@ DUP 2 0 }} F@ F* F* F-
DUP 0 1 }} F@ DUP 1 0 }} F@ 2 2 }} F@ F* F* F- ;
FVARIABLE W^2
FVARIABLE X^2
FVARIABLE Y^2
FVARIABLE Z^2
FVARIABLE 2WX
FVARIABLE 2YZ
FVARIABLE 2WY
FVARIABLE 2XZ
FVARIABLE 2WZ
FVARIABLE 2XY
: UNSUMDIF ( f: a+b a-b - a b )
FOVER F+ F2/ FSWAP FOVER F- ;
: WXYZ-BIGGEST ( - n ; f: - w^2|x^2|y^2|z^2 )
W^2 F@ 0
X^2 F@ F2DUP F< IF
FSWAP 1+
THEN FDROP
Y^2 F@ F2DUP F< IF
FSWAP DROP 2
THEN FDROP
Z^2 F@ F2DUP F< IF
FSWAP DROP 3
THEN FDROP ;
: }}DISSECT ( mat{{ )
( Given a matrix assumed to be of the form
| w^2+x^2-y^2-z^2 2xy-2wz 2xz+2wy |
| 2xy+2wz w^2-x^2+y^2-z^2 2yz-2wx |
| 2xz-2wy 2yz+2wx w^2-x^2-y^2+z^2 |,
as computed by }}QUAT>MATRIX, figures out what 2wx etc. are
and what w^2 etc. should be. )
DUP 1 0 }} F@ DUP 0 1 }} F@ UNSUMDIF 2WZ F! 2XY F!
DUP 2 1 }} F@ DUP 1 2 }} F@ UNSUMDIF 2WX F! 2YZ F!
DUP 0 2 }} F@ DUP 2 0 }} F@ UNSUMDIF 2WY F! 2XZ F!
DUP }}DET3 FABS 1E0 3E0 F/ F** ( this should be w^2+x^2+y^2+z^2 )
FDUP DUP 0 0 }} F@ F+ DUP 1 1 }} F@ F+ DUP 2 2 }} F@ F+ F2/ F2/ W^2 F!
FDUP DUP 0 0 }} F@ F+ DUP 1 1 }} F@ F- DUP 2 2 }} F@ F- F2/ F2/ X^2 F!
FDUP DUP 0 0 }} F@ F- DUP 1 1 }} F@ F+ DUP 2 2 }} F@ F- F2/ F2/ Y^2 F!
DUP 0 0 }} F@ F- DUP 1 1 }} F@ F- 2 2 }} F@ F+ F2/ F2/ Z^2
F! ;
: }}MATRIX>QUAT ( mat{{ ; f: - q )
( If mat{{ was produced from q by }}QUAT>MATRIX, this will return
either q or its additive inverse. If mat{{ is not orthogonal,
this will return garbage. )
}}DISSECT
WXYZ-BIGGEST FSQRT CASE
0 OF
2WX F@ FOVER F/ F2/
2WY F@ 2 FPICK F/ F2/
2WZ F@ 3 FPICK F/ F2/ ENDOF
1 OF
2WX F@ FOVER F/ F2/ FSWAP
2XY F@ FOVER F/ F2/
2XZ F@ 2 FPICK F/ F2/ ENDOF
2 OF
2WY F@ FOVER F/ F2/ FSWAP
2XY F@ FOVER F/ F2/ FSWAP
2YZ F@ FOVER F/ F2/ ENDOF
3 OF
2WZ F@ FOVER F/ F2/ FSWAP
2XZ F@ FOVER F/ F2/ FSWAP
2YZ F@ FOVER F/ F2/ FSWAP ENDOF
ENDCASE ;
: }}MAT*VEC ( addr{{ ; f: x y z - x' y' z' )
FRAME| a b c |
3 0 DO
DUP I 2 }} F@ a F* DUP I 1 }} F@ b F* DUP I 0 }} F@ c F* F+ F+
LOOP DROP |FRAME ;
: QROTATE ( q x y z - x' y' z' )
V>Q QOVER QCONJ Q* Q* Q>V ;
TEST-CODE? [IF]
3 3 FLOAT matrix orient{{
orient{{ Q .5 .5 .5 .5 }}QUAT>MATRIX
CR .( This should print)
CR .( 0 0 1)
CR .( 1 0 0)
CR .( 0 1 0) CR
3 3 orient{{ }}fprint
CR .( This should print 1 0 i 0 j 0 k ) CR
Q 1 2 3 4 QDUP Q/ Q.
CR .( This should print .707 .707 0 ) CR
45E0 DEGREES YAWQ orient{{ }}QUAT>MATRIX
1E0 0E0 0E0 orient{{ }}MAT*VEC FROT F. FSWAP F. F. CR
45E0 DEGREES YAWQ 1E0 0E0 0E0 QROTATE FROT F. FSWAP F. F. CR
: TwistTest ( f: q )
QDUP NORMALIZE orient{{ }}QUAT>MATRIX
3 3 orient{{ }}fprint
orient{{ }}MAT*VEC Q. ;
: 2FLIP ( f: q - q ) Q 0 1 0 0 QSWAP Q* ;
: 3TWIST ( f: q - q ) Q .5 .5 .5 .5 QSWAP Q* ;
\ : 5TWIST ( about .85067 .52573 0, which is a corner of
\ an icosahedron ) Q .809016994374947416 .5 .309016994374947416 0
\ Q* ;
: 5TWIST ( f: q - q )
36E0 DEGREES FCOS 5E-1 F2DUP F- 0E0 QSWAP Q* ;
( Roundoff error in 8-byte float arithmetic
after executing 5twist five times, versus digits after 7494:
7456 6.4E-16
7444 6.4E-16
7432 6.4E-16
74 -3.84E-16
7416 4.8E-16
)
: MEXIT Q 0 0 0 0 Q* ;
: IcosaTest
." This is a test of roundoff error in quaternion multiplication" CR
." by rotating an icosahedron." CR
." Hit any of 2, 3, 5, or Q"
Q 1 0 0 0
BEGIN
KEY CASE
[CHAR] 2 OF 2FLIP ENDOF
[CHAR] 3 OF 3TWIST ENDOF
[CHAR] 5 OF 5TWIST ENDOF
[CHAR] Q OF MEXIT ENDOF
ENDCASE
CR QDUP Q.
QDUP NORM F0=
UNTIL QDROP ;
CR
..( Tests available: ) CR
..( IcosaTest Twiddle an icosahedron to test roundoff error. ) CR
S" TwistTest ( q - ) Turns the vector part of a quaternion about its
" TYPE CR
..( corresponding matrix. ) CR
[THEN] ( if test )