I was in need of a little math help that I can't seem to find the answer to, any links to documentation would be greatly appreciated.
Heres my situation, I have no idea where I am in this maze, but I need to move around and find my way back to the start. I was thinking of implementing a waypoint list of places i've been offset from my start at 0,0. This is a 2D cartesian plane.
I've been given 2 properties, my translation speed from 0-1 and my rotation speed from -1 to 1. -1 is very left and +1 is very right. These are speed and not angles so thats where my problem lies. If I'm given 0 as a translation speed and 0.2 I will continually turn to my right at a slow speed.
How do I figure out the offsets given these 2 variables? I can store it every time I take a 'step'.
I just need to figure out the offsets in x and y terms given the translations and rotation speeds. And the rotation to get to those points.
Any help is appreciated.
Your question is unclear on a couple of points, so I have to make some assumptions:
During each time interval, translation speed and rotational velocity are constant.
You know the values of these variables in every time interval (and you know rotational velocity in usable units, like radians per second, not just "very left").
You know initial heading.
You can maintain enough precision that roundoff error is not a problem.
Given that, there is an exact solution. First the easy part:
delta_angle = omega * delta_t
Where omega is the angular velocity. The distance traveled (maybe along a curve) is
dist = speed * delta_t
and the radius of the curve is
radius = dist / delta_angle
(This gets huge when angular velocity is near zero-- we'll deal with that in a moment.) If angle (at the beginning of the interval) is zero, defined as pointing in the +x direction, then the translation in the interval is easy, and we'll call it deta_x_0 and delta_y_0:
delta_x_0 = radius * sin(delta_angle)
delta_y_0 = radius * (1 - cos(delta_angle))
Since we want to be able to deal with very small delta_angle and very large radius, we'll expand sin and cos, and use this only when angular velocity is close to zero:
dx0 = r * sin(da) = (dist/da) * [ da - da^3/3! + da^5/5! - ...]
= dist * [ 1 - da^2/3! + da^4/5! - ...]
dy0 = r * (1-cos(da)) = (dist/da) * [ da^2/2! - da^4/4! + da^6/6! - ...]
= dist * [ da/2! - da^3/4! + da^5/6! - ...]
But angle generally isn't equal to zero, so we have to rotate these displacements:
dx = cos(angle) * dx0 - sin(angle) * dy0
dy = sin(angle) * dx0 - cos(angle) * dy0
You could do it in two stages. First work out the change of direction to get a new direction vector and then secondly work out the new position using this new direction. Something like
angle = angle + omega * delta_t;
const double d_x = cos( angle );
const double d_y = sin( angle );
x = x + d_x * delta_t * v;
y = y + d_y * delta_t * v;
where you store your current angle out at each step. ( d_x, d_y ) is the current direction vector and omega is the rotation speed that you have. delta_t is obviously your timestep and v is your speed.
This may be too naive to split it up into two distinct stages. I'm not sure I haven't really thought it through too much and haven't tested it but if it works let me know!
If I have a latitude or longitude reading in standard NMEA format is there an easy way / formula to convert that reading to meters, which I can then implement in Java (J9)?
Edit:
Ok seems what I want to do is not possible easily, however what I really want to do is:
Say I have a lat and long of a way point and a lat and long of a user is there an easy way to compare them to decide when to tell the user they are within a reasonably close distance of the way point? I realise reasonable is subject but is this easily do-able or still overly maths-y?
Here is a javascript function:
function measure(lat1, lon1, lat2, lon2){ // generally used geo measurement function
var R = 6378.137; // Radius of earth in KM
var dLat = lat2 * Math.PI / 180 - lat1 * Math.PI / 180;
var dLon = lon2 * Math.PI / 180 - lon1 * Math.PI / 180;
var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.cos(lat1 * Math.PI / 180) * Math.cos(lat2 * Math.PI / 180) *
Math.sin(dLon/2) * Math.sin(dLon/2);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c;
return d * 1000; // meters
}
Explanation: https://en.wikipedia.org/wiki/Haversine_formula
The haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
Given you're looking for a simple formula, this is probably the simplest way to do it, assuming that the Earth is a sphere with a circumference of 40075 km.
Length in km of 1° of latitude = always 111.32 km
Length in km of 1° of longitude = 40075 km * cos( latitude ) / 360
For approximating short distances between two coordinates I used formulas from
http://en.wikipedia.org/wiki/Lat-lon:
m_per_deg_lat = 111132.954 - 559.822 * cos( 2 * latMid ) + 1.175 * cos( 4 * latMid);
m_per_deg_lon = 111132.954 * cos ( latMid );
.
In the code below I've left the raw numbers to show their relation to the formula from wikipedia.
double latMid, m_per_deg_lat, m_per_deg_lon, deltaLat, deltaLon,dist_m;
latMid = (Lat1+Lat2 )/2.0; // or just use Lat1 for slightly less accurate estimate
m_per_deg_lat = 111132.954 - 559.822 * cos( 2.0 * latMid ) + 1.175 * cos( 4.0 * latMid);
m_per_deg_lon = (3.14159265359/180 ) * 6367449 * cos ( latMid );
deltaLat = fabs(Lat1 - Lat2);
deltaLon = fabs(Lon1 - Lon2);
dist_m = sqrt ( pow( deltaLat * m_per_deg_lat,2) + pow( deltaLon * m_per_deg_lon , 2) );
The wikipedia entry states that the distance calcs are within 0.6m for 100km longitudinally and 1cm for 100km latitudinally but I have not verified this as anywhere near that accuracy is fine for my use.
Here is the R version of b-h-'s function, just in case:
measure <- function(lon1,lat1,lon2,lat2) {
R <- 6378.137 # radius of earth in Km
dLat <- (lat2-lat1)*pi/180
dLon <- (lon2-lon1)*pi/180
a <- sin((dLat/2))^2 + cos(lat1*pi/180)*cos(lat2*pi/180)*(sin(dLon/2))^2
c <- 2 * atan2(sqrt(a), sqrt(1-a))
d <- R * c
return (d * 1000) # distance in meters
}
The earth is an annoyingly irregular surface, so there is no simple formula to do this exactly. You have to live with an approximate model of the earth, and project your coordinates onto it. The model I typically see used for this is WGS 84. This is what GPS devices usually use to solve the exact same problem.
NOAA has some software you can download to help with this on their website.
There are many tools that will make this easy. See monjardin's answer for more details about what's involved.
However, doing this isn't necessarily difficult. It sounds like you're using Java, so I would recommend looking into something like GDAL. It provides java wrappers for their routines, and they have all the tools required to convert from Lat/Lon (geographic coordinates) to UTM (projected coordinate system) or some other reasonable map projection.
UTM is nice, because it's meters, so easy to work with. However, you will need to get the appropriate UTM zone for it to do a good job. There are some simple codes available via googling to find an appropriate zone for a lat/long pair.
One nautical mile (1852 meters) is defined as one arcminute of longitude at the equator. However, you need to define a map projection (see also UTM) in which you are working for the conversion to really make sense.
There are quite a few ways to calculate this. All of them use aproximations of spherical trigonometry where the radius is the one of the earth.
try http://www.movable-type.co.uk/scripts/latlong.html for a bit of methods and code in different languages.
'below is from
'http://www.zipcodeworld.com/samples/distance.vbnet.html
Public Function distance(ByVal lat1 As Double, ByVal lon1 As Double, _
ByVal lat2 As Double, ByVal lon2 As Double, _
Optional ByVal unit As Char = "M"c) As Double
Dim theta As Double = lon1 - lon2
Dim dist As Double = Math.Sin(deg2rad(lat1)) * Math.Sin(deg2rad(lat2)) + _
Math.Cos(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) * _
Math.Cos(deg2rad(theta))
dist = Math.Acos(dist)
dist = rad2deg(dist)
dist = dist * 60 * 1.1515
If unit = "K" Then
dist = dist * 1.609344
ElseIf unit = "N" Then
dist = dist * 0.8684
End If
Return dist
End Function
Public Function Haversine(ByVal lat1 As Double, ByVal lon1 As Double, _
ByVal lat2 As Double, ByVal lon2 As Double, _
Optional ByVal unit As Char = "M"c) As Double
Dim R As Double = 6371 'earth radius in km
Dim dLat As Double
Dim dLon As Double
Dim a As Double
Dim c As Double
Dim d As Double
dLat = deg2rad(lat2 - lat1)
dLon = deg2rad((lon2 - lon1))
a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) + Math.Cos(deg2rad(lat1)) * _
Math.Cos(deg2rad(lat2)) * Math.Sin(dLon / 2) * Math.Sin(dLon / 2)
c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a))
d = R * c
Select Case unit.ToString.ToUpper
Case "M"c
d = d * 0.62137119
Case "N"c
d = d * 0.5399568
End Select
Return d
End Function
Private Function deg2rad(ByVal deg As Double) As Double
Return (deg * Math.PI / 180.0)
End Function
Private Function rad2deg(ByVal rad As Double) As Double
Return rad / Math.PI * 180.0
End Function
To convert latitude and longitude in x and y representation you need to decide what type of map projection to use. As for me, Elliptical Mercator seems very well. Here you can find an implementation (in Java too).
Here is a MySQL function:
SET #radius_of_earth = 6378.137; -- In kilometers
DROP FUNCTION IF EXISTS Measure;
DELIMITER //
CREATE FUNCTION Measure (lat1 REAL, lon1 REAL, lat2 REAL, lon2 REAL) RETURNS REAL
BEGIN
-- Multiply by 1000 to convert millimeters to meters
RETURN 2 * #radius_of_earth * 1000 * ASIN(SQRT(
POW(SIN((lat2 - lat1) / 2 * PI() / 180), 2) +
COS(lat1 * PI() / 180) *
COS(lat2 * PI() / 180) *
POW(SIN((lon2 - lon1) / 2 * PI() / 180), 2)
));
END; //
DELIMITER ;
Why limiting to one degree?
The formula is based on the proportion:
distance[m] : distance[deg] = max circumference[m] : 360[deg]
Lets say you are given an angle for a latitude and one for longitude both in degrees: (longitude[deg], latitude[deg])
For the latitude, the max circumference is always the one passing for the poles. In a spherical model, with radius R (in meters) the max circumference is 2 * pi * R and the proportions resolves to:
latitude[m] = ( 2 * pi * R[m] * latitude[deg] ) / 360[deg]
(note that deg and deg simplifies, and what remains is meters on both sides).
For the longitude the max circumference is proportional to the cosine of the latitude (as you can imagine running in circle the north pole is shorter than running in circle around the equator), so it is 2 * pi * R * cos(latitude[rad]).
Therefore
longitude distance[m] = ( 2 * pi * R[m] * cos(latitude[rad]) * longitude[deg] ) / 360[deg]
Note that you will have to convert the latitude from deg to rad before computing the cos.
Omitting details for who is just looking for the formula:
lat_in_m = 111132.954 * lat_in_degree / 360
lon_in_m = 111132.954 * cos(lat_in_radians) * lon_in_deg ) / 360
If its sufficiently close you can get away with treating them as coordinates on a flat plane. This works on say, street or city level if perfect accuracy isnt required and all you need is a rough guess on the distance involved to compare with an arbitrary limit.
Here is a version in Swift:
func toDegreeAt(point: CLLocationCoordinate2D) -> CLLocationDegrees {
let latitude = point.latitude
let earthRadiusInMetersAtSeaLevel = 6378137.0
let earthRadiusInMetersAtPole = 6356752.314
let r1 = earthRadiusInMetersAtSeaLevel
let r2 = earthRadiusInMetersAtPole
let beta = latitude
let earthRadiuseAtGivenLatitude = (
( pow(pow(r1, 2) * cos(beta), 2) + pow(pow(r2, 2) * sin(beta), 2) ) /
( pow(r1 * cos(beta), 2) + pow(r2 * sin(beta), 2) )
)
.squareRoot()
let metersInOneDegree = (2 * Double.pi * earthRadiuseAtGivenLatitude * 1.0) / 360.0
let value: CLLocationDegrees = self / metersInOneDegree
return value
}
Original poster asked
"If I have a latitude or longitude reading in standard NMEA format is there an easy way / formula to convert that reading to meters"
I haven't used Java in a while so I did the solution here in "PARI".
Just plug your point's latitude and longitudes
into the equations below to get
the exact arc lengths and scales
in meters per (second of Longitude)
and meters per (second of Latitude).
I wrote these equations for
the free-open-source-mac-pc math program "PARI".
You can just paste the following into it
and the I will show how to apply them to two made up points:
\\=======Arc lengths along Latitude and Longitude and the respective scales:
\p300
default(format,"g.42")
dms(u)=[truncate(u),truncate((u-truncate(u))*60),((u-truncate(u))*60-truncate((u-truncate(u))*60))*60];
SpinEarthRadiansPerSec=7.292115e-5;\
GMearth=3986005e8;\
J2earth=108263e-8;\
re=6378137;\
ecc=solve(ecc=.0001,.9999,eccp=ecc/sqrt(1-ecc^2);qecc=(1+3/eccp^2)*atan(eccp)-3/eccp;ecc^2-(3*J2earth+4/15*SpinEarthRadiansPerSec^2*re^3/GMearth*ecc^3/qecc));\
e2=ecc^2;\
b2=1-e2;\
b=sqrt(b2);\
fl=1-b;\
rfl=1/fl;\
U0=GMearth/ecc/re*atan(eccp)+1/3*SpinEarthRadiansPerSec^2*re^2;\
HeightAboveEllipsoid=0;\
reh=re+HeightAboveEllipsoid;\
longscale(lat)=reh*Pi/648000/sqrt(1+b2*(tan(lat))^2);
latscale(lat)=reh*b*Pi/648000/(1-e2*(sin(lat))^2)^(3/2);
longarc(lat,long1,long2)=longscale(lat)*648000/Pi*(long2-long1);
latarc(lat1,lat2)=(intnum(th=lat1,lat2,sqrt(1-e2*(sin(th))^2))+e2/2*sin(2*lat1)/sqrt(1-e2*(sin(lat1))^2)-e2/2*sin(2*lat2)/sqrt(1-e2*(sin(lat2))^2))*reh;
\\=======
To apply that to your type of problem I will make up
that one of your data points was at
[Latitude, Longitude]=[+30, 30]
and the other at
[Latitude, Longitude]=[+30:00:16.237796,30:00:18.655502].
To convert those points to meters in two coordinates:
I can setup a system of coordinates in meters
with the first point being at the origin: [0,0] meters.
Then I can define the coordinate x-axis as due East-West,
and the y-axis as due North-South.
Then the second point's coordinates are:
? [longarc(30*Pi/180,30*Pi/180,((18.655502/60+0)/60+30)*Pi/180),latarc(30*Pi/180,((16.237796/60+0)/60+30)*Pi/180)]
%9 = [499.999998389040060103621525561027349597207, 499.999990137812119668486524932382720606325]
Warning on precision:
Note however:
Since the surface of the Earth is curved,
2-dimensional coordinates obtained on it can't follow
the same rules as cartesian coordinates
such as the Pythagorean Theorem perfectly.
Also lines pointing due North-South
converge in the Northern Hemisphere.
At the North Pole it becomes obvious
that North-South lines won't serve well for
lines parallel to the y-axis on a map.
At 30 degrees Latitude with 500 meter lengths,
the x-coordinate changes by 1.0228 inches if the scale is set from [0,+500] instead of [0,0]:
? [longarc(((18.655502/60+0)/60+30)*Pi/180,30*Pi/180,((18.655502/60+0)/60+30)*Pi/180),latarc(30*Pi/180,((16.237796/60+0)/60+30)*Pi/180)]
%10 = [499.974018595036400823218815901067566617826, 499.999990137812119668486524932382720606325]
? (%10[1]-%9[1])*1000/25.4
%12 = -1.02282653557713702372872677007019603860352
?
The error there of 500meters/1inch is only about 1/20000,
good enough for most diagrams,
but one might want to reduce the 1 inch error.
For a completely general way to convert
lat,long to orthogonal x,y coordinates
for any point on the globe, I would chose to abandon
aligning coordinate lines with East-West
and North-South, except still keeping the center
y-axis pointing due North. For example you could
rotate the globe around the poles (around the 3-D Z-axis)
so the center point in your map is at longitude zero.
Then tilt the globe (around the 3-D y-axis) to
bring your center point to lat,long = [0,0].
On the globe points at lat,long = [0,0] are
farthest from the poles and have a lat,long
grid around them that is most orthogonal
so you can use these new "North-South", "East-West"
lines as coordinate x,y lines without incurring
the stretching that would have occurred doing
that before rotating the center point away from the pole.
Showing an explicit example of that would take a lot more space.
Based on average distance for degress in the Earth.
1° = 111km;
Converting this for radians and dividing for meters, take's a magic number for the RAD, in meters: 0.000008998719243599958;
then:
const RAD = 0.000008998719243599958;
Math.sqrt(Math.pow(lat1 - lat2, 2) + Math.pow(long1 - long2, 2)) / RAD;
If you want a simple solution then use the Haversine formula as outlined by the other comments. If you have an accuracy sensitive application keep in mind the Haversine formula does not guarantee an accuracy better then 0.5% as it is assuming the earth is a sphere. To consider that Earth is a oblate spheroid consider using Vincenty's formulae.
Additionally, I'm not sure what radius we should use with the Haversine formula: {Equator: 6,378.137 km, Polar: 6,356.752 km, Volumetric: 6,371.0088 km}.
You need to convert the coordinates to radians to do the spherical geometry. Once converted, then you can calculate a distance between the two points. The distance then can be converted to any measure you want.