GPS Conversion - pixel coords to GPS coords - gps

I am doing some movement tracking from video data. Using some video processing (i.e. translating to a top-down view) I have obtained a movement path. I now need to translate the pixel coordinates of the path (x,y) to world coordinates (lat, long).
I have four reference points in the image and their associated latitude and longitude points. (Lat / long -> pixel coordinates):
(51.606733, -3.986813) -> (246, 399)
(51.607357, -3.987126) -> (407, 781)
(51.607337, -3.987266) -> (838, 781)
(51.606671, -3.987175) -> (1247, 402)
How I can transform the pixel coordinates to world coordinates using these reference points?

Problem
GPS projection onto a 2D surface is usually a tricky issue since the Earth's surface is curved, not flat. However, if the sample GPS data you have provided is actual data (hope the weather is pleasant in Swansea, by the way!) I will assume that the data set is confined to a very small area, and therefore you can assume that lines of longitude are parallel. The problem then becomes a simple algebraic one, and you only need two reference points where x1 != x2 and y1 != y2.
Solution
ISO coordinates are given as (latitude, longitude) = (y, x) while plotted coordinates are given as (x, y). I'm just going to show you how to do y (latitude). We need to map the origin of the source (the screen) to the origin of the target (the world), and the scale of the source to the scale of the world. I'm going to name these variables as follows:
screenY0 //Screen origin (pixel corresponding to zero degrees latitude)
worldY0 //World origin (zero degrees latitude)
screenYscale //Screen scale (distance between 2 pixels)
worldYscale //World scale (distance between two latitude lines)
screenYpoint //Screen point (pixel y location)
worldYpoint //World point (latitude on the ground)
I'm going to use the following coordinate pairs because these are the furthest apart:
(51.606733, -3.986813) -> (246, 399)
(51.607337, -3.987266) -> (838, 781)
Our formula is going to look like this:
screenY0 + screenYscale * screenYpoint = worldY0 + worldYscale * worldYpoint.
We know that the world origin is 0, and the world scale is 1, so we can condense this to:
screenY0 + screenYscale * screenYpoint = worldYpoint.
We can plug in our values to form 2 simultaneous equations:
screenY0 + screenYscale * 399 = 51.606733 and screenY0 + screenYscale * 781 = 51.60733
Solving:
screenY0 = 51.606733 - screenYscale * 399 and screenY0 = 51.607337 - screenYscale * 781
=> 51.606733 - screenYscale * 399 = 51.607337 - screenYscale * 781
=> screenYscale * 781 - screenYscale * 399 = 51.607337 - 51.606733
=> screenYscale * 382 = 0.000604
=> screenYscale = 0.00000158115
So each pixel on your map represents 0.00000158115 of a degree of longitude. Plugging in to find the origin:
screenY0 + screenYscale * 399 = 51.606733
=> screenY0 + 0.00000158115 * 399 = 51.606733
=> screenY0 + 0.00063087885 = 51.606733
=> screenY0 = 51.606733 - 0.00063087885
=> screenY0 = 51.6061021212
Therefore the pixel at 0 represents 51.6061021212 in the real world.
Formula
Our formula to find the real world latitude is thus:
51.6061021212 + 0.00000158115 * screenYpoint = worldYpoint.
Testing
Let's test this with your other reference latitude: 51.606671 -> 402
51.6061021212 + 0.00000158115 * screenYpoint = worldYpoint
51.6061021212 + 0.00000158115 * 402 = 51.606671
51.6061021212 + 0.0006356223 = 51.606671
51.6067377435 = 51.606671
This is approximately equal; considering that 1 degree of latitude is equal to 111.2km (at the Earth's mean radius) this corresponds to an error of about 7.4 meters.
Hope this helps and gets you on your way to solving for longitude, AKA x, as well! If you have any trouble or would like me to clarify, please leave a comment.

Related

"Get 100 meters out from" Haversin Formula

I'm interested in working with coordinates, and I was wondering, how is it possible to get the distance between two points (coordinates) in meters. After a long search I found the Haversine Formula, and the Objective-C implementation of it here.
It's the following (I modified it a little bit for myself):
- (CGFloat)directMetersFromCoordinate:(CLLocation *)from toCoordinate:(CLLocation *)to {
static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
static const double EARTH_RADIUS_IN_METERS = 6372797.560856;
double latitudeArc = (from.coordinate.latitude - to.coordinate.latitude) * DEG_TO_RAD;
double longitudeArc = (from.coordinate.longitude - to.coordinate.longitude) * DEG_TO_RAD;
double latitudeH = sin(latitudeArc * 0.5);
latitudeH *= latitudeH;
double lontitudeH = sin(longitudeArc * 0.5);
lontitudeH *= lontitudeH;
double tmp = cos(from.coordinate.latitude*DEG_TO_RAD) * cos(to.coordinate.latitude*DEG_TO_RAD);
return EARTH_RADIUS_IN_METERS * 2.0 * asin(sqrt(latitudeH + tmp*lontitudeH)); }
My question is:
How is it possible to get 100 meters distance (for latitude and for longitude) for the current location?
This formula is too complicated for me, I don't understand it, so I can't "code it back" to get the result what I want.
I need only the actual location (Paris, Tokyo, London, New York, whatever), and a (float) number for latitude and a (float) number for longitude, which (float) number represents 100 meters distance from the actual location.
If you open this page, here you can "calculate out" the 100 meters distance between two points ("actual" and the one 100 meters away).
For example:
Point1: 47.0, 19.0
Point2: 47.0, 19.0013190
---That's 0.1000 km (100 m) distance.
Point1: 47.0, 19.0
Point2: 47.0008995, 19.0
---is also 0.1000 km (100 m) distance.
Here you can see, that at that coordinate (latitude 47.0 and longitude 19.0) 100 meters distance is 0.0008995 (latitude) and 0.0013190 (longitude).
And I want to get these data with the help of the Haversine Formula, just don't know, how.
Could you help me to figure it out?
Thanks!
UPDATE:
Thank you for the answers, right now I don't have time to try them out, but as I understood, I didn't explain exactly what I want.
Maybe this is a better example, how I want to use these "100 meters":
So, right now I'm at the coordinate "lat x" and "lon y". There is another point (let's say a hotel) at another given coordinate, "lat a" and "lon b".
My question is: how can I calculate out, if this hotel is (less than) 100 meters from me? So it doesn't matter, if it's only 5 meters or 99 meters, both of them are less (or equal) than 100 meters far from me.
With the code what I provided, I can calculate this out, that's what that formula is for.
But let's say, I have a million of other coordinates (hotel locations) that I want to work with. And I need only a list of them, what are (less than) 100 meters away from me. So yes, that's a circle around me with a radius of 100 meters, and I need the results within that.
It would take much more time and resource to take all the coordinates of these "million" hotels and calculate the distance one by one, that's why I thought it would be much easier to calculate out, how much 100 meters are in latitude and longitude (changes the value as we are on different locations, that' why I can't use simply the ones what I calculated out in the example above).
So if I would know how much 100 meters are in latitude and longitude for example at London's coordinate (if I'm there), I could simply get the list of the hotels what are (less than) 100 meters far from me, by a simple dividing:
if
((hotelLocation.coordinate.latitude <= (myLocation.coordinate.latitude + "100metersInLatitude")) || (hotelLocation.coordinate.latitude >= (myLocation.coordinate.latitude - "100metersInLatitude")))
&&
((hotelLocation.coordinate.longitude <= (myLocation.coordinate.longitude + "100metersInLongitude")) || (hotelLocation.coordinate.longitude >= (myLocation.coordinate.longitude - "100metersInLongitude")))
{
NSLog(#"Be Happy :-) !");
}
I just need these "100metersInLatitude" and "100metersInLongitude", calculated always from "myLocation".
Whoa, I hope, somebody will understand what I just wrote down, because it's not easy for me, neither... :-)))
Assuming you have a point with latitude and longitude, and you want to find another point that is a distance d on a bearing b, when the distance is small (you said "100 meters" which is very small on the surface of the earth) then you can do a simple approximation - treating the earth's surface locally as "flat". Here is a simple C program that implements this (using the numbers you had above). I updated it to include the "accurate" formulation as well - it's just a few more calculations, but it is accurate at all distances (and not just the short ones). The equation I used came from the link you referenced - subheading "Destination point given distance and bearing from start point"
updated - I moved the accurate calculation into a separate function, and added a loop to compute the new point for all integer bearings from 0 to 359, printing out every 30th. This give you the "circle" I talked about in my initial comment.
#include <stdio.h>
#include <math.h>
double radians(double x) {
return acos(0.0) * x / 90.0;
}
void calcNewPosition(double lat, double lon, double bearing, double d, double *newLat, double *newLon) {
double lat1, lon1, br, pi;
double Re = 6371000;
// convert everything to radians first:
lat1 = radians(lat);
lon1 = radians(lon);
br = radians(bearing);
pi = 2 * acos(0.0);
double lat2, lon2;
lat2 = asin( sin(lat1) * cos(d/Re) +
cos( lat1 ) * sin( d / Re ) * cos(br ) );
lon2 = lon1 + atan2(sin(br) * sin( d / Re ) * cos( lat1 ), \
cos( d / Re ) - sin(lat1 ) * sin( lat2 ) );
*newLat = 180. * lat2 / pi;
*newLon = 180. * lon2 / pi;
}
int main(void) {
double lon = 47., lat=19.;
double newLongitude, newLatitude;
double dx, dy, dLong, dLat;
double Re = 6371000, d = 100, bearing = 0.0;
double pi;
double lat1, lon1, br;
// convert everything to radians first:
lat1 = radians(lat);
lon1 = radians(lon);
br = radians(bearing);
pi = 2 * acos(0.0);
// approximate calculation - using equirectangular approximation
// and noting that distance between meridians (lines of longitude)
// get closer at higher latitudes, with cos(latitude).
dx = d * sin(br); // distance in E-W direction
dy = d * cos(br); // distance in N-S direction
dLat = 360 * dy / (2.0 * pi * Re); // convert N-S to degrees latitude
dLong = 360 * dx / (2.0 * pi * Re * cos(lat1)); // convert E-W to degrees longitude
newLatitude = lat + dLat;
newLongitude = lon + dLong;
printf("simple forumula: the new position is %.8lf lon, %.8lf lat\n", newLongitude, newLatitude);
// more accurate formula: based on http://www.movable-type.co.uk/scripts/latlong.html
double lat2, lon2;
calcNewPosition(lat, lon, bearing, d, &lat2, &lon2);
printf("more accurate: the new position is %.8lf lon, %.8lf lat\n", lon2, lat2);
// now loop over all bearings and compute the "circle of points":
int iBearing;
double lonArray[360], latArray[360];
for(iBearing = 0; iBearing < 360; iBearing++) {
calcNewPosition(lat, lon, (double)iBearing, d, &latArray[iBearing], &lonArray[iBearing]);
if (iBearing % 30 == 0) printf("bearing %03d: new lat = %.8lf, new lon = %.8lf\n", iBearing, latArray[iBearing], lonArray[iBearing]);
}
return 0;
}
The output of this is
simple forumula: the new position is 47.00000000 lon, 19.00089932 lat
more accurate: the new position is 47.00000000 lon, 19.00089932 lat
bearing 000: new lat = 19.00089932, new lon = 47.00000000
bearing 030: new lat = 19.00077883, new lon = 47.00047557
bearing 060: new lat = 19.00044966, new lon = 47.00082371
bearing 090: new lat = 19.00000000, new lon = 47.00095114
bearing 120: new lat = 18.99955034, new lon = 47.00082371
bearing 150: new lat = 18.99922116, new lon = 47.00047557
bearing 180: new lat = 18.99910068, new lon = 47.00000000
bearing 210: new lat = 18.99922116, new lon = 46.99952443
bearing 240: new lat = 18.99955034, new lon = 46.99917629
bearing 270: new lat = 19.00000000, new lon = 46.99904886
bearing 300: new lat = 19.00044966, new lon = 46.99917629
bearing 330: new lat = 19.00077883, new lon = 46.99952443
As you can see, it is accurate to within a fraction of a meter (your code gave 19.0008995 - it is actually possible that your result was "wrong" in the last digit as these two methods agree to 8 significant digits even though they use different equations).
The question isn't really answerable if the OP wants a location a distance (100 meters) from the current location without a desired bearing being provided, there being an infinite number of points in a circle around the point.
So, this answer may or may not be what the OP wants, it is a way with CLLocation to calculate the distance between two points.
Create two CLLocation point and use the method Have you looked at theCLLocationmethod- (CLLocationDistance)distanceFromLocation:(const CLLocation *)location`.
CLLocation *location1 = [[CLLocation alloc] initWithLatitude:)latitude1 longitude:longitude1];
CLLocation *location2 = [[CLLocation alloc] initWithLatitude:)latitude2 longitude:longitude2];
double distance = [location1 distanceFromLocation:location2];
The radius of the earth at equator = 6,371 km. The equator is divided into 360 degrees of longitude, so each degree at the equator represents approximately 111.32 km. Moving away from the equator towards a pole this distance decreases to zero at the pole.To calculate the distance at different latitudes multiply it by the cosine of the latitude
3 decimal places,0.001 degrees aproximates to
111.32 meters at equator
96.41meters at 30 degrees N/S
78.71 meters at 45 degrees N/S
55.66 meters at 60 degrees N/S
28.82 meters at 75 degrees N/S
For for small distances (100 meters) Pythagoras’ theorem can be used on an equirectangular projection to calculate distance. This is less complicated than Haversine or Spherical Law of Cosines.
var R = 6371; // km
lat/lng in radians
In pseudo code as I don't know Objective-C
var x = (lng2-lng1) * cos((lat1+lat2)/2);
var y = (lat2-lat1);
var d = sqrt(x*x + y*y) * R;

Minimum distance between a point and a line in latitude, longitude

I have a line with two points in latitude and longitude
A: 3.222895, 101.719751
B: 3.227511, 101.724318
and 1 point
C: 3.224972, 101.722932
How can I calculate minimum distance between point C and a line consists of point A and B?
It will be convenient if you can provide the calculation and objective-c code too. The distance is around 89 meters (using ruler in Google Earth).
Thanks to mimi and this great article http://www.movable-type.co.uk/scripts/latlong.html but they don't give the whole picture. Here is a detail one. All this points are collected using Google Earth using Placemark to mark the locations. Make sure lat/long are set to decimal degrees in Preferences.
lat A = 3.222895
lon A = 101.719751
lat B = 3.222895
lon B = 101.719751
lat C = 3.224972
lon C = 101.722932
Earth radius, R = 6371
1. First you have to find the bearing from A to C and A to B.
Bearing formula
bearingAC = atan2( sin(Δλ)*cos(φ₂), cos(φ₁)*sin(φ₂) − sin(φ₁)*cos(φ₂)*cos(Δλ) )
bearingAB = atan2( sin(Δλ)*cos(φ₂), cos(φ₁)*sin(φ₂) − sin(φ₁)*cos(φ₂)*cos(Δλ) )
φ is latitude, λ is longitude, R is earth radius
2. Find A to C distance using spherical law of cosines
distanceAC = acos( sin(φ₁)*sin(φ₂) + cos(φ₁)*cos(φ₂)*cos(Δλ) )*R
3. Find cross-track distance
distance = asin(sin(distanceAC/ R) * sin(bearingAC − bearing AB)) * R
Objective-C code
double lat1 = 3.227511;
double lon1 = 101.724318;
double lat2 = 3.222895;
double lon2 = 101.719751;
double lat3 = 3.224972;
double lon3 = 101.722932;
double y = sin(lon3 - lon1) * cos(lat3);
double x = cos(lat1) * sin(lat3) - sin(lat1) * cos(lat3) * cos(lat3 - lat1);
double bearing1 = radiansToDegrees(atan2(y, x));
bearing1 = 360 - ((bearing1 + 360) % 360);
double y2 = sin(lon2 - lon1) * cos(lat2);
double x2 = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lat2 - lat1);
double bearing2 = radiansToDegrees(atan2(y2, x2));
bearing2 = 360 - ((bearing2 + 360) % 360);
double lat1Rads = degreesToRadians(lat1);
double lat3Rads = degreesToRadians(lat3);
double dLon = degreesToRadians(lon3 - lon1);
double distanceAC = acos(sin(lat1Rads) * sin(lat3Rads)+cos(lat1Rads)*cos(lat3Rads)*cos(dLon)) * 6371;
double min_distance = fabs(asin(sin(distanceAC/6371)*sin(degreesToRadians(bearing1)-degreesToRadians(bearing2))) * 6371);
NSLog(#"bearing 1: %g", bearing1);
NSLog(#"bearing 2: %g", bearing2);
NSLog(#"distance AC: %g", distanceAC);
NSLog(#"min distance: %g", min_distance);
Actually there's a library for this. You can find it here https://github.com/100grams/CoreLocationUtils
Calculate bearing for each: C to A , and C to B:
var y = Math.sin(dLon) * Math.cos(lat2);
var x = Math.cos(lat1)*Math.sin(lat2) -
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
var brng = Math.atan2(y, x).toDeg();
dLon= lon2-lon1;
Calculate cross-track distance:
var dXt = Math.asin(Math.sin(distance_CB/R)*Math.sin(bearing_CA-bearing_CB)) * R;
R is the radius of earth, dXt is the minimum distance you wanted to calculate.
Code to carry out this calculation is posted at here.
This implements an accurate solution in terms of ellipsoidal geodesics.
For the basic geodesic calculations, you can use
GeographicLib or the port of these algorithms to C which are included in version 4.9.0 of PROJ.4. This C interface is documented here.
Here's the result of compiling and running intercept.cpp:
$ echo 3.222895 101.719751 3.227511 101.724318 3.224972 101.722932 | ./intercept
Initial guess 3.225203 101.7220345
Increment 0.0003349040566247297 0.0003313413822354505
Increment -4.440892098500626e-16 0
Increment 0 0
...
Final result 3.225537904056624 101.7223658413822
Azimuth to A1 -135.1593040635131
Azimuth to A2 44.84069593652217
Azimuth to B1 134.8406959363608
Distance to line is 88.743m:
$ echo 3.224972 101.722932 3.225537904056624 101.7223658413822 | GeodSolve -i
-45.15927221 -45.15930407 88.743
See post here:
https://stackoverflow.com/a/33343505/4083623
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;
If you know how to calculate the distance of two points, get the distances between each two points, you get AB, AC, and BC. You want to know the closest distance between point C and line AB.
First get the value of P
P=(AB+BC+AC)/2
Using P, you need to get S
S=SQRT((P(P-AC)(P-AB)(P-AC))
SQRT means square root. Then you get what you want by
2*S/AB

Longitude, Latitude, Altitude to 3D-Cartesian Coordinate Systems

I'm wondering about the transformation from Lat,Lon,Alt Values to 3D-Systems like ECEF (Earth-Centered).
This can be implemented as follows (https://gist.github.com/1536054):
/*
* WGS84 ellipsoid constants Radius
*/
private static final double a = 6378137;
/*
* eccentricity
*/
private static final double e = 8.1819190842622e-2;
private static final double asq = Math.pow(a, 2);
private static final double esq = Math.pow(e, 2);
void convert(latitude,longitude,altitude){
double lat = Math.toRadians(latitude);
double lon = Math.toRadians(longitude);
double alt = altitude;
double N = a / Math.sqrt(1 - esq * Math.pow(Math.sin(lat), 2));
x = (N + alt) * Math.cos(lat) * Math.cos(lon);
y = (N + alt) * Math.cos(lat) * Math.sin(lon);
z = ((1 - esq) * N + alt) * Math.sin(lat);
}
What in my opinion seems very strange is the fact, that a little change of the altitude, affects x,y and z, where I would expect, that it just affect one axis. For example, if I have two GPS-Points, which have same lat/lon values but different altitude values, I'll get 3 different x,y,z coordinates.
Can someone explain the "idea" behind this? This looks very curious to me... Is there any other 3D-System, in which only one of the values is changing, when I lower/higher my altitude value?
Thanks a lot!
If you look at this picture: ECEF Coordinate System
then you see why.
ECEF is a cube arround the earth, centered in earth center.
if altitude raises you move out.
Lat/lon is an "angular" coordinate system where lat,lon are angles,
ECEF is a cartesian coordinate system!
Probaly you thougt ECEF is like LatLon with center earth has altitude 0, but this is not the case.

How to find z by arbitrary x,y coordinates within triangle if you have triangle vertices

Given vertices V1 (x1,y1,z1), V2 (x2,y2,z2), V3 (x3,y3,z3) of a triangle T, I have to find z coordinate of a point by it's x,y coordinate if I know that (x,y) lies within projection of triangle Tp (x1,y1), (x2,y2), (x3,y3).
Actually, triangle plane in 3D is defined by equation: Ax+By+Cz+D=0, and I can find z = (D-Ax-By)/C
The problem is that A, B, C, D are too expensive to calculate in run-time:
A = y1(z2-z3) + y2(z3-z1) + y3(z1-z2)
B = z1(x2-x3) + z2(x3-x1) + z3(x1-x2)
C = x1(y2-y3) + x2(y3-y1) + x3(y1-y2)
D = -x1(y2*z3 – y3*z2) – x2(y3*z1 – y1*z3) – x3 (y1*z2 – y2*z1)
Is it possible to calculate A, B, C, D using, say, opengl shaders? Are there optimized algorithms to find plane coefficients?
The technique is called Barycentric coordinates but the wiki page is pretty hard to follow -
See http://www.alecjacobson.com/weblog/?p=1596
float calcY(vec3 p1, vec3 p2, vec3 p3, float x, float z) {
float det = (p2.z - p3.z) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.z - p3.z);
float l1 = ((p2.z - p3.z) * (x - p3.x) + (p3.x - p2.x) * (z - p3.z)) / det;
float l2 = ((p3.z - p1.z) * (x - p3.x) + (p1.x - p3.x) * (z - p3.z)) / det;
float l3 = 1.0f - l1 - l2;
return l1 * p1.y + l2 * p2.y + l3 * p3.y;
}
Code from http://www.gamedev.net/topic/597393-getting-the-height-of-a-point-on-a-triangle/ - carefull about computer graphics vs maths use of Y Z
ps. I Don't know of any faster version using shaders. One quick dirty+solution is to render the triangle using colors based on the height of the vertices and pick the pixel color at your X,Y - in practice this never ends up being much faster on a desktop machine, don't know about opengl-es

calculate latitude and longitude along a circumference of a known circle with known distance

I dont need help with a programming language, I need help from someone to calculate the gps coordinates of points of a specific distance, namely 22 feet apart, on the circumference of a circle. I know the beginning gps coordinates and the radius. I am pretty sure the haversine, or the speherical law of cosines has the answer, but its been a long time since I have used any trig formulas and I cant figure it out. I am using decimal degrees and am programing in this vb.net. If someone could dumb this down for me it would be a great help.
As I understand you have:
Coordinate of the center of
circumference.
Distance between
two point on the circumference of a
circle.
Radius of circumference.
In my opinion this is not enough for calculating other point's coordinates. You should have as minimum yet one point coordinate, because we only can guess where points are placed on the circumference.
Here's the basic algorithm:
Calculate the angular measure whose arc length is 22 feet, with the given radius
numPoints = Math.PI * 2 / angularMeasure
for i in range(numPoints):
calculate proportion around the circle we are, in terms of degrees or radians
calculate the location of the endpoint of a great circle or rhumb arc from the center point moving in the specific azimuth direction (from the proportion around the circle) the given radius
This last point is the hardest part. Here's code from the WorldWind SDK (available: http://worldwind.arc.nasa.gov/java/) (Note- you'll have to calculate the radius in terms of angles, which you can do pretty easily given the radius / circumference of the earth)
/*
Copyright (C) 2001, 2006 United States Government
as represented by the Administrator of the
National Aeronautics and Space Administration.
All Rights Reserved.
*/
/**
* Computes the location on a rhumb line with the given starting location, rhumb azimuth, and arc distance along the
* line.
*
* #param p LatLon of the starting location
* #param rhumbAzimuth rhumb azimuth angle (clockwise from North)
* #param pathLength arc distance to travel
*
* #return LatLon location on the rhumb line.
*/
public static LatLon rhumbEndPosition(LatLon p, Angle rhumbAzimuth, Angle pathLength)
{
if (p == null)
{
String message = Logging.getMessage("nullValue.LatLonIsNull");
Logging.logger().severe(message);
throw new IllegalArgumentException(message);
}
if (rhumbAzimuth == null || pathLength == null)
{
String message = Logging.getMessage("nullValue.AngleIsNull");
Logging.logger().severe(message);
throw new IllegalArgumentException(message);
}
double lat1 = p.getLatitude().radians;
double lon1 = p.getLongitude().radians;
double azimuth = rhumbAzimuth.radians;
double distance = pathLength.radians;
if (distance == 0)
return p;
// Taken from http://www.movable-type.co.uk/scripts/latlong.html
double lat2 = lat1 + distance * Math.cos(azimuth);
double dPhi = Math.log(Math.tan(lat2 / 2.0 + Math.PI / 4.0) / Math.tan(lat1 / 2.0 + Math.PI / 4.0));
double q = (lat2 - lat1) / dPhi;
if (Double.isNaN(dPhi) || Double.isNaN(q) || Double.isInfinite(q))
{
q = Math.cos(lat1);
}
double dLon = distance * Math.sin(azimuth) / q;
// Handle latitude passing over either pole.
if (Math.abs(lat2) > Math.PI / 2.0)
{
lat2 = lat2 > 0 ? Math.PI - lat2 : -Math.PI - lat2;
}
double lon2 = (lon1 + dLon + Math.PI) % (2 * Math.PI) - Math.PI;
if (Double.isNaN(lat2) || Double.isNaN(lon2))
return p;
return new LatLon(
Angle.fromRadians(lat2).normalizedLatitude(),
Angle.fromRadians(lon2).normalizedLongitude());
}
You are looking for the equation of what is called a "small circle". Look at this book for the equation of the small circle and the arc length equation for that small circle. However, because your distances are so small, you could consider your area flat and use simpler geometry. Using UTM coordinates would make the calculations much simpler than using lat/long.
The Haversine formula relates to great circles, not small circles...