Kawa: Quaternions

Quaternions

Kawa extends the Scheme numeric tower to include quaternions as a proper superset of the complex numbers. Quaternions provide a convenient notation to represent rotations in three-dimensional space, and are therefore commonly found in applications such as computer graphics, robotics, and spacecraft engineering. The Kawa quaternion API is modeled after this with some additions.

A quaternion is a number that can be expressed in the form ‘w+xi+yj+zk’, where w, x, y, and z are real, and i, j, and k are imaginary units satisfying i² = j² = k² = ijk = -1. The magnitude of a quaternion is defined to be its Euclidean norm when viewed as a point in R⁴.

The real–part of a quaternion is also called its ‘scalar’, while the i–part, j–part, and k–part taken together are also called its ‘vector’. A quaternion with zero j–part and k–part is an ordinary complex number. (If the i–part is also zero, then it is a real). A quaternion with zero real–part is called a ‘vector quaternion’.

The reader syntax for number literals has been extended to support both rectangular and polar (hyperspherical) notation for quaternions. The rectangular notation is as above, i.e. w+xi+yj+zk. The polar notation takes the form r@t%u&v, where r is the magnitude, t is the first angle, and u and v are two other angles called the “colatitude” and “longitude”.

The rectangular coordinates and polar coordinates are related by the equations:

w = r * cos t
x = r * sin t * cos u
y = r * sin t * sin u * cos v
z = r * sin t * sin u * sin v

With either notation, zero elements may be omitted.

Procedure: make-rectangular w x

Procedure: make-rectangular w x y z

These procedures construct quaternions from Cartesian coordinates.

Procedure: make-polar r t

Procedure: make-polar r t u v

These procedures construct quaternions from polar coordinates.

Procedure: + q …

Procedure: - q …

Procedure: * q …

Procedure: / q

Procedure: / q₁ q₂ q₃ …

Procedure: expt q₁ q₂

Procedure: exp q

Procedure: log q

Procedure: sqrt q

Procedure: sin q

Procedure: cos q

Procedure: tan q

Procedure: asin q

Procedure: acos q

Procedure: atan q

All of the arithmetic and transcendental functions defined for complex arguments have been extended to support quaternions.

Quaternion multiplication is not commutative, so there are two possible interpretations of (/ q1 q2) which would yield different results: either (* q1 (/ q2)), or (* (/ q2) q1). Division in this implementation has been defined such that (/ q1 q2 ...) is equivalent to (* q1 (/ q2) ...), but it is recommended to use reciprocals (unary /) and multiplication.

Procedure: real-part q

Return the real–part of q.

(real-part 0)          ⇒  0
(real-part -i)         ⇒  0
(real-part 1+2i-3j+4k) ⇒  1

Procedure: imag-part q

Return the i–part of q.

(imag-part 0)          ⇒  0
(imag-part -i)         ⇒  -1
(imag-part 1+2i-3j+4k) ⇒  2

Procedure: magnitude q

Return the Euclidean norm of q. If q is a+bi+cj+dk, then (magnitude q) is (sqrt (apply + (map square (list a b c d))))

Procedure: angle q

Return the angle of q.

The `(kawa quaternions)` module

The following additional functionality is made available by doing one of:

