Android Question Get shortest distance from point to line on map

RB Smissaert

Well-Known Member
Licensed User
Longtime User
Have a point and a line on a map and need the shortest distance (in meters) between that point and that line.
The point is set by a latitude, longitude type and the lines is set by 2 of these types.

This seems the simplest way to do this:

For distance up To a few thousands meters I would simplify the issue from sphere To plane. Then, the issue Is pretty simply As a easy triangle calculation can be used:
We have points A And B And look For a distance X To line AB. Then:

Location a;
Location b;
Location x;

double ax = a.distanceTo(x)
double alfa = ((Math.abs(a.bearingTo(b) - a.bearingTo(x))) / 180) * Math.PI
double distance = Math.sin(alfa) * ax

Having some trouble though to translate this to B4A and sofar been unable to get the right result.

For the distance I have this Sub:

B4X:
Public Sub DistVincenty(tLL1 As TMapLatLng, tLL2 As TMapLatLng) As ResumableSub
    'INPUTS: Latitude and Longitude of initial and destination points in decimal format.
    'OUTPUT: Distance between the two points in Meters.
    '
    '=================================================================================
    ' Calculate geodesic distance (in m) between two points specified by latitude/longitude (in numeric [decimal] degrees)
    ' using Vincenty inverse formula for ellipsoids
    '=================================================================================
    ' Code has been ported by lost_species from www.aliencoffee.co.uk to VBA from javascript published at:
    ' http://www.movable-type.co.uk/scripts/latlong-vincenty.html
    ' * from: Vincenty inverse formula - T Vincenty, "Direct and Inverse Solutions of Geodesics on the
    ' *       Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975
    ' *       http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    'Additional Reference: http://en.wikipedia.org/wiki/Vincenty%27s_formulae
    '=================================================================================
    ' Copyright lost_species 2008 LGPL http://www.fsf.org/licensing/licenses/lgpl.html
    '=================================================================================
    ' Code modifications to prevent "Formula Too Complex" errors in Excel (2010) VBA implementation
    ' provided by Jerry Latham, Microsoft MVP Excel Group, 2005-2011
    ' July 23 2011
    '=================================================================================

    Dim low_a As Double
    Dim low_b As Double
    Dim f As Double
    Dim L As Double
    Dim U1 As Double
    Dim U2 As Double
    Dim sinU1 As Double
    Dim sinU2 As Double
    Dim cosU1 As Double
    Dim cosU2 As Double
    Dim lambda As Double
    Dim lambdaP As Double
    Dim iterLimit As Int
    Dim sinLambda As Double
    Dim cosLambda As Double
    Dim sinSigma As Double
    Dim cosSigma As Double
    Dim sigma As Double
    Dim sinAlpha As Double
    Dim cosSqAlpha As Double
    Dim cos2SigmaM As Double
    Dim C As Double
    Dim uSq As Double
    Dim upper_A As Double
    Dim upper_B As Double
    Dim deltaSigma As Double
    Dim s As Double    ' final result, will be returned rounded to 3 decimals (mm).
    'added by JLatham to break up "too Complex" formulas
    'into pieces to properly calculate those formulas as noted below
    'and to prevent overflow errors when using Excel 2010 x64 on Windows 7 x64 systems
    Dim P1 As Double    ' used to calculate a portion of a complex formula
    Dim P2 As Double    ' used to calculate a portion of a complex formula
    Dim P3 As Double    ' used to calculate a portion of a complex formula

    'See http://en.wikipedia.org/wiki/World_Geodetic_System
    'for information on various Ellipsoid parameters for other standards.
    'low_a and low_b in meters
    ' === GRS-80 ===
    ' low_a = 6378137
    ' low_b = 6356752.314245
    ' f = 1 / 298.257223563
    '
    ' === Airy 1830 ===  Reported best accuracy for England and Northern Europe.
    low_a = 6377563.396
    low_b = 6356256.910
    f = 1 / 299.3249646
    '
    ' === International 1924 ===
    ' low_a = 6378388
    ' low_b = 6356911.946
    ' f = 1 / 297
    '
    ' === Clarke Model 1880 ===
    ' low_a = 6378249.145
    ' low_b = 6356514.86955
    ' f = 1 / 293.465
    '
    ' === GRS-67 ===
    ' low_a = 6378160
    ' low_b = 6356774.719
    ' f = 1 / 298.247167

    '=== WGS-84 Ellipsoid Parameters ===
