Quaternions in Common Lisp

QUATERNION is a Common Lisp package implementing 4-d quaternions and along with functionality making it useful to perform calculations involving rotations with multiple frames of reference.

The latest version of the Quaternion package can be obtained here.  [2009-12-03]

Discussion

Quaternions are a form of non-commutative division algebra, first described by the Scottish mathematician William Hamilton in 1743. (He was so impressed by his discovery while walking across the Broom Bridge in Dublin that he carved it's essential formula into one of it's stones with his penknife for fear he would forget it.)

Traditionally, rotations are calculated using trigonometry, usually in the form of rotation matrices. The accuracy of these calculations depends on the angles themselves in a non-linear way. And due to the ambiguity of azimuth at both nadir and zenith elevations, they are susceptible to gimbal lock — the loss of a degree of freedom which occurs when two angle planes become parallel. The inability of a gimballed telescope to track a flying object which passes directly overhead is a classic example.

Compared with rotation matrices and Euler Angle calculations, quaternions are:

  • more compact (4 numbers instead of 9
  • much easier to translate between axis and angles
  • performed using only addition and multiplication
  • less susceptible to rounding errors
  • avoid gimbal lock
  • much easier to achieve smooth motion (see spherical linear interpolation ).

I will begin by following the Wikipedia article, Quaternions and Spatial Rotations , adding code examples to illustrate usage of the package.

Definitions and notation

Quaternions are generalized complex numbers of the form:

    A = w + xi + yj + zk,
with w,x,y,z all real, and i,j,k imaginary units with the following multiplcation rules:
    ii =  jj = kk = -1 
    ij = -ji = k
    jk = -kj = i
    ik = -ki = j

The real part of A is w and the imaginary part of A is the vector sum of all the imaginary terms. A quaternion can be written in condensened form as:

    Re(A) = w,
    Im(A) = v = xi + yj + zk,
       A  = w + v
If the real part is 0, the quaternion is said to be imaginary or pure.

Representing rotations as quaternions

drawing: hypersphere of rotations

Every rotation in R3 can be represented by a vector along the axis of rotation whose length is the degree of rotation. In the figure to the right are two examples, both rotations in the xy-plane. All rotations of 180° lay along the equator, where as the zenith and nadir poles represent 0° of rotation. Horizontal slices through the sphere represent a subset of all horizontal rotations of fixed degree given by its radius. The 'latitude' of the plane will be half the angle of rotation. The 'longitude' represents the axis of rotation.

Rotations along similar vectors in the yz-plane would look just like this, only the sphere would be rotated 90° along the y-axis.

It is easy to see from this representation that every rotation exists in a neighborhood of rotations which are nearly the same, thus they are continuous. This is why there is no gimbal lock.

Let (w,x,y,z) be the coordinates of a rotation by α around the axis u. Define the quaternion q

    q = w + xi + yj + zk
      = w + (x,y,z)
      = cos(α/2) + sin(α/2)u
where u is a unit vector. Let v be the R3 vector
    v = (x,y,z)
Then the following quaternion product
    vr = qvq'
where vr is v rotated around u by angle α. (Please indulge the lame HTML math formatting.) This operation is referred to as conjugation of u by q.

Example

q = (0.996195, 0.000000, 0.000000, 0.087156) is quaternion which rotates by 10° around the positive z axis.
Applied to u = (1 0 0), the x-axis, compute a sequence of rotations of 360°:

 (  w       x       y       z   ) degrees
 ( 1.000,  1.000,  0.000,  0.000),   0.00
 ( 1.000,  0.985,  0.174,  0.000),  10.00
 ( 1.000,  0.940,  0.342,  0.000),  20.00
 ( 1.000,  0.866,  0.500,  0.000),  30.00
 ( 1.000,  0.766,  0.643,  0.000),  40.00
 ( 1.000,  0.643,  0.766,  0.000),  50.00
 ( 1.000,  0.500,  0.866,  0.000),  60.00
 ( 1.000,  0.342,  0.940,  0.000),  70.00
 ( 1.000,  0.174,  0.985,  0.000),  80.00
 ( 1.000,  0.000,  1.000,  0.000),  90.00
 ( 1.000, -0.174,  0.985,  0.000), 100.00
 ( 1.000, -0.342,  0.940,  0.000), 110.00
 ( 1.000, -0.500,  0.866,  0.000), 120.00
 ( 1.000, -0.643,  0.766,  0.000), 130.00
 ( 1.000, -0.766,  0.643,  0.000), 140.00
 ( 1.000, -0.866,  0.500,  0.000), 150.00
 ( 1.000, -0.940,  0.342,  0.000), 160.00
 ( 1.000, -0.985,  0.174,  0.000), 170.00
 ( 1.000, -1.000,  0.000,  0.000), 180.00
 ( 1.000, -0.985, -0.174,  0.000), 190.00
 ( 1.000, -0.940, -0.342,  0.000), 200.00
 ( 1.000, -0.866, -0.500,  0.000), 210.00
 ( 1.000, -0.766, -0.643,  0.000), 220.00
 ( 1.000, -0.643, -0.766,  0.000), 230.00
 ( 1.000, -0.500, -0.866,  0.000), 240.00
 ( 1.000, -0.342, -0.940,  0.000), 250.00
 ( 1.000, -0.174, -0.985,  0.000), 260.00
 ( 1.000, -0.000, -1.000,  0.000), 270.00
 ( 1.000,  0.174, -0.985,  0.000), 280.00
 ( 1.000,  0.342, -0.940,  0.000), 290.00
 ( 1.000,  0.500, -0.866,  0.000), 300.00
 ( 1.000,  0.643, -0.766,  0.000), 310.00
 ( 1.000,  0.766, -0.643,  0.000), 320.00
 ( 1.000,  0.866, -0.500,  0.000), 330.00
 ( 1.000,  0.940, -0.342,  0.000), 340.00
 ( 1.000,  0.985, -0.174,  0.000), 350.00
 ( 1.000,  1.000, -0.000,  0.000), 360.00

Example

In this example a non-trivial vector is rotated around the positive axis along 45° in the xy-plane. q = (0.996195, 0.061628, 0.061628, 0.000000) is quaternion which rotates by 10° around the positive z axis.
Applied to u = (0.0d0 0.7071067811865475d0 0.7071067811865475d0), compute a sequence of rotations of 360°:

Example: rotate #
 in 10.00 degree increments around q:
 (  w       x       y       z   ) degrees
 ( 1.000,  0.000,  0.707,  0.707),   0.00 
 ( 1.000,  0.092,  0.615,  0.783),  10.00 
 ( 1.000,  0.192,  0.515,  0.835),  20.00 
 ( 1.000,  0.297,  0.410,  0.862),  30.00 
 ( 1.000,  0.404,  0.303,  0.863),  40.00 
 ( 1.000,  0.509,  0.198,  0.838),  50.00 
 ( 1.000,  0.610,  0.097,  0.787),  60.00 
 ( 1.000,  0.702,  0.005,  0.712),  70.00 
 ( 1.000,  0.785, -0.077,  0.615),  80.00 
 ( 1.000,  0.854, -0.146,  0.500),  90.00 
 ( 1.000,  0.907, -0.200,  0.370), 100.00 
 ( 1.000,  0.944, -0.237,  0.228), 110.00 
 ( 1.000,  0.963, -0.256,  0.079), 120.00 
 ( 1.000,  0.964, -0.257, -0.071), 130.00 
 ( 1.000,  0.946, -0.239, -0.220), 140.00 
 ( 1.000,  0.910, -0.203, -0.362), 150.00 
 ( 1.000,  0.857, -0.150, -0.493), 160.00 
 ( 1.000,  0.789, -0.081, -0.610), 170.00 
 ( 1.000,  0.707, -0.000, -0.707), 180.00 
 ( 1.000,  0.615,  0.092, -0.783), 190.00 
 ( 1.000,  0.515,  0.192, -0.835), 200.00 
 ( 1.000,  0.410,  0.297, -0.862), 210.00 
 ( 1.000,  0.303,  0.404, -0.863), 220.00 
 ( 1.000,  0.198,  0.509, -0.838), 230.00 
 ( 1.000,  0.097,  0.610, -0.787), 240.00 
 ( 1.000,  0.005,  0.702, -0.712), 250.00 
 ( 1.000, -0.077,  0.785, -0.615), 260.00 
 ( 1.000, -0.146,  0.854, -0.500), 270.00 
 ( 1.000, -0.200,  0.907, -0.370), 280.00 
 ( 1.000, -0.237,  0.944, -0.228), 290.00 
 ( 1.000, -0.256,  0.963, -0.079), 300.00 
 ( 1.000, -0.257,  0.964,  0.071), 310.00 
 ( 1.000, -0.239,  0.946,  0.220), 320.00 
 ( 1.000, -0.203,  0.910,  0.362), 330.00 
 ( 1.000, -0.150,  0.857,  0.493), 340.00 
 ( 1.000, -0.081,  0.789,  0.610), 350.00 
 ( 1.000, -0.000,  0.707,  0.707), 360.00 

Composition of Rotations

Let A and B be two normalized quaternions corresponding to the follwing rotations

     A: s --> s'
     B: s'--> s 
The resulting quaternion
     C: s --> s
depends on the represntation of A and B. If the axes of A and B are both given in the same reference frame s, then
     C = BA
 BAsA'B'=(0.47716727744489523, -0.07869467880737654, -0.68626486895611550, -0.89682976946884000)
  CsC'  =(0.47716727744489530, -0.07869467880737652, -0.68626486895611520, -0.89682976946883990)

Note: if the axis of A is in S but B is in S', then C must be computed differently:

     C = -AB'

Some Applications

image: 5-axes motion head

Controlling Motion of a '5-axis' Machine Head

Industrial 3-axis lasers, plasma-cutters, CNC machines, etc. use and servos and stepper motors to control linear motion along the x,y,z-axes. Two additional axes are often added by mounting a cutting head assembly which can rotate the 'tool' mount around both the z-axis, and an additional rotational axes is rotated away from the z-axes.

Consider a case where θ is defined as rotation of a head assembly around one axes (the z-axis in this case, without loss of generality), and γ is defined as the rotation of the tool holder itself around a second axis held at a fixed angle offset from the first axis. The image to the right shows a the two rotational axes of a commercial 5-axis water-jet cutter.

TBD...