ARKit and CoreLocation: Part Two

Sunday. August 20, 2017 - 14 mins

Demo Code

ARKit and CoreLocation: Part One

ARKit and CoreLocation: Part Two

ARKit and CoreLocation: Part Three

Maths and Calculating Bearing Between Coordinates

<figcaption>Source</figcaption>

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.

<figcaption>Source</figcaption>

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/π.

extension Double {
    func toRadians() -> Double {
        return self * .pi / 180.0
    }

    func toDegrees() -> Double {
        return self * 180.0 / .pi
    }
}

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.

<figcaption>Spherical triangle solved by the law of haversines — source</figcaption>

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

<figcaption>Great circle distance — source</figcaption>

sin = opposite/hypotenuse

cos = adjacent/hypotenuse

tan = opposite/adjacent

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

tan 30 = 0.577

Means: The tangent of 30 degrees is 0.577

arctan 0.577 = 30

Means: 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 ∆L

Y = 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 )

<figure class="highlight"><pre><code class="language-swift" data-lang="swift"><span class="k">let</span> <span class="nv">metersPerRadianLat</span><span class="p">:</span> <span class="kt">Double</span> <span class="o">=</span> <span class="mf">6373000.0</span>
<span class="k">let</span> <span class="nv">metersPerRadianLon</span><span class="p">:</span> <span class="kt">Double</span> <span class="o">=</span> <span class="mf">5602900.0</span>

<span class="kd">extension</span> <span class="kt">CLLocationCoordinate2D</span> <span class="p">{</span>

  <span class="c1">// adapted from https://github.com/ProjectDent/ARKit-CoreLocation/blob/master/ARKit%2BCoreLocation/Source/CLLocation%2BExtensions.swift</span>

    <span class="kd">func</span> <span class="nf">coordinate</span><span class="p">(</span><span class="n">with</span> <span class="nv">bearing</span><span class="p">:</span> <span class="kt">Double</span><span class="p">,</span> <span class="n">and</span> <span class="nv">distance</span><span class="p">:</span> <span class="kt">Double</span><span class="p">)</span> <span class="o">-&gt;</span> <span class="kt">CLLocationCoordinate2D</span> <span class="p">{</span>

        <span class="k">let</span> <span class="nv">distRadiansLat</span> <span class="o">=</span> <span class="n">distance</span> <span class="o">/</span> <span class="n">metersPerRadianLat</span>  <span class="c1">// earth radius in meters latitude</span>
        <span class="k">let</span> <span class="nv">distRadiansLong</span> <span class="o">=</span> <span class="n">distance</span> <span class="o">/</span> <span class="n">metersPerRadianLon</span> <span class="c1">// earth radius in meters longitude</span>

        <span class="k">let</span> <span class="nv">lat1</span> <span class="o">=</span> <span class="k">self</span><span class="o">.</span><span class="n">latitude</span><span class="o">.</span><span class="nf">toRadians</span><span class="p">()</span>
        <span class="k">let</span> <span class="nv">lon1</span> <span class="o">=</span> <span class="k">self</span><span class="o">.</span><span class="n">longitude</span><span class="o">.</span><span class="nf">toRadians</span><span class="p">()</span>

        <span class="k">let</span> <span class="nv">lat2</span> <span class="o">=</span> <span class="nf">asin</span><span class="p">(</span><span class="nf">sin</span><span class="p">(</span><span class="n">lat1</span><span class="p">)</span> <span class="o">*</span> <span class="nf">cos</span><span class="p">(</span><span class="n">distRadiansLat</span><span class="p">)</span> <span class="o">+</span> <span class="nf">cos</span><span class="p">(</span><span class="n">lat1</span><span class="p">)</span> <span class="o">*</span> <span class="nf">sin</span><span class="p">(</span><span class="n">distRadiansLat</span><span class="p">)</span> <span class="o">*</span> <span class="nf">cos</span><span class="p">(</span><span class="n">bearing</span><span class="p">))</span>
        <span class="k">let</span> <span class="nv">lon2</span> <span class="o">=</span> <span class="n">lon1</span> <span class="o">+</span> <span class="nf">atan2</span><span class="p">(</span><span class="nf">sin</span><span class="p">(</span><span class="n">bearing</span><span class="p">)</span> <span class="o">*</span> <span class="nf">sin</span><span class="p">(</span><span class="n">distRadiansLong</span><span class="p">)</span> <span class="o">*</span> <span class="nf">cos</span><span class="p">(</span><span class="n">lat1</span><span class="p">),</span> <span class="nf">cos</span><span class="p">(</span><span class="n">distRadiansLong</span><span class="p">)</span> <span class="o">-</span> <span class="nf">sin</span><span class="p">(</span><span class="n">lat1</span><span class="p">)</span> <span class="o">*</span> <span class="nf">sin</span><span class="p">(</span><span class="n">lat2</span><span class="p">))</span>

        <span class="k">return</span> <span class="kt">CLLocationCoordinate2D</span><span class="p">(</span><span class="nv">latitude</span><span class="p">:</span> <span class="n">lat2</span><span class="o">.</span><span class="nf">toDegrees</span><span class="p">(),</span> <span class="nv">longitude</span><span class="p">:</span> <span class="n">lon2</span><span class="o">.</span><span class="nf">toDegrees</span><span class="p">())</span>
    <span class="p">}</span>   
<span class="p">}</span></code></pre></figure>