'    low_a = 6378137       ' +/- 2m
'    low_b = 6356752.3142
'    f = 1 / 298.257223563
    '====================================
    L = ToRad(tLL2.fLng - tLL1.fLng)
    U1 = ATan((1 - f) * Tan(ToRad(tLL1.fLat)))
    U2 = ATan((1 - f) * Tan(ToRad(tLL2.fLat)))
    sinU1 = Sin(U1)
    cosU1 = Cos(U1)
    sinU2 = Sin(U2)
    cosU2 = Cos(U2)
    
    lambda = L
    lambdaP = 2 * cPI
    iterLimit = 100    ' can be set as low as 20 if desired.

    Do While (Abs(lambda - lambdaP) > dEPSILON) And (iterLimit > 0)
        iterLimit = iterLimit - 1

        sinLambda = Sin(lambda)
        cosLambda = Cos(lambda)
        sinSigma = Sqrt(Power(cosU2 * sinLambda, 2) + Power(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2))
        If sinSigma = 0 Then
            Return 0  'co-incident points
         End If
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda
        
        '----------------------------------------------------------------------------------
        sigma = ATan2(sinSigma, cosSigma) 'arguments need to be reversed
        'sigma = ATan2(cosSigma, sinSigma) '<<<<<<<<<<<<<<<<<<<<<<<<<<<< original VBA code!
        '----------------------------------------------------------------------------------
        
        sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma
        cosSqAlpha = 1 - sinAlpha * sinAlpha

        If cosSqAlpha = 0 Then    'check for a divide by zero
            cos2SigmaM = 0    '2 points on the equator
        Else
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
        End If
        
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
        lambdaP = lambda

        'the original calculation is "Too Complex" for Excel VBA to deal with
        'so it is broken into segments to calculate without that issue
        'the original implementation to calculate lambda
        'lambda = L + (1 - C) * f * sinAlpha * _
         '(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * (cos2SigmaM ^ 2))))
        'calculate portions
        P1 = -1 + 2 * Power(cos2SigmaM, 2)
        P2 = (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * P1))
        'complete the calculation
        lambda = L + (1 - C) * f * sinAlpha * P2
        
    Loop

    If iterLimit < 1 Then
        Log("iteration limit has been reached, something didn't work.")
        Return 0
    End If
    
    uSq = cosSqAlpha * (Power(low_a, 2) - Power(low_b, 2)) / Power(low_b, 2)

    'the original calculation is "Too Complex" for Excel VBA to deal with
    'so it is broken into segments to calculate without that issue
    'the original implementation to calculate upper_A
    'upper_A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    'calculate one piece of the equation
    P1 = (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    'complete the calculation
    upper_A = 1 + uSq / 16384 * P1

    'oddly enough, upper_B calculates without any issues - JLatham
    upper_B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))

    'the original calculation is "Too Complex" for Excel VBA to deal with
    'so it is broken into segments to calculate without that issue
    'the original implementation to calculate deltaSigma
    'deltaSigma = upper_B * sinSigma * (cos2SigmaM + upper_B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM ^ 2) _
    ' - upper_B / 6 * cos2SigmaM * (-3 + 4 * sinSigma ^ 2) * (-3 + 4 * cos2SigmaM ^ 2)))
    'calculate pieces of the deltaSigma formula
    'broken into 3 pieces to prevent overflow error that may occur in
    'Excel 2010 64-bit version.
    P1 = (-3 + 4 * Power(sinSigma, 2)) * (-3 + 4 * Power(cos2SigmaM, 2))
    P2 = upper_B * sinSigma
    P3 = (cos2SigmaM + upper_B / 4 * (cosSigma * (-1 + 2 * Power(cos2SigmaM, 2)) - upper_B / 6 * cos2SigmaM * P1))
    'complete the deltaSigma calculation
    deltaSigma = P2 * P3

    'calculate the distance
    s = low_b * upper_A * (sigma - deltaSigma)
    
    'round distance to millimeters
    Return s

End Sub

And for the bearing (line direction) I have this Sub (thanks to Klaus):