(require 'quaternions) ;; or
(import (kawa quaternions))

Alias: quaternion

An alias for gnu.math.Quaternion, useful for type declarations.

Procedure: quaternion? x

Return #t if x is a quaternion, i.e. an ordinary number, and #f otherwise.

(quaternion? 0)          ⇒  #t
(quaternion? -i)         ⇒  #t
(quaternion? 1+2i-3j+4k) ⇒  #t
(quaternion? 10.0m)      ⇒  #f
(quaternion? "x")        ⇒  #f

Procedure: jmag-part q

Return the j–part of q.

(jmag-part 0)          ⇒  0
(jmag-part -i)         ⇒  0
(jmag-part 1+2i-3j+4k) ⇒  -3

Procedure: kmag-part q

(kmag-part 0)          ⇒  0
(kmag-part -i)         ⇒  0
(kmag-part 1+2i-3j+4k) ⇒  4

Procedure: complex-part q

Return the projection of q into the complex plane: (+ (real-part q) (* +i (imag-part q)))
(complex-part 0)          ⇒  0
(complex-part -i)         ⇒  -1i
(complex-part 1+2i-3j+4k) ⇒  1+2i

Procedure: vector-part q

Return the vector–part of q.

(vector-part 0)          ⇒  0
(vector-part -i)         ⇒  -1i
(vector-part 1+2i-3j+4k) ⇒  +2i-3j+4k

Procedure: unit-quaternion q

Return a quaternion of unit magnitude with the same direction as q. If q is zero, return zero. This is like a 4D version of a signum function.
(unit-quaternion 0)          ⇒  0
(unit-quaternion -i)         ⇒  -1i
(unit-quaternion 1+2i-3j+4k) ⇒  0.18257418583505536+0.3651483716701107i-0.5477225575051661j+0.7302967433402214k

Procedure: unit-vector q

Return the vector–part of q, scaled to have magnitude 1. If the vector–part is zero, then return zero.
(unit-vector 0)          ⇒  0
(unit-vector -i)         ⇒  -1i
(unit-vector 1+2i-3j+4k) ⇒  +0.3713906763541037i-0.5570860145311556j+0.7427813527082074k

Procedure: colatitude q

Return the colatitude of q.

Procedure: longitude q

Return the longitude of q.

Procedure: vector-quaternion? obj

Return #t if obj is a vector quaternion, i.e. a quaternion with zero real–part.

Procedure: make-vector-quaternion x y z

Construct vector quaternion xi+yj+zk. This is equivalent to (make-rectangular 0 x y z).

Procedure: vector-quaternion->list vq

Return a newly allocated list of the x, y, and z components of vq. This is equivalent to (list (imag-part vq) (jmag-part vq) (kmag-part vq)).

Procedure: dot-product q₁ q₂

For two vector quaternions q₁ = ai+bj+ck and q₂ = di+ej+fk, return ad + be + cf. This is equal to the R^3 dot product for vectors (a,b,c) and (d,e,f), and is also equal to (- (real-part (* q1 q2))). It is an error if either q₁ or q₂ has a non-zero real–part.

Procedure: cross-product q₁ q₂

For two vector quaternions q₁ = ai+bj+ck and q₂ = di+ej+fk, return the R^3 cross product for vectors (a,b,c) and (d,e,f), which is equal to (vector-part (* q1 q2)). It is an error if either q₁ or q₂ has a non-zero real–part.

Procedure: conjugate q

Return (+ (real-part q) (* -1 (vector-part q))).
(conjugate 0)          ⇒  0
(conjugate -i)         ⇒  +1i
(conjugate 1+2i-3j+4k) ⇒  1-2i+3j-4k

The `(kawa rotations)` module

The (kawa rotations) library provides a set of functions which use unit quaternions to represent 3D spatial rotations. To use these functions, the library must be imported:

(import (kawa rotations))

These functions normalize their quaternion inputs as needed to be of length 1.

Rotation Representation Conversions

Conversions to and from several alternate representations of rotations are supported.

The set of unit quaternions provides a double covering of all possible 3D rotations: q and -q represent the same rotation. Most other representations also have multiple numerical values which map to the same rotation (for example, the rotation about axis-vec by angle is the same as the rotation about -axis-vec by -angle+2pi). Therefore, these functions do not necessarily act as inverses in the sense of equal?. Furthermore, rotations involve trigonometric functions, so there will typically be some floating point error: (acos (cos 0.1)) returns 0.09999999999999945, which is very close to 0.1 but not exact.

Rotation Matrices

Procedure: quaternion->rotation-matrix q

Procedure: rotation-matrix->quaternion m

The quaternion->rotation-matrix procedure returns a 3x3 rotation matrix representing the same rotation as q. The rotation matrix is instantiated as a SRFI-25 multi-dimensional array backed by an f64vector.

The rotation-matrix->quaternion procedure performs the reverse operation, producing an equivalent unit quaternion for the rotation matrix (multi-dimensional array) m.
(rotation-matrix->quaternion (quaternion->rotation-matrix -1)) ⇒ 1.0

Axis-Angle Representation

Procedure: rotation-axis q

Procedure: rotation-angle q

Procedure: rotation-axis/angle q

The rotation-axis procedure returns the axis of rotation of the quaternion q as a unit-length vector quaternion. If the axis of rotation is not well-defined (the angle of rotation is 0), then +i is arbitrarily chosen as the axis.

The rotation-angle procedure returns the corresponding angle of rotation. Note that this is not the same as the result of the angle procedure.

The rotation-axis/angle procedure returns the rotation axis and angle as multiple values.
(let* ((q 1/2+1/2i+1/2j+1/2k)
       (ar (rotation-angle q))
       (ad (java.lang.Math:toDegrees ar))
       (exact-ad (exact ad)))
  (rationalize exact-ad 1/10)) ⇒ 120

Procedure: make-axis/angle axis-vec angle

Procedure: make-axis/angle axis-x axis-y axis-z angle

The make-axis/angle procedure returns a quaternion representing the given axis/angle rotation. The axis is specified as either a single vector quaternion argument axis-vec, or as three reals axis-x, axis-y, and axis-z.

Procedure: rotx angle

Procedure: roty angle

Procedure: rotz angle

The procedures rotx, roty, and rotz return quaternions representing rotations about the X-, Y-, and Z-axes.

Intrinsic Angle Sets

The intrinsic angle sets represent arbitrary rotations as a sequence of three rotations about coordinate frame axes attached to the rotating body (i.e. the axes rotate with the body).

There are twelve possible angle sets which neatly divide into two groups of six. The six with same first and third axes are also known as “Euler angles”. The six with different first and third axes are also known as “Tait-Bryan angles”.

Procedure: intrinsic-xyx q

Procedure: intrinsic-xzx q

Procedure: intrinsic-yxy q

Procedure: intrinsic-yzy q

Procedure: intrinsic-zxz q

Procedure: intrinsic-zyz q

These functions decompose the rotation represented by q into Euler angles of the given set (XYX, XZX, YXY, YZY, ZXZ, or ZYZ) and returns the three angles as multiple values. The middle angle will be in the range [0,pi]. If it is on the edges of that range (within 1.0E-12 of 0 or pi), such that the first and third axes are colinear, then the first angle will be set to 0.
(intrinsic-zyz (* (rotz 0.3) (roty 0.8) (rotz -0.6))) ⇒ 0.3000000000000001 0.7999999999999999 -0.5999999999999999

Alias: euler-xyx

Alias: euler-xzx

Alias: euler-yxy

Alias: euler-yzy

Alias: euler-zxz

Alias: euler-zyz

Aliases for the corresponding intrinsic- procedures.

Procedure: intrinsic-xyz q

Procedure: intrinsic-xzy q

Procedure: intrinsic-yxz q

Procedure: intrinsic-yzx q

Procedure: intrinsic-zxy q

Procedure: intrinsic-zyx q

These functions decompose the rotation represented by q into Tait-Bryan angles of the given set (XYZ, XZY, YXZ, YZX, ZXY, or ZYX) and returns the three angles as multiple values. The middle angle will be in the range [-pi/2,pi/2]. If it is on the edges of that range, such that the first and third axes are colinear, then the first angle will be set to 0.

Alias: tait-bryan-xyz

Alias: tait-bryan-xzy

Alias: tait-bryan-yxz

Alias: tait-bryan-yzx

Alias: tait-bryan-zxy

Alias: tait-bryan-zyx

Aliases for the corresponding intrinsic- procedures.

Procedure: make-intrinsic-xyx alpha beta gamma

Procedure: make-intrinsic-xzx alpha beta gamma

Procedure: make-intrinsic-yxy alpha beta gamma

Procedure: make-intrinsic-yzy alpha beta gamma

Procedure: make-intrinsic-zxz alpha beta gamma

Procedure: make-intrinsic-zyz alpha beta gamma

These functions return quaternions representing the given Euler angle rotations.

Alias: make-euler-xyx

Alias: make-euler-xzx

Alias: make-euler-yxy

Alias: make-euler-yzy

Alias: make-euler-zxz

Alias: make-euler-zyz

Aliases for the corresponding make-intrinsic- procedures.
(let-values (((a b c) (euler-xyx (make-euler-xyx 1.0 0.0 2.0))))
  (list a b c)) ⇒ (0.0 0.0 3.0)

Procedure: make-intrinsic-xyz alpha beta gamma

Procedure: make-intrinsic-xzy alpha beta gamma

Procedure: make-intrinsic-yxz alpha beta gamma

Procedure: make-intrinsic-yzx alpha beta gamma

Procedure: make-intrinsic-zxy alpha beta gamma

Procedure: make-intrinsic-zyx alpha beta gamma

These functions return quaternions representing the given Tait-Bryan angle rotations.

Alias: make-tait-bryan-xyz

Alias: make-tait-bryan-xzy

Alias: make-tait-bryan-yxz

Alias: make-tait-bryan-yzx

Alias: make-tait-bryan-zxy

Alias: make-tait-bryan-zyx

Aliases for the corresponding make-intrinsic- procedures.

Extrinsic Angle Sets

The extrinsic angle sets represent arbitrary rotations as a sequence of three rotations about fixed-frame axes (i.e. the axes do not rotate with the body).

There are twelve possible extrinsic angle sets, and each is the dual of an intrinsic set. The extrinsic rotation about axes A, B, and C by angles a, b, and c is the same as the intrinsic rotation about axes C, B, and A by angles c, b, and a, with the order of the three axes reversed.

Procedure: extrinsic-xyx q

Procedure: extrinsic-xyz q

Procedure: extrinsic-xzx q

Procedure: extrinsic-zxy q

Procedure: extrinsic-yxy q

Procedure: extrinsic-yxz q

Procedure: extrinsic-yzx q

Procedure: extrinsic-yzy q

Procedure: extrinsic-zxy q

Procedure: extrinsic-zxz q

Procedure: extrinsic-zyx q

Procedure: extrinsic-zyz q

These functions decompose the rotation represented by q into extrinsic angles of the given set and returns the three angles as multiple values.

Procedure: make-extrinsic-xyx gamma beta alpha

Procedure: make-extrinsic-xyz gamma beta alpha

Procedure: make-extrinsic-xzx gamma beta alpha

Procedure: make-extrinsic-xzy gamma beta alpha

Procedure: make-extrinsic-yxy gamma beta alpha

Procedure: make-extrinsic-yxz gamma beta alpha

Procedure: make-extrinsic-yzx gamma beta alpha

Procedure: make-extrinsic-yzy gamma beta alpha

Procedure: make-extrinsic-zxy gamma beta alpha

Procedure: make-extrinsic-zxz gamma beta alpha

Procedure: make-extrinsic-zyx gamma beta alpha

Procedure: make-extrinsic-zyz gamma beta alpha

These functions return quaternions representing the given extrinsic angle rotations.

Alias: rpy

Alias: make-rpy

Aliases for extrinsic-xyz and make-extrinsic-xyz.

(let ((r (make-rpy 0.12 -0.23 0.34)))
  (let-values (((a b c) (tait-bryan-zyx r)))
    (list a b c))) ⇒ (0.3400000000000001 -0.2300000000000001 0.12000000000000002)

Rotation Operations

Procedure: rotate-vector rq vq

Applies the rotation represented by quaternion rq to the vector represented by vector quaternion vq, and returns the rotated vector. This is equivalent to (* rq vq (conjugate rq)) for normalized rq.
(rotate-vector +k +2i)                      ⇒ -2i
(rotate-vector 1/2+1/2i+1/2j+1/2k +i+2j+3k) ⇒ +3.0i+1.0j+2.0k

Procedure: make-rotation-procedure rq

A partial application of rotate-vector. Returns a single-argument procedure which will take a vector quaternion argument and rotate it by rq. The returned procedure closes over both rq and its conjugate, so this will likely be more efficient than rotate-vector at rotating many vectors by the same rotation.