simplify.swift

simplify.swift

//: Playground - noun: a place where people can play

import UIKit

var str = "Hello, playground"

//
//  Simplify.swift
//
//  Simplification of a 3D-polyline.
//  A port of https://github.com/hgoebl/simplify-java for Swift
//
//
//  The MIT License (MIT)
//
//  Created by Lachlan Hurst on 10/02/2015.
//  Copyright (c) 2015 Lachlan Hurst.
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
//

import UIKit
import CoreLocation

struct Point3D {
    
    init(x: Float, y: Float, z: Float) {
        self.x = x
        self.y = y
        self.z = z
    }
    
    var x: Float = 0
    var y: Float = 0
    var z: Float = 0
    var t: Date = Date()
    
    //func toLocation() -> Location
    
    func isEqualTo(_ other: Point3D) -> Bool {
        let areEqual =
            self.y == other.y &&
                self.x == other.x &&
                self.t == other.t &&
                self.z == other.z
        
        return areEqual
    }
    
}

class TrackSimplify {
    
    func simplify(points:[Point3D], tolerance:Float, highestQuality:Bool) -> [Point3D] {
        if (points.count < 2) {
            //would crash if number of points is less than 2
            return points
        }
        let sqTolerance = tolerance * tolerance
        
        var newpoints = points
        
        if !highestQuality {
            newpoints = simplifyRadialDistance(points: newpoints, sqTolerance: sqTolerance)
        }
        
        newpoints = simplifyDouglasPeucker(points: newpoints, sqTolerance: sqTolerance)
        
        return newpoints
    }
    
    func simplifyRadialDistance(points:[Point3D], sqTolerance:Float ) -> [Point3D] {
        var point:Point3D? = nil;
        var prevPoint = points[0]
        
        var newPoints:[Point3D] = []
        newPoints.append(prevPoint)
        
        newPoints.append(prevPoint)
        
        for p in points {
            point = p
            if (getSquareDistance(p1: p, p2:prevPoint) > sqTolerance) {
                newPoints.append(p)
                prevPoint = p
            }
        }
        
        if let p = point, !p.isEqualTo(prevPoint) {
            newPoints.append(p)
        }
        
        return newPoints
    }
    
    
    func simplifyDouglasPeucker(points:[Point3D], sqTolerance:Float) -> [Point3D] {
        
        var bitSet = [Bool](repeating:false, count:points.count)
        
        bitSet[0] = true
        bitSet[points.count - 1] = true
        
        var stack:[Range] = []
        let initRange = Range(firstVal: 0, lastVal: points.count - 1)
        stack.append(initRange)
        
        while (stack.count != 0) {
            let range = stack.remove(at: stack.count - 1)
            
            var index = -1
            var maxSqDist:Float = 0
            
            // find index of point with maximum square distance from first and last point
            for i in range.first + 1 ..< range.last {
                let sqDist = getSquareSegmentDistance(p0: points[i], p1: points[range.first], p2: points[range.last])
                
                if (sqDist > maxSqDist) {
                    index = i
                    maxSqDist = sqDist
                }
            }
            
            if (maxSqDist > sqTolerance) {
                bitSet[index] = true;
                
                stack.append(Range(firstVal:range.first, lastVal:index))
                stack.append(Range(firstVal:index, lastVal:range.last))
            }
        }
        
        var newPoints:[Point3D] = []
        for index in 0 ..< bitSet.count {
            if bitSet[index] {
                newPoints.append(points[index])
            }
        }
        
        return newPoints;
    }
    
    func getSquareDistance(p1:Point3D, p2:Point3D) -> Float {
        let dx = p1.x - p2.x
        let dy = p1.y - p2.y
        let dz = p1.z - p2.z
        
        return dx * dx + dy * dy + dz * dz
    }
    
    func getSquareSegmentDistance(p0:Point3D, p1:Point3D, p2:Point3D) -> Float{
        var x1 = p1.x
        var y1 = p1.y
        var z1 = p1.z
        let x2 = p2.x
        let y2 = p2.y
        let z2 = p2.z
        let x0 = p0.x
        let y0 = p0.y
        let z0 = p0.z
        
        var dx = x2 - x1
        var dy = y2 - y1
        var dz = z2 - z1
        
        if (dx != 0.0 || dy != 0.0 || dz != 0.0) {
            let numerator = ((x0 - x1) * dx + (y0 - y1) * dy + (z0 - z1) * dz)
            let denom = (dx * dx + dy * dy + dz * dz)
            let t =  numerator / denom
            
            if (t > 1.0) {
                x1 = x2
                y1 = y2
                z1 = z2
            } else if (t > 0.0) {
                x1 += dx * t
                y1 += dy * t
                z1 += dz * t
            }
        }
        
        dx = x0 - x1
        dy = y0 - y1
        dz = z0 - z1
        
        return dx * dx + dy * dy + dz * dz
    }
    
    class Range{
        var first:Int
        var last:Int
        
        init(firstVal:Int, lastVal:Int) {
            first = firstVal
            last = lastVal
        }
    }
}

let trackPoints = [
    Point3D(x: 0, y: 0, z: 0),
    Point3D(x: 1, y: 0, z: 0),
    Point3D(x: 1, y: 1, z: 0),
    Point3D(x: 10, y: 20, z: 0)
]

let tolerance : Float = 0.000005

var pts = TrackSimplify().simplify(points: trackPoints, tolerance: tolerance, highestQuality: true)


for p in pts {
    print("\(p.x), \(p.y), \(p.z)")
}