B4X:
Sub GetDirectionOfTwoLatLngs(tLL1 As TMapLatLng, tLL2 As TMapLatLng) As Double
    
    Dim x1 As Double
    Dim y1 As Double
    Dim x2 As Double
    Dim y2 As Double
    Dim dAngle As Double
    
    x1 = tLL1.fLng * CosD(tLL1.fLat)
    y1 = tLL1.flat
    x2 = tLL2.fLng * CosD(tLL2.fLat)
    y2 = tLL2.flat
    
    dAngle = ATan2D((y1 - y2), (x2 - x1)) 'trigonometric angle 3 o'clock, positive clockwise
    dAngle = dAngle + 90 'angle 12 o'clock, positve clockwise
    dAngle = dAngle + 360 'only positive values
    dAngle = dAngle Mod 360 'values between 0 and 360
    
    Return dAngle
End Sub

Tried various coding, for example:

B4X:
'Supplied map point and both ends of map line
Sub GetDistanceFromRoute(tLLX As TMapLatLng, tLLA As TMapLatLng, tLLB As TMapLatLng) As ResumableSub
    
'   For distance up to a few thousands meters I would simplify the issue from sphere to plane.
'   Then, the issue is pretty simply as an easy triangle calculation can be used:
'    We have points A and B And look for a distance X to line AB. Then:

'    Location a;
'    Location b;
'    Location x;

'    double ax = a.distanceTo(x)
'    double alfa = ((Math.abs(a.bearingTo(b) - a.bearingTo(x))) / 180)    * Math.PI
'    double distance = Math.sin(alfa) * ax

    Dim dDistanceFromRoute As Double
    Dim dBearingDiff As Double
    
    Dim rs As ResumableSub = MapUtilities.DistVincenty(tLLA, tLLX)
    Wait For (rs) Complete (dDistanceLX As Double)
    
    dBearingDiff = (Abs(GetDirectionOfTwoLatLngs(tLLA, tLLB) - GetDirectionOfTwoLatLngs(tLLA, tLLX)) / 180) * dPi
    
    dDistanceFromRoute = Abs(Sin(dBearingDiff)) * dDistanceLX
    
    Return dDistanceFromRoute
    
End Sub

But as said, not got the right results yet.
Any suggestion how this should be coded?

RBS
 

Shelby

Well-Known Member
Licensed User
Longtime User
Of course one of the simple rules of geometry is that a right angle of the discoverable line distance is a right angle to the original line. I see some of that theorem used here. Pythagoras is watching.
 
Upvote 0

RB Smissaert

Well-Known Member
Licensed User
Longtime User
Of course one of the simple rules of geometry is that a right angle of the discoverable line distance is a right angle to the original line. I see some of that theorem used here. Pythagoras is watching.View attachment 167798
Yes, it looks Pythagoras geometry formula is used in this Sub:

B4X:
Sub GetNearestPointOnLineABFromX(tLLA As TMapLatLng, tLLB As TMapLatLng, tLLX As TMapLatLng) As ResumableSub
    
    Dim tLL As TMapLatLng
    Dim ABx As Double = tLLB.fLng - tLLA.fLng
    Dim ABy As Double = tLLB.fLat - tLLA.fLat
    Dim APx As Double = tLLX.fLng - tLLA.fLng
    Dim APy As Double = tLLX.fLat - tLLA.fLat
    
    Dim ab2 As Double = ABx * ABx + ABy * ABy
    Dim ap_ab As Double = APx * ABx + APy * ABy
    
    Dim t As Double = ap_ab / ab2
    
    ' Limit t to correspond to the segment, if you want the segment, not the infinite line
    If t < 0 Then t = 0
    If t > 1 Then t = 1
    
    Dim cx As Double = tLLA.fLng + t * ABx
    Dim cy As Double = tLLA.fLat + t * ABy
    
    tLL.fLat = cy
    tLL.fLng = cx
    
    Return tLL
    
End Sub

Funny thing is that it seems to work OK on lat/lng data, that is when the distances are relatively short.

RBS
 
Upvote 0

TILogistic

Expert
Licensed User
Longtime User
A while ago, with some friends and colleagues, we developed something similar to what you want, it was a solution for hikers where there was no internet coverage, the idea was that the routes would be drawn before doing them and these would be fed with the observations of new trails to follow, this allowed that in case of leaving the route, with the information previously collected by the hikers, the app would indicate which route or trail to follow to resume the route or continue the route suggested by the app, with offline maps.
 
Upvote 0

TILogistic

Expert
Licensed User
Longtime User
? Free
and Other
 
Upvote 0

TILogistic

Expert
Licensed User
Longtime User
yes

see:


and use:
B4X:
' Búsqueda binaria para el punto más cercano en la línea geodésica
Public Sub PuntoMasCercano(latA As Double, lonA As Double, latB As Double, lonB As Double, latP As Double, lonP As Double) As Double()
    Dim invAB As Map = VincentyInverse(latA, lonA, latB, lonB)
    If invAB = Null Then Return Null
    Dim distAB As Double = invAB.Get("distance")
    Dim azimutAB As Double = invAB.Get("initialBearing")

    Dim low As Double = 0
    Dim high As Double = distAB
    Dim tol As Double = 0.1
    Dim bestPoint() As Double = Null
    Dim minDist As Double = 1E9

    Do While (high - low) > tol
        Dim mid As Double = (low + high) / 2
        Dim Q() As Double = VincentyDirect(latA, lonA, azimutAB, mid)
        Dim invQ As Map = VincentyInverse(Q(0), Q(1), latP, lonP)
        Dim distQ As Double = invQ.Get("distance")

        If distQ < minDist Then
            minDist = distQ
            bestPoint = Q
        End If

        Dim Qleft() As Double = VincentyDirect(latA, lonA, azimutAB, (low + mid) / 2)
        Dim invLeft As Map = VincentyInverse(Qleft(0), Qleft(1), latP, lonP)
        Dim distLeft As Double = invLeft.Get("distance")

        Dim Qright() As Double = VincentyDirect(latA, lonA, azimutAB, (mid + high) / 2)
        Dim invRight As Map = VincentyInverse(Qright(0), Qright(1), latP, lonP)
        Dim distRight As Double = invRight.Get("distance")

        If distLeft < distRight Then
            high = mid
        Else
            low = mid
        End If
    Loop

    Return bestPoint
End Sub
 
Upvote 0

RB Smissaert

Well-Known Member
Licensed User
Longtime User
Did you manage to handle situations where a route point was missed or where the user did somehow made a wrong turn? I am still working on this. It is quite complex.

RBS
 
Upvote 0

TILogistic

Expert
Licensed User
Longtime User
Did you manage to handle situations where a route point was missed or where the user did somehow made a wrong turn? I am still working on this. It is quite complex.

RBS
Yes.

Question: The app is for:
1. Route monitoring
2. Urban or rural areas
3. Route deviation tolerance.
4. GPS coverage.
5. GSM coverage
 
Upvote 0

emexes

Expert
Licensed User
Longtime User
seems to work OK on lat/lng data

As you head away from the equator, it is going to become increasingly biased towards preferring line segments (and points on that line) that are north-south rather than east-west.

Eg for a point in London at latitude 51.5 degrees, it is going to prefer a line segment that is 12 metres north (0.00010792 degrees) rather than a line segment that is 9 metres east (0.00013003 degrees) ie it would choose the line segment that is 3 metres further away.

Using the Cos(Latitude) factor to make the longitude and latitude units equal will eliminate this bias.

On the other hand... for small distances, then close enough is good enough: I doubt people are going to complain about walking an extra 3 metres. But if we were talking about 12 km vs 9 km, you might get some people grumbling about the extra 3 km.

Lol sometimes even small discrepancies matter, eg Google Maps routing used to drive me nuts in 2015, where if there was a rear alley at a destination address, then it would often try guide me through some convoluted backstreet route to the rear of the property, rather than to the front door, because the centre of the property was closer to the rear alley. Seems to be fixed now:


 
Last edited:
Upvote 0

TILogistic

Expert
Licensed User
Longtime User
Did you manage to handle situations where a route point was missed or where the user did somehow made a wrong turn? I am still working on this. It is quite complex.

RBS

The most common way to detect if the user has deviated from the route, and with what is already available, is to establish a deviation threshold (meters or kilometers). To do this, using the GPS position (or AVL), the distance of this position must be verified with the route segments.

Example:

B4X:
Sub GPS_LocationChanged(Location1 As Location)
    Dim gpsLat As Double = Location1.Latitude
    Dim gpsLon As Double = Location1.Longitude

    For i = 0 To Route.Size - 2
        Dim p1 As Map = Route.Get(i)
        Dim p2 As Map = Route.Get(i + 1)
        Dim dist As Double = DistancePointToSegment(gpsLat, gpsLon, p1.Get("lat"), p1.Get("lon"), p2.Get("lat"), p2.Get("lon"))
        If dist < minDist Then minDist = dist
    Next

    If minDist > UmbralMeters Then
        Log("Deviation detected. Distance to the nearest segment: " & NumberFormat(minDist, 1, 2) & " meters")
    Else
        Log("En route. Distance to the nearest segment: " & NumberFormat(minDist, 1, 2) & " meters")
    End If