### Three Dimensional Transformations

matrix × matrix = combined matrix

matrix × 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,&nbsp;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&nbsp;_angle_

![](https://cdn-images-1.medium.com/max/204/1*71mr0tiJmZNpy3VJCWczpw.png)


<figure class="highlight"><pre><code class="language-swift" data-lang="swift"><span class="kd">import</span> <span class="kt">GLKit</span><span class="o">.</span><span class="kt">GLKMatrix4</span>
<span class="kd">import</span> <span class="kt">SceneKit</span>

<span class="kd">class</span> <span class="kt">MatrixHelper</span> <span class="p">{</span>

    <span class="c1">//    column 0  column 1  column 2  column 3</span>
    <span class="c1">//        cosθ      0       sinθ      0    </span>
    <span class="c1">//         0        1         0       0    </span>
    <span class="c1">//       −sinθ      0       cosθ      0    </span>
    <span class="c1">//         0        0         0       1    </span>

    <span class="kd">static</span> <span class="kd">func</span> <span class="nf">rotateAroundY</span><span class="p">(</span><span class="n">with</span> <span class="nv">matrix</span><span class="p">:</span> <span class="n">matrix_float4x4</span><span class="p">,</span> <span class="k">for</span> <span class="nv">degrees</span><span class="p">:</span> <span class="kt">Float</span><span class="p">)</span> <span class="o">-&gt;</span> <span class="n">matrix_float4x4</span> <span class="p">{</span>
        <span class="k">var</span> <span class="nv">matrix</span> <span class="p">:</span> <span class="n">matrix_float4x4</span> <span class="o">=</span> <span class="n">matrix</span>

        <span class="n">matrix</span><span class="o">.</span><span class="n">columns</span><span class="o">.</span><span class="mi">0</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="nf">cos</span><span class="p">(</span><span class="n">degrees</span><span class="p">)</span>
        <span class="n">matrix</span><span class="o">.</span><span class="n">columns</span><span class="o">.</span><span class="mi">0</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="o">-</span><span class="nf">sin</span><span class="p">(</span><span class="n">degrees</span><span class="p">)</span>

        <span class="n">matrix</span><span class="o">.</span><span class="n">columns</span><span class="o">.</span><span class="mi">2</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="nf">sin</span><span class="p">(</span><span class="n">degrees</span><span class="p">)</span>
        <span class="n">matrix</span><span class="o">.</span><span class="n">columns</span><span class="o">.</span><span class="mi">2</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="nf">cos</span><span class="p">(</span><span class="n">degrees</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">matrix</span><span class="o">.</span><span class="n">inverse</span>
    <span class="p">}</span>
<span class="p">}</span></code></pre></figure>


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&nbsp;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&nbsp;vectors.

![](https://cdn-images-1.medium.com/max/221/1*9XT1QlNvlvjOS9VGOeydpg.png)


<figure class="highlight"><pre><code class="language-swift" data-lang="swift"><span class="kd">class</span> <span class="kt">MatrixHelper</span> <span class="p">{</span>

    <span class="c1">//    column 0  column 1  column 2  column 3</span>
    <span class="c1">//         1        0         0       X          x        x + X*w </span>
    <span class="c1">//         0        1         0       Y      x   y    =   y + Y*w </span>
    <span class="c1">//         0        0         1       Z          z        z + Z*w </span>
    <span class="c1">//         0        0         0       1          w           w    </span>

    <span class="kd">static</span> <span class="kd">func</span> <span class="nf">translationMatrix</span><span class="p">(</span><span class="nv">translation</span> <span class="p">:</span> <span class="n">vector_float4</span><span class="p">)</span> <span class="o">-&gt;</span> <span class="n">matrix_float4x4</span> <span class="p">{</span>
        <span class="k">var</span> <span class="nv">matrix</span> <span class="o">=</span> <span class="n">matrix_identity_float4x4</span>
        <span class="n">matrix</span><span class="o">.</span><span class="n">columns</span><span class="o">.</span><span class="mi">3</span> <span class="o">=</span> <span class="n">translation</span>
        <span class="k">return</span> <span class="n">matrix</span>
    <span class="p">}</span>
<span class="p">}</span></code></pre></figure>


### Putting It All&nbsp;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.

import SceneKit
import ARKit
import CoreLocation

class BaseNode: SCNNode {

    let title: String

     var anchor: ARAnchor? {
        didSet {
            guard let transform = anchor?.transform else { return }
            self.position = positionFromTransform(transform)
        }
    }

    var location: CLLocation!

    init(title: String, location: CLLocation) {
        self.title = title
        super.init()
        let billboardConstraint = SCNBillboardConstraint()
        billboardConstraint.freeAxes = SCNBillboardAxis.Y
        constraints = [billboardConstraint]
    }

    required init?(coder aDecoder: NSCoder) {
        fatalError("init(coder:) has not been implemented")
    }
}

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:

import SceneKit
import ARKit
import CoreLocation

class BaseNode: SCNNode {

  // Basic sphere graphic

   func createSphereNode(with radius: CGFloat, color: UIColor) -> SCNNode {
        let geometry = SCNSphere(radius: radius)
        geometry.firstMaterial?.diffuse.contents = color
        let sphereNode = SCNNode(geometry: geometry)
        let trailEmitter = createTrail(color: color, geometry: geometry)
        addParticleSystem(trailEmitter)
        return sphereNode
   }

  // Add graphic as child node - basic

   func addSphere(with radius: CGFloat, and color: UIColor) {
        let sphereNode = createSphereNode(with: radius, color: color)
        addChildNode(sphereNode)
    }

   // Add graphic as child node - with text

    func addNode(with radius: CGFloat, and color: UIColor, and text: String) {
        let sphereNode = createSphereNode(with: radius, color: color)
        let newText = SCNText(string: title, extrusionDepth: 0.05)
        newText.font = UIFont (name: "AvenirNext-Medium", size: 1)
        newText.firstMaterial?.diffuse.contents = UIColor.red
        let _textNode = SCNNode(geometry: newText)
        let annotationNode = SCNNode()
        annotationNode.addChildNode(_textNode)
        annotationNode.position = sphereNode.position
        addChildNode(sphereNode)
        addChildNode(annotationNode)
    }
}

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.

class BaseNode: SCNNode {

    var anchor: ARAnchor? {
        didSet {
            guard let transform = anchor?.transform else { return }
            self.position = positionFromTransform(transform)
        }
    }

    // Setup

    func positionFromTransform(_ transform: matrix_float4x4) -> SCNVector3 {

           //    column 0  column 1  column 2  column 3
           //         1        0         0       X       
           //         0        1         0       Y      
           //         0        0         1       Z       
           //         0        0         0       1    

        return SCNVector3Make(transform.columns.3.x, transform.columns.3.y, transform.columns.3.z)
    }
}