ARKit and CoreLocation: Part Two

Navigation With Linear Algebra (and Trig)

Published in

Journey Of One Thousand Apps

7 min readAug 30, 2017

Demo CodeARKit and CoreLocation: Part OneARKit and CoreLocation: Part TwoARKit and CoreLocation: Part Three

Maths and Calculating Bearing Between Coordinates

If you haven’t had a chance, checkout part one first.

Now we need to figure out how to get the bearing (the angle) between two coordinates. Finding the bearing set us up to create a rotation transformation to orient our node in the proper direction.

Definition

radian: The radian is a unit of angular measure defined such that an angle of one radian subtended from the center of a unit circle produces an arc with arc length 1. One radian is equal to 180/π degrees so to convert from radians to degrees, multiply by 180/π.

Haversine

One of the drawbacks of the Haversine formula is that it can get less accurate over longer distances. If we were designing a navigation system for a commercial airliner that might be a problem, but it is unlikely that the distances will be long enough to make a difference for an ARKit demo.

Definition

Azimuth: is a angular measurement on spherical coordinate system.

Spherical triangle solved by the law of haversines — source

If you have two different latitude — longitude values of two different point on earth, then with the help of Haversine Formula, you can easily compute the great-circle distance (The shortest distance between two points on the surface of a Sphere).

— Movable-Type

sin = opposite/hypotenusecos = adjacent/hypotenusetan = opposite/adjacent

atan2: An arctangent or inverse tangent function with two arguments.

tan 30 = 0.577Means: The tangent of 30 degrees is 0.577arctan 0.577 = 30Means: The angle whose tangent is 0.577 is 30 degrees.

Keys

‘R’ is the radius of Earth‘L’ is the longitude‘θ’ is latitude‘β‘ is bearing‘∆‘ is delta / change in

In general, your current heading will vary as you follow a great circle path (orthodrome); the final heading will differ from the initial heading by varying degrees according to distance and latitude (if you were to go from say 35°N,45°E (≈ Baghdad) to 35°N,135°E (≈ Osaka), you would start on a heading of 60° and end up on a heading of 120°!).
This formula is for the initial bearing (sometimes referred to as forward azimuth) which if followed in a straight line along a great-circle arc will take you from the start point to the end point

Formula

β = atan2(X,Y)where, X and Y are two quantities and can be calculated as:X = cos θb * sin ∆LY = cos θa * sin θb — sin θa * cos θb * cos ∆L

Getting Coordinates For Distance

While MKRoute gives us a good framework for building an ARKit navigation experience, the steps along the route can be space far enough apart that it ruins the experience. To mitigate this we need to iterate through our steps and generate coordinate for distance intervals between them.

Given a start point, initial bearing, and distance, this will calculate the destination point and final bearing travelling along a (shortest distance) great circle arc.

‘d‘ being the distance travelled‘R’ is the radius of Earth‘L’ is the longitude‘φ’ is latitude‘θ‘ is bearing (clockwise from north)‘δ‘ is the angular distance d/R

Formula

φ2 = asin( sin φ1 ⋅ cos δ + cos φ1 ⋅ sin δ ⋅ cos θ )L2 = L1 + atan2( sin θ ⋅ sin δ ⋅ cos φ1, cos δ − sin φ1 ⋅ sin φ2 )

Three Dimensional Transformations

matrix × matrix = combined matrixmatrix × coordinate = transformed coordinate

Intuitively, it might seem obvious that three dimensions should be represented in [3x3] matrices ([x, y, z]). However there is an extra matrix row, so three-dimensional graphic use [4x4] matrices: [x, y, z, w].

Really…W?

Yup W. This fourth dimension is called “projective space,” and the coordinates in the projective space are called “homogeneous coordinates.” When w is equal to 1, it does not affect x, y or z because the vector is a position in space. When W=0, the coordinate represents a point at infinity (a vector with infinite length) which is used to represent a direction.

Rotation Matrix

To get our objects points in the right direction we need to implement a rotation transformation.

A rotation transformation rotates a vector around the origin (0,0,0) using a given axis and angle

Most rotations in with 3D graphics and ARKit in particular revolve around the camera transform. However, we’re not concerned about placing our object in relation to the POV, we are interested in placing it in relation to our current location and rotate based on the compass.

Translation Matrix

Rotation and scaling transformation matrices only require three columns. But, in order to do translation, the matrices need to have at least four columns. This is why transformations are often 4x4 matrices. However, a matrix with four columns can not be multiplied with a 3D vector, due to the rules of matrix multiplication. A four-column matrix can only be multiplied with a four-element vector, which is why we often use homogeneous 4D vectors instead of 3D vectors.

Putting It All Together

Combining Matrix Transforms

The order in which you combine your transforms matters. When you combine your transforms you should do so in following order:

Transform = Scaling * Rotation * Translation

SIMD (Single Instruction Multiple Data)

So you may have seen simd_mul operation before in regards to matrices. So what is it? It’s pretty straight forward: simd_mul: single instruction multiple data multiplications. In iOS 8 and OS X Yosemite, Apple tacked on a library called simd for implementing SIMD (single-instruction, multiple-data) arithmetic for scalars, vectors, and matrices.

Enter simd.h: This built-in library gives us a standard interface for working with 2D, 3D, and 4D vector and matrix operations across various processors on OS X and iOS. It automatically falls back to software routines if the CPU doesn't natively support the given operation (for example splitting up a 4-lane vector into two 2-lane operations). It also has the bonus of easily transferring data between the GPU and CPU using Metal.
SIMD is a technology that spans the gap between GPU shaders and old-fashioned CPU instructions, allowing the CPU to issue a single instruction that crunches chunks of data in parallel
— www.russbishop.net

So when you see sims_mul being performed, that’s what it means. One thing you should note: simd_mul performs operations in the right to left order.

Creating Our SCNNode Subclass

The next thing we should do is create our node class. We’ll subclass SCNNode and give it a title property which is a string, an anchor property that is an optional ARAnchor that updates the position when it is set. Finally, we’ll give our BaseNode class a location property which is a CLLocation.

We’ll need to add methods to create the sphere graphics. We’ll implement something similar to the sphere in part one, but modified for our new conditions. Since we only want text over the spheres from MKRouteStep instructions we should create to methods:

When we update our position, we take the anchor’s matrix transform and use the x, y and z values from the last column, which are the values of the position transform.