Check if a point lies on a polyline in swift. Return -1 if it does not or i if it lies between polyline[i] and polyline[i+1]

locationIndexOnEdgeOrPath.swift

import UIKit

struct LatLng {
    var latitude: Double
    var longitude: Double
}

let EARTH_RADIUS = 6371009.0

func hav(_ x: Double) -> Double {
    return sin(x * 0.5) * sin(x * 0.5);
}

func toRadians(_ number: Double) -> Double {
    return number * .pi / 180
}

func havDistance(lat1: Double, lat2: Double, dLng: Double) -> Double {
    return hav(lat1 - lat2) + hav(dLng) * cos(lat1) * cos(lat2)
}

func sinFromHav(_ h: Double) -> Double {
    return 2 * sqrt(h * (1 - h))
}

func havFromSin(_ x: Double) -> Double {
    let x2 = x * x
    return x2 / (1 + sqrt(1 - x2)) * 0.5
}

func sinSumFromHav(x: Double, y: Double) -> Double {
    let a = sqrt(x * (1 - x))
    let b = sqrt(y * (1 - y))
    return 2 * (a + b - 2 * (a * y + b * x))
}


func mercator(_ lat: Double) -> Double {
    return log(tan(lat * 0.5 + .pi/4))
}

func inverseMercator(_ y: Double) -> Double {
     return 2 * atan(exp(y)) - .pi / 2;
}

func sinDeltaBearing(lat1: Double,
                     lng1: Double,
                     lat2: Double,
                     lng2: Double,
                     lat3: Double,
                     lng3: Double) -> Double {
    let sinLat1 = sin(lat1)
    let cosLat2 = cos(lat2)
    let cosLat3 = cos(lat3)
    let lat31 = lat3 - lat1
    let lng31 = lng3 - lng1
    let lat21 = lat2 - lat1
    let lng21 = lng2 - lng1
    let a = sin(lng31) * cosLat3
    let c = sin(lng21) * cosLat2
    let b = sin(lat31) + 2 * sinLat1 * cosLat3 * hav(lng31)
    let d = sin(lat21) + 2 * sinLat1 * cosLat2 * hav(lng21)
    let denom = (a * a + b * b) * (c * c + d * d)
    return denom <= 0 ? 1 : (a * d - b * c) / sqrt(denom)
}

func wrap(n: Double, min: Double, max: Double) -> Double {
    return (n >= min && n < max) ? n : (mod(x: n - min, m: max - min) + min)
}

func mod(x: Double, m: Double) -> Double {
    return (x.truncatingRemainder(dividingBy: m) + m).truncatingRemainder(dividingBy: m)
}

func clamp (x: Double, low: Double, high: Double) -> Double {
    return x < low ? low : (x > high ? high : x)
}

func isOnSegmentGC(_ lat1: Double,
                   lng1: Double,
                   lat2: Double,
                   lng2: Double,
                   lat3: Double,
                   lng3: Double,
                   havTolerance: Double) -> Bool {
    let havDist13 = havDistance(lat1: lat1, lat2: lat3, dLng: lng1 - lng3)
    if havDist13 <= havTolerance {
        return true;
    }
    let havDist23 = havDistance(lat1: lat2, lat2: lat3, dLng: lng2 - lng3)
    if havDist23 <= havTolerance {
        return true
    }
    let sinBearing = sinDeltaBearing(lat1: lat1, lng1: lng1, lat2: lat2, lng2: lng2, lat3: lat3, lng3: lng3)
    let sinDist13 = sinFromHav(havDist13)
    let havCrossTrack = havFromSin(sinDist13 * sinBearing)
    if havCrossTrack > havTolerance {
        return false
    }
    let havDist12 = havDistance(lat1: lat1, lat2: lat2, dLng: lng1 - lng2)
    let term = havDist12 + havCrossTrack * (1 - 2 * havDist12)
    if havDist13 > term || havDist23 > term {
        return false
    }
    if havDist12 < 0.74 {
        return true;
    }
    let cosCrossTrack = 1 - 2 * havCrossTrack
    let havAlongTrack13 = (havDist13 - havCrossTrack) / cosCrossTrack
    let havAlongTrack23 = (havDist23 - havCrossTrack) / cosCrossTrack
    let sinSumAlongTrack = sinSumFromHav(x: havAlongTrack13, y: havAlongTrack23)
    return sinSumAlongTrack > 0
}

func locationIndexOnEdgeOrPath(point: LatLng,
                               poly: [LatLng],
                               closed: Bool,
                               geodesic: Bool,
                               toleranceEarth: Double) -> Int {
    let size = poly.count
    if size == 0 {
        return -1
    }
    let tolerance = toleranceEarth / EARTH_RADIUS
    let havTolerance = hav(tolerance)
    let lat3 = toRadians(point.latitude)
    let lng3 = toRadians(point.longitude)
    let prev = poly[closed ? size - 1 : 0]
    var lat1 = toRadians(prev.latitude)
    var lng1 = toRadians(prev.longitude)
    var idx: Int = 0
    if geodesic {
        for point2 in poly {
            let lat2 = toRadians(point2.latitude);
            let lng2 = toRadians(point2.longitude);
            if isOnSegmentGC(lat1, lng1: lng1, lat2: lat2, lng2: lng2, lat3: lat3, lng3: lng3, havTolerance: havTolerance) {
                return max(0, idx - 1);
            }
            lat1 = lat2
            lng1 = lng2
            idx += 1
        }
    } else {
        let minAcceptable = lat3 - tolerance
        let maxAcceptable = lat3 + tolerance
        var y1 = mercator(lat1)
        let y3 = mercator(lat3)
        var xTry: [Double] = [0, 0, 0]
        for point2 in poly {
            let lat2 = toRadians(point2.latitude)
            let y2 = mercator(lat2)
            let lng2 = toRadians(point2.longitude)
            if (max(lat1, lat2) >= minAcceptable && min(lat1, lat2) <= maxAcceptable) {
                let x2 = wrap(n: lng2 - lng1, min: -.pi, max: .pi)
                let x3Base = wrap(n: lng3 - lng1, min: -.pi, max: .pi)
                xTry[0] = x3Base
                xTry[1] = x3Base + 2 * .pi
                xTry[2] = x3Base - 2 * .pi
                for x3 in xTry {
                    let dy = y2 - y1;
                    let len2 = x2 * x2 + dy * dy
                    let t = len2 <= 0 ? 0 : clamp(x: (x3 * x2 + (y3 - y1) * dy) / len2, low: 0, high: 1)
                    let xClosest = t * x2
                    let yClosest = y1 + t * dy
                    let latClosest = inverseMercator(yClosest)
                    let havDist = havDistance(lat1: lat3, lat2: latClosest, dLng: x3 - xClosest)
                    if havDist < havTolerance {
                        return max(0, idx - 1)
                    }
                }
            }
            lat1 = lat2
            lng1 = lng2
            y1 = y2
            idx += 1
        }
    }
    return -1;
}