ARKit – Transform matrices, quaternions and related conversions

This post explains how to properly convert transform matrices to Euler Angles and vice versa.

The following story is common to many ARKit or Unity 3D developer.
You create some 3D object, you script its position, quite easy.
Then you need to rotate it along some axis, and add some offset to the result. Or you need to evaluate some angle between two transforms. You manage to do it after digging through some documentation but you still feel like you are lacking some understanding of the underlying concepts.

Understanding the standard structures or 3D rendering engines such as transform, quaternions and Euler angles will help you to visualize rotations easily and to capture general concepts useful for Unity3D, ARKit, OpenGL and so on.

What is a Transform ?

A transform is basically a mathematical object that holds all transformations that has been applied to some 3D object to move it from the origin of your 3D space to a specific Scale, Rotation and Translation. Note that to move an object from point A to point B you must first scale it, then rotate it, then translate it. Performing operations in a different order will lead to different results. Thus, you can always write something like : TB = T * R * S * TA to move a transform to a different state.

To store position (Vector3), rotation (Vector4, even if we rotate in 3D space we need a fourth dimension to represent rotation around an axis), and scale (Vector3), we use a 4 x 4 matrix [M11, M12, …, M21, M22, …, M41, …, M44] built this way :

- The inner 3 x 3 matrix [M11, ..., M33] is a rotation matrix, we will extract quaternions from it later
- The easy part [M41, M42, M43] is the position vector

Extracting the quaternion

A quaternion is a mathematical object that extends the concept of complex numbers :
To represent some quantity in one dimension we can use a real number then to represent a quantity in 2D space we can use a complex number q0 + i*q1 and rotations are simple in this space, multiply by i you get a 90° rotation (-q1 + i*q0). In 3D, you need 2 more dimensions (which can seem counter-intuitive) :

q = w + ix + jy + kz

This quaternion represents a rotation and is stored in the rotation matrix.
It can be converted this way: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

Deducing Euler Angles

This is simply some cosines to move from the quaternion to Euler Angles, you also have to deal with some edge cases (north, south poles).
See : http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
Euler angles are usually ordered this way : (pitch, yaw, bank)

Applying it to ARKit

Here is a simple Swift Extension to get relevant data from a matrix_float_4_4 (the same principles can be applied to SCNVector4) which does not provide any conversion methods :

import SceneKit
import ARKit

public extension matrix_float4x4 {

/// Retrieve translation from a quaternion matrix
public var translation: SCNVector3 {
    get {
        return SCNVector3Make(columns.3.x, columns.3.y, columns.3.z)
    }
}

/// Retrieve euler angles from a quaternion matrix
public var eulerAngles: SCNVector3 {
    get {
        //first we get the quaternion from m00...m22
        //see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
        let qw = sqrt(1 + self.columns.0.x + self.columns.1.y + self.columns.2.z) / 2.0
        let qx = (self.columns.2.y - self.columns.1.z) / (qw * 4.0)
        let qy = (self.columns.0.z - self.columns.2.x) / (qw * 4.0)
        let qz = (self.columns.1.x - self.columns.0.y) / (qw * 4.0)

        //then we deduce euler angles with some cosines
        //see https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
        // roll (x-axis rotation)
        let sinr = +2.0 * (qw * qx + qy * qz)
        let cosr = +1.0 - 2.0 * (qx * qx + qy * qy)
        let roll = atan2(sinr, cosr)

        // pitch (y-axis rotation)
        let sinp = +2.0 * (qw * qy - qz * qx)
        var pitch: Float
        if fabs(sinp) >= 1 {
             pitch = copysign(Float.pi / 2, sinp)
        } else {
            pitch = asin(sinp)
        }

        // yaw (z-axis rotation)
        let siny = +2.0 * (qw * qz + qx * qy)
        let cosy = +1.0 - 2.0 * (qy * qy + qz * qz)
        let yaw = atan2(siny, cosy)

        return SCNVector3(pitch, yaw, roll)
    }
}
}