Simplification of a 3D polyline using the Ramer–Douglas–Peucker algorithm in Swift

Simplify.swift

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

class Point3D {
    var x: Float = 0.0
    var y: Float = 0.0
    var z: Float = 0.0
}

class Simplify {

    func simplify(#points:[Point3D], tolerance:Float, highestQuality:Bool) -> [Point3D] {
        let sqTolerance = tolerance * tolerance
        
        var newpoints = points
        
        if !highestQuality {
            newpoints = simplifyRadialDistance(newpoints, sqTolerance: sqTolerance)
        }
        
        newpoints = simplifyDouglasPeucker(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 var i = 1; i < points.count; ++i {
            point = points[i];
            
            if (getSquareDistance(point!, p2:prevPoint) > sqTolerance) {
                newPoints.append(point!);
                prevPoint = point!;
            }
        }
        
        if (prevPoint !== point) {
            newPoints.append(point!);
        }
        
        return newPoints;
    }
    
    
    func simplifyDouglasPeucker(points:[Point3D], sqTolerance:Float) -> [Point3D] {
        
        var bitSet = [Bool](count:points.count, repeatedValue:false)
        
        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) {
            var range = stack.removeAtIndex(stack.count - 1);
            
            var index = -1;
            var maxSqDist:Float = 0
            
            // find index of point with maximum square distance from first and last point
            for (var i = range.first + 1; i < range.last; ++i) {
                var sqDist = getSquareSegmentDistance(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 var index = 0; index < bitSet.count; index++ {
            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
        }
    }
}