LatLngUtils.kt

import com.google.android.gms.maps.model.LatLng


// Compute latitude and longitude from current lat lng,
// using Vincenty's direct formulae https://en.wikipedia.org/wiki/Vincenty's_formulae
fun LatLng.computeLatLngByDistanceAndBearing(distance: Double, bearing: Double): LatLng {

    val PI = Math.PI / 180.0
    val MAXITERS = 20

    // Convert to radians
    val alpha1 = bearing * PI
    val lat1 = latitude * PI
    val lon1 = longitude * PI

    val a = 6378137.0 // WGS84 major axis
    val b = 6356752.3142 // WGS84 semi-major axis
    val f = (a - b) / a

    val cosAlpha1 = Math.cos(alpha1)

    val U1 = Math.atan((1 - f) * Math.tan(lat1))
    val tanU1 = Math.tan(U1)
    val cosU1 = Math.cos(U1)
    val sinU1 = Math.sin(U1)

    val sigma1 = Math.atan2(tanU1, cosAlpha1)
    val sinAlpha1 = Math.sin(alpha1)
    val sinAlpha = cosU1 * sinAlpha1
    val cosSqAlpha = 1 - sinAlpha * sinAlpha
    val uSq = cosSqAlpha * (a * a - b * b) / (b * b)

    val A = 1 + (uSq / 16384) * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    val B = (uSq / 1024) * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))

    var sigma = distance / (b * A)

    var cosSqSigmaM: Double
    var cosSigmaM: Double = 0.0
    var sinSigma: Double = 0.0
    var cosSigma: Double = 0.0
    for (i in 0 until MAXITERS) {
        val sigmaOrig = sigma

        sinSigma = Math.sin(sigma)
        cosSigma = Math.cos(sigma)

        cosSigmaM = Math.cos(2 * sigma1 + sigma)
        cosSqSigmaM = cosSigmaM * cosSigmaM

        val deltaSigma = B * sinSigma *
                (cosSigmaM + (B / 4) * (cosSigma * (-1 + 2 * cosSqSigmaM)
                        - (B / 6)  * cosSigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cosSqSigmaM)))

        sigma = distance / (b * A) + deltaSigma

        if (Math.abs(sigma - sigmaOrig) < 1.0e-12) {
            break
        }
    }

    val lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
            (1 - f) * Math.sqrt(sinAlpha * sinAlpha + Math.pow(sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1, 2.0)))

    val lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1)

    val C = (f / 16) * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))

    val L = lambda - (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cosSigmaM + C * cosSigma * (-1 + 2 * cosSigmaM * cosSigmaM)))

    val lon2 = L + lon1

    // alpha2 is not used currently
//    val alpha2 = Math.atan2(sinAlpha, - (sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1))

    return LatLng(lat2 * 180 / Math.PI, lon2 * 180 / Math.PI)
}