End Sub


B4X:
Private Sub DistancePointToSegment(px As Double, py As Double, Ax As Double, Ay As Double, Bx As Double, By As Double) As Double
    Dim abDist As Double = HaversineDistance(Ax, Ay, Bx, By)
    If abDist = 0 Then Return HaversineDistance(px, py, Ax, Ay)

    Dim t As Double = ((px - Ax) * (Bx - Ax) + (py - Ay) * (By - Ay)) / (abDist * abDist)
 
    If t < 0 Then Return HaversineDistance(px, py, Ax, Ay)
    Else If t > 1 Then Return HaversineDistance(px, py, Bx, By)

    Dim Cx As Double = Ax + t * (Bx - Ax)
    Dim Cy As Double = Ay + t * (By - Ay)

    Return HaversineDistance(px, py, Cx, Cy)
End Sub

Private Sub HaversineDistance(lat1 As Double, lon1 As Double, lat2 As Double, lon2 As Double) As Double
    Dim R As Double = 6371000 ' Terrestrial radius in meters
    Dim dLat As Double = DegToRad(lat2 - lat1)
    Dim dLon As Double = DegToRad(lon2 - lon1)
    Dim a As Double = Sin(dLat / 2) * Sin(dLat / 2) + Cos(DegToRad(lat1)) * Cos(DegToRad(lat2)) * Sin(dLon / 2) * Sin(dLon / 2)
    Dim c As Double = 2 * Atan2(Sqrt(a), Sqrt(1 - a))
    Dim distance As Double = R * c
    Return distance
End Sub

Private Sub DegToRad(Deg As Double) As Double
    Return Deg * cPI / 180
End Sub

Note:
You can also get the approximate point within the segment where the deviation occurs.

One situation you should consider is when the route has curved sections. To do this, you should complement this with other algorithms that detect when the route contains curved sections.

ex.
 
Last edited:
Upvote 0

TILogistic

Expert
Licensed User
Longtime User
@emexes, I still remember your joke, a very good joke, which was true to a certain extent.
 
Upvote 0

RB Smissaert

Well-Known Member
Licensed User
Longtime User
Yes, I should correct for routes close to the equator, but for now this app is only for personal use.
It is actually part of a much larger clinical app for GP's (doctors, general practioners) to have access (offline) to all relevant patient data.
First step would be to separate this map route part of the app from the main app and then I should solve the equator problem.

I had a go though with your Cos(Latitude) factor but wasn't quite sure how it it should be implemented in this Sub:

B4X:
Sub GetNearestPointOnLineABFromX(tLLA As TMapLatLng, tLLB As TMapLatLng, tLLX As TMapLatLng) As ResumableSub
    
    Dim tLL As TMapLatLng
    Dim ABx As Double = tLLB.fLng - tLLA.fLng
    Dim ABy As Double = tLLB.fLat - tLLA.fLat
    Dim APx As Double = tLLX.fLng - tLLA.fLng
    Dim APy As Double = tLLX.fLat - tLLA.fLat
    
    Dim ab2 As Double = ABx * ABx + ABy * ABy
    Dim ap_ab As Double = APx * ABx + APy * ABy
    
    Dim t As Double = ap_ab / ab2
    
    ' Limit t to correspond to the segment, if you want the segment, not the infinite line
    If t < 0 Then t = 0
    If t > 1 Then t = 1
    
    Dim cx As Double = tLLA.fLng + t * ABx
    Dim cy As Double = tLLA.fLat + t * ABy
    
    tLL.fLat = cy
    tLL.fLng = cx
    
    Return tLL
    
End Sub

The question here is how to apply the Cos(Latitude) factor to the final, produced tLL?
As I take it there are 3 different factors here to apply to provided Lng values. Which one (or an average of the 2 or 3)
to apply to the final tLL?
How would you code the above sub?

RBS
 
Upvote 0

RB Smissaert

Well-Known Member
Licensed User
Longtime User
>> The most common way to detect if the user has deviated from the route, and with what is already available, is to establish a deviation threshold (meters or kilometers). To do this, using the GPS position (or AVL), the distance of this position must be verified with the route segments.

Yes, that is exactly what I am doing and that was the essence of the posted question.

>> One situation you should consider is when the route has curved sections.

I am kind of solving that by putting some more route points on the map in that situation.

RBS
 
Upvote 0
Cookies are required to use this site. You must accept them to continue using the site. Learn more…