I need to calculate the shortest distance from a lat/lng GPS point P to a line segment described by 2 other lat/lng GPS points A and B.
'Cross-track distance' helps me to calculate the shortest distance between P and the great circle described by A and B.
However, this is not what I want. I need need the distance between P and the line segment of A-B, not the entire great circle.
I have used the following implementation from http://www.movable-type.co.uk/scripts/latlong.html
Formula: dxt = asin( sin(δ13) ⋅ sin(θ13−θ12) ) ⋅ R
where:
δ13 is (angular) distance from start point to third point
θ13 is (initial) bearing from start point to third point
θ12 is (initial) bearing from start point to end point
R is the earth’s radius
The following images hopefully demonstrate the problem I am trying to solve:
In the first image the Cross-Track distance, indicated by the green line is correct and indeed the shortest distance to the line segment AB.
In the second image the problem with cross-track distance is shown, In this case I would want the shortest distance to be the simple distance AP, but Cross-Track distance gives me the distance indicated by the red line.
How do I change my algoritm to take this into account, or check whether or not point X is within AB. Is it possible to do this computationally? Or is iterative the only possible (expensive) solution? (take N points along AB and calculate the min distance from P to all these points)
For simplicity purposes all lines in the images are straight. In reality, these are minor arcs on a great circle
First, some nomenclature:
Our arc is drawn from p1 to p2.
Our third point is p3.
The imaginary point that intersects the great circle is p4.
p1 is defined by lat1,lon1; p2 by lat2,lon2; etc.
dis12 is the distance from p1 to p2; etc.
bear12 is the bearing from p1 to p2; etc.
dxt is cross-track distance.
dxa is cross-arc distance, our goal!
Notice that the cross-track formula relies on the relative bearing, bear13-bear12
We have 3 cases to deal with.
Case 1: The relative bearing is obtuse. So, dxa=dis13.
Case 2.1: The relative bearing is acute, AND p4 falls on our arc.
So, dxa=dxt.
Case 2.2: The relative bearing is acute,AND p4 falls beyond our arc.
So, dxa=dis23
The algorithm:
Step 1: If relative bearing is obtuse, dxa=dis13
Done!
Step 2: If relative bearing is acute:
2.1: Find dxt.
2.3: Find dis12.
2.4: Find dis14.
2.4: If dis14>dis12, dxa=dis23.
Done!
2.5: If we reach here, dxa=abs(dxt)
MATLAB code:
function [ dxa ] = crossarc( lat1,lon1,lat2,lon2,lat3,lon3 )
%// CROSSARC Calculates the shortest distance in meters
%// between an arc (defined by p1 and p2) and a third point, p3.
%// Input lat1,lon1,lat2,lon2,lat3,lon3 in degrees.
lat1=deg2rad(lat1); lat2=deg2rad(lat2); lat3=deg2rad(lat3);
lon1=deg2rad(lon1); lon2=deg2rad(lon2); lon3=deg2rad(lon3);
R=6371000; %// Earth's radius in meters
%// Prerequisites for the formulas
bear12 = bear(lat1,lon1,lat2,lon2);
bear13 = bear(lat1,lon1,lat3,lon3);
dis13 = dis(lat1,lon1,lat3,lon3);
diff = abs(bear13-bear12);
if diff > pi
diff = 2 * pi - diff;
end
%// Is relative bearing obtuse?
if diff>(pi/2)
dxa=dis13;
else
%// Find the cross-track distance.
dxt = asin( sin(dis13/R)* sin(bear13 - bear12) ) * R;
%// Is p4 beyond the arc?
dis12 = dis(lat1,lon1,lat2,lon2);
dis14 = acos( cos(dis13/R) / cos(dxt/R) ) * R;
if dis14>dis12
dxa=dis(lat2,lon2,lat3,lon3);
else
dxa=abs(dxt);
end
end
end
function [ d ] = dis( latA, lonA, latB, lonB )
%DIS Finds the distance between two lat/lon points.
R=6371000;
d = acos( sin(latA)*sin(latB) + cos(latA)*cos(latB)*cos(lonB-lonA) ) * R;
end
function [ b ] = bear( latA,lonA,latB,lonB )
%BEAR Finds the bearing from one lat/lon point to another.
b=atan2( sin(lonB-lonA)*cos(latB) , ...
cos(latA)*sin(latB) - sin(latA)*cos(latB)*cos(lonB-lonA) );
end
Sample outputs: Demonstrate all cases. See maps below.
>> crossarc(-10.1,-55.5,-15.2,-45.1,-10.5,-62.5)
ans =
7.6709e+05
>> crossarc(40.5,60.5,50.5,80.5,51,69)
ans =
4.7961e+05
>> crossarc(21.72,35.61,23.65,40.7,25,42)
ans =
1.9971e+05
Those same outputs on the map!:
Demonstrates case 1:
Demonstrates case 2.1:
Demonstrates case 2.2:
Credit to: http://www.movable-type.co.uk/scripts/latlong.html
for the formulas
and: http://www.darrinward.com/lat-long/?id=1788764
for generating the map images.
And adding a python translation of Sga's implementation:
def bear(latA, lonA, latB, lonB):
# BEAR Finds the bearing from one lat / lon point to another.
return math.atan2(
math.sin(lonB - lonA) * math.cos(latB),
math.cos(latA) * math.sin(latB) - math.sin(latA) * math.cos(latB) * math.cos(lonB - lonA)
)
def pointToLineDistance(lon1, lat1, lon2, lat2, lon3, lat3):
lat1 = math.radians(lat1)
lat2 = math.radians(lat2)
lat3 = math.radians(lat3)
lon1 = math.radians(lon1)
lon2 = math.radians(lon2)
lon3 = math.radians(lon3)
R = 6378137
bear12 = bear(lat1, lon1, lat2, lon2)
bear13 = bear(lat1, lon1, lat3, lon3)
dis13 = distance( (lat1, lon1), (lat3, lon3)).meters
# Is relative bearing obtuse?
if math.fabs(bear13 - bear12) > (math.pi / 2):
return dis13
# Find the cross-track distance.
dxt = math.asin(math.sin(dis13 / R) * math.sin(bear13 - bear12)) * R
# Is p4 beyond the arc?
dis12 = distance((lat1, lon1), (lat2, lon2)).meters
dis14 = math.acos(math.cos(dis13 / R) / math.cos(dxt / R)) * R
if dis14 > dis12:
return distance((lat2, lon2), (lat3, lon3)).meters
return math.fabs(dxt)
Adding a Java version to wdickerson answer:
public static double pointToLineDistance(double lon1, double lat1, double lon2, double lat2, double lon3, double lat3) {
lat1 = Math.toRadians(lat1);
lat2 = Math.toRadians(lat2);
lat3 = Math.toRadians(lat3);
lon1 = Math.toRadians(lon1);
lon2 = Math.toRadians(lon2);
lon3 = Math.toRadians(lon3);
// Earth's radius in meters
double R = 6371000;
// Prerequisites for the formulas
double bear12 = bear(lat1, lon1, lat2, lon2);
double bear13 = bear(lat1, lon1, lat3, lon3);
double dis13 = dis(lat1, lon1, lat3, lon3);
// Is relative bearing obtuse?
if (Math.abs(bear13 - bear12) > (Math.PI / 2))
return dis13;
// Find the cross-track distance.
double dxt = Math.asin(Math.sin(dis13 / R) * Math.sin(bear13 - bear12)) * R;
// Is p4 beyond the arc?
double dis12 = dis(lat1, lon1, lat2, lon2);
double dis14 = Math.acos(Math.cos(dis13 / R) / Math.cos(dxt / R)) * R;
if (dis14 > dis12)
return dis(lat2, lon2, lat3, lon3);
return Math.abs(dxt);
}
private static double dis(double latA, double lonA, double latB, double lonB) {
double R = 6371000;
return Math.acos(Math.sin(latA) * Math.sin(latB) + Math.cos(latA) * Math.cos(latB) * Math.cos(lonB - lonA)) * R;
}
private static double bear(double latA, double lonA, double latB, double lonB) {
// BEAR Finds the bearing from one lat / lon point to another.
return Math.atan2(Math.sin(lonB - lonA) * Math.cos(latB), Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(lonB - lonA));
}
For 100 - 1000m spherical problems, it is easy to just convert to
cartesian space, using a equirectangular projection.
Then it continues with school mathematics:
Use the function "distance from line segment" which is easy to find ready implemented.
This fucntion uses (and sometimes returns) a relative forward/backward position for the projected point X on the line A,B. The value is
in the interval [0,1] if the projected point is inside the line segment.
it is negative if X is outside before A,
it is >1 if outside after B.
If the relative position is between 0,1 the normal distance is taken, if outside the shorter distance of the both start and line-end points, A,B.
An example of such / or very similar an cartesian implementaion is Shortest distance between a point and a line segment
/**
* Calculates the euclidean distance from a point to a line segment.
*
* #param v the point
* #param a start of line segment
* #param b end of line segment
* #return an array of 2 doubles:
* [0] distance from v to the closest point of line segment [a,b],
* [1] segment coeficient of the closest point of the segment.
* Coeficient values < 0 mean the closest point is a.
* Coeficient values > 1 mean the closest point is b.
* Coeficient values between 0 and 1 mean how far along the segment the closest point is.
*
* #author Afonso Santos
*/
public static
double[]
distanceToSegment( final R3 v, final R3 a, final R3 b )
{
double[] results = new double[2] ;
final R3 ab_ = b.sub( a ) ;
final double ab = ab_.modulus( ) ;
final R3 av_ = v.sub( a ) ;
final double av = av_.modulus( ) ;
if (ab == 0.0) // a and b coincide
{
results[0] = av ; // Distance
results[1] = 0.0 ; // Segment coeficient.
}
else
{
final double avScaProjAb = av_.dot(ab_) / ab ;
final double abCoeficient = results[1] = avScaProjAb / ab ;
if (abCoeficient <= 0.0) // Point is before start of the segment ?
results[0] = av ; // Use distance to start of segment.
else if (abCoeficient >= 1.0) // Point is past the end of the segment ?
results[0] = v.sub( b ).modulus() ; // Use distance to end of segment.
else // Point is within the segment's start/end perpendicular boundaries.
{
if (avScaProjAb >= av) // Test to avoid machine float representation epsilon rounding errors that would result in expection on sqrt.
results[0] = 0.0 ; // a, b and v are colinear.
else
results[0] = Math.sqrt( av * av - avScaProjAb * avScaProjAb ) ; // Perpendicular distance from point to segment.
}
}
return results ;
}
the above method requires cartesian 3D space arguments and you asked to use lat/lon arguments. To do the conversion use
/**
* Calculate 3D vector (from center of earth).
*
* #param latDeg latitude (degrees)
* #param lonDeg longitude (degrees)
* #param eleMtr elevation (meters)
* #return 3D cartesian vector (from center of earth).
*
* #author Afonso Santos
*/
public static
R3
cartesian( final double latDeg, final double lonDeg, final double eleMtr )
{
return versor( latDeg, lonDeg ).scalar( EARTHMEANRADIUS_MTR + eleMtr ) ;
}
For the rest of the 3D/R3 code or how to calculate distance to a path/route/track check
https://sourceforge.net/projects/geokarambola/
Adding an ObjectiveC translation of wdickerson implementation:
#define DEGREES_RADIANS(angle) ((angle) / 180.0 * M_PI)
#define RADIANS_DEGREES(angle) ((angle) / M_PI * 180)
- (double)crossArcFromCoord:(CLLocationCoordinate2D)fromCoord usingArcFromCoord:(CLLocationCoordinate2D)arcCoord1 toArcCoord:(CLLocationCoordinate2D)arcCoord2 {
fromCoord.latitude = DEGREES_RADIANS(fromCoord.latitude);
fromCoord.longitude = DEGREES_RADIANS(fromCoord.longitude);
arcCoord1.latitude = DEGREES_RADIANS(arcCoord1.latitude);
arcCoord1.longitude = DEGREES_RADIANS(arcCoord1.longitude);
arcCoord2.latitude = DEGREES_RADIANS(arcCoord2.latitude);
arcCoord2.longitude = DEGREES_RADIANS(arcCoord2.longitude);
double R = 6371000; // Earth's radius in meters
// Prerequisites for the formulas
double bear12 = [self bearFromCoord:arcCoord1 toCoord:arcCoord2];
double bear13 = [self bearFromCoord:arcCoord1 toCoord:fromCoord];
double dis13 = [self distFromCoord:arcCoord1 toCoord:fromCoord];
double diff = fabs(bear13 - bear12);
if (diff > M_PI) {
diff = 2 * M_PI - diff;
}
// Is relative bearing obtuse?
if (diff > (M_PI/2)) {
return dis13;
}
// Find the cross-track distance
double dxt = asin(sin(dis13 / R) * sin(bear13 - bear12)) * R;
// Is p4 beyond the arc?
double dis12 = [self distFromCoord:arcCoord1 toCoord:arcCoord2];
double dis14 = acos(cos(dis13 / R) / cos(dxt / R)) * R;
if (dis14 > dis12) {
return [self distFromCoord:arcCoord2 toCoord:fromCoord];
}
return fabs(dxt);
}
- (double)distFromCoord:(CLLocationCoordinate2D)coord1 toCoord:(CLLocationCoordinate2D)coord2 {
double R = 6371000;
return acos(sin(coord1.latitude) * sin(coord2.latitude) + cos(coord1.latitude) * cos(coord2.latitude) * cos(coord2.longitude - coord2.longitude)) * R;
}
- (double)bearFromCoord:(CLLocationCoordinate2D)fromCoord toCoord:(CLLocationCoordinate2D)toCoord {
return atan2(sin(toCoord.longitude - fromCoord.longitude) * cos(toCoord.latitude),
cos(fromCoord.latitude) * sin(toCoord.latitude) - (sin(fromCoord.latitude) * cos(toCoord.latitude) * cos(toCoord.longitude - fromCoord.longitude)));
}
Adding a python+numpy implementation (now you can pass your longitudes and latitudes as arrays and compute all your distances simultaneously without loops).
def _angularSeparation(long1, lat1, long2, lat2):
"""All radians
"""
t1 = np.sin(lat2/2.0 - lat1/2.0)**2
t2 = np.cos(lat1)*np.cos(lat2)*np.sin(long2/2.0 - long1/2.0)**2
_sum = t1 + t2
if np.size(_sum) == 1:
if _sum < 0.0:
_sum = 0.0
else:
_sum = np.where(_sum < 0.0, 0.0, _sum)
return 2.0*np.arcsin(np.sqrt(_sum))
def bear(latA, lonA, latB, lonB):
"""All radians
"""
# BEAR Finds the bearing from one lat / lon point to another.
result = np.arctan2(np.sin(lonB - lonA) * np.cos(latB),
np.cos(latA) * np.sin(latB) - np.sin(latA) * np.cos(latB) * np.cos(lonB - lonA)
)
return result
def pointToLineDistance(lon1, lat1, lon2, lat2, lon3, lat3):
"""All radians
points 1 and 2 define an arc segment,
this finds the distance of point 3 to the arc segment.
"""
result = lon1*0
needed = np.ones(result.size, dtype=bool)
bear12 = bear(lat1, lon1, lat2, lon2)
bear13 = bear(lat1, lon1, lat3, lon3)
dis13 = _angularSeparation(lon1, lat1, lon3, lat3)
# Is relative bearing obtuse?
diff = np.abs(bear13 - bear12)
if np.size(diff) == 1:
if diff > np.pi:
diff = 2*np.pi - diff
if diff > (np.pi / 2):
return dis13
else:
solved = np.where(diff > (np.pi / 2))[0]
result[solved] = dis13[solved]
needed[solved] = 0
# Find the cross-track distance.
dxt = np.arcsin(np.sin(dis13) * np.sin(bear13 - bear12))
# Is p4 beyond the arc?
dis12 = _angularSeparation(lon1, lat1, lon2, lat2)
dis14 = np.arccos(np.cos(dis13) / np.cos(dxt))
if np.size(dis14) == 1:
if dis14 > dis12:
return _angularSeparation(lon2, lat2, lon3, lat3)
else:
solved = np.where(dis14 > dis12)[0]
result[solved] = _angularSeparation(lon2[solved], lat2[solved], lon3[solved], lat3[solved])
if np.size(lon1) == 1:
return np.abs(dxt)
else:
result[needed] = np.abs(dxt[needed])
return result
How do I calculate the angle in degrees between the coordinates of two POIs (points of interest) on an iPhone map application?
I'm guessing you try to calculate the degrees between the coordinates of two points of interest (POI).
Calculating the arc of a great circle:
+(float) greatCircleFrom:(CLLocation*)first
to:(CLLocation*)second {
int radius = 6371; // 6371km is the radius of the earth
float dLat = second.coordinate.latitude-first.coordinate.latitude;
float dLon = second.coordinate.longitude-first.coordinate.longitude;
float a = pow(sin(dLat/2),2) + cos(first.coordinate.latitude)*cos(second.coordinate.latitude) * pow(sin(dLon/2),2);
float c = 2 * atan2(sqrt(a),sqrt(1-a));
float d = radius * c;
return d;
}
Another option is to pretend you are on cartesian coordinates (faster but not without error on long distances):
+(float)angleFromCoordinate:(CLLocationCoordinate2D)first
toCoordinate:(CLLocationCoordinate2D)second {
float deltaLongitude = second.longitude - first.longitude;
float deltaLatitude = second.latitude - first.latitude;
float angle = (M_PI * .5f) - atan(deltaLatitude / deltaLongitude);
if (deltaLongitude > 0) return angle;
else if (deltaLongitude < 0) return angle + M_PI;
else if (deltaLatitude < 0) return M_PI;
return 0.0f;
}
If you want the result in degrees instead radians, you have to apply the following conversion:
#define RADIANS_TO_DEGREES(radians) ((radians) * 180.0 / M_PI)
You are calculating the 'Bearing' from one point to another here. There's a whole bunch of formula for that, and lots of other geographic quantities like distance and cross-track error, on this web page:
http://www.movable-type.co.uk/scripts/latlong.html
the formulae are in several formats so you can easily convert to whatever language you need for your iPhone. There's also javascript calculators so you can test your code gets the same answers as theirs.
If the other solutions dont work for you try this:
- (int)getInitialBearingFrom:(CLLocation *)first
to:(CLLocation *)second
{
float lat1 = [self degreesToRad:first.coordinate.latitude];
float lat2 = [self degreesToRad:second.coordinate.latitude];
float lon1 = [self degreesToRad:first.coordinate.longitude];
float lon2 = [self degreesToRad:second.coordinate.longitude];
float dLon = lon2 - lon1;
float y = sin (dLon) * cos (lat2);
float x1 = cos (lat1) * sin (lat2);
float x2 = sin (lat1) * cos (lat2) * cos (dLon);
float x = x1 - x2;
float bearingRadRaw = atan2f (y, x);
float bearingDegRaw = bearingRadRaw * 180 / M_PI;
int bearing = ((int) bearingDegRaw + 360) % 360; // +- 180 deg to 360 deg
return bearing;
}
For final bearing, simply take the initial bearing from the end point to the start point and reverse it (using θ = (θ+180) % 360).
You need these 2 helpers:
-(float)radToDegrees:(float)radians
{
return radians * 180 / M_PI;
}
-(float)degreesToRad:(float)degrees
{
return degrees * M_PI /180;
}
I want to work out the distance between 2 latlon points.
The simple distance formula http://www.purplemath.com/modules/distform.htm is not correct because we are dealing with 2 different measures (lat and lon).
Is there a standard solution to this problem?
use Haversine formula.
see this link http://www.movable-type.co.uk/scripts/latlong.html
try this,
This uses the ‘haversine’ formula to calculate great-circle distances between the two points – that is, the shortest distance over the earth’s surface – giving an ‘as-the-crow-flies’ distance between the points (ignoring any hills!).
Haversine formula:
R = earth’s radius (mean radius = 6,371km)
Δlat = lat2− lat1
Δlong = long2− long1
a = sin²(Δlat/2) + cos(lat1).cos(lat2).sin²(Δlong/2)
c = 2.atan2(√a, √(1−a))
d = R.c
or go with the link,http://www.movable-type.co.uk/scripts/latlong.html
Try this javascript haversine function alongside the torad() helper function, which I use for my map app
function calculateHaversineDistance(lat1x, lon1, lat2x, lon2) {
var R = 6371; // km
var dLat = toRad(lat2x-lat1x);
var dLon = toRad(lon2-lon1);
var lat1 = toRad(lat1x);
var lat2 = toRad(lat2x);
var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.sin(dLon/2) * Math.sin(dLon/2) * Math.cos(lat1) * Math.cos(lat2);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
return R * c;
}
function toRad(x) {
return x * Math.PI / 180;
}
Hope this helps.
I'm working in another iPhone App that uses AR, and I'm creating my own framework, but I'm having trouble trying to get the angle of a second coordinate relative to the current device position, anyone know a good resource that could help me with this?
Thanks in advance!
If the two points are close enough together, and well away from the poles, you can use some simple trig:
float dy = lat2 - lat1;
float dx = cosf(M_PI/180*lat1)*(long2 - long1);
float angle = atan2f(dy, dx);
EDIT: I forgot to mention that latN and longN — and therefore dx and dy — can be in degrees or radians, so long as you don't mix units. angle, however, will always come back in radians. Of course, you can get it back to degrees if you multiply by 180/M_PI.
Here is the android version of this code
import com.google.android.maps.GeoPoint;
public double calculateAngle(GeoPoint startPoint, GeoPoint endPoint) {
double lat1 = startPoint.getLatitudeE6() / 1E6;
double lat2 = endPoint.getLatitudeE6() / 1E6;
double long2 = startPoint.getLongitudeE6() / 1E6;
double long1 = endPoint.getLongitudeE6() / 1E6;
double dy = lat2 - lat1;
double dx = Math.cos(Math.PI / 180 * lat1) * (long2 - long1);
double angle = Math.atan2(dy, dx);
return angle;
}