I've been writing a raycaster in C++ and to render stuff I use GDI/GDI+. I know that using WGDI to render graphics is not the best idea in the world and I should probably use OpenGL, SFML and etc. but this raycaster does not involve any super-high-level real-time graphics, so in this case WGDI does the job. Besides I probably will be showing this in my school and installing OpenGL there would be a huge pain.
Okay, so the actual problem I wanted to talk about is that whenever I change the map grid from 8x8 to e.g. 8x16, the way that some walls are rendered is pretty bizzarre:
If someone can explain why such issue occurrs I would be very happy to discover what's wrong with my code.
main.cpp
/*
* Pseudo-code of the void renderer():
* Horizontal gridline check:
* Set horizontal distance to a pretty high value, horizontal coordinates to camera coordinates
* Calculate negative inverse of tangent
* Set DOF variable to 0
* If ray angle is bigger than PI calculate ray Y-coordinate to be as close as possible to the gridline position and subtract 0.0001 for precision, calculate ray X-coordinate and offset coordinates for the ray moovement over the gridline
* If ray angle is smaller than PI do the same as if ray angle < PI but add whatever the size of the map is to ray Y-coordinate
* If ray angle is straight up or down set ray coordinates to camera coordinates and DOF to map size
* Loop only if DOF is smaller than map size:
* Calculate actual gridline coordinates
* If the grid cell at [X, Y] is a wall break out from the loop, save the current ray coordinates, calculate the distance between the camera and the wall
* Else update ray coordinates with the earlier calculated offsets
*
* Vertical gridline check:
* Set vertical distance to a pretty high value, vertical coordinates to camera coordinates
* Calculate inverse of tangent
* Set DOF variable to 0
* If ray angle is bigger than PI / 2 and smaller than 3 * PI / 2 calculate ray X-coordinate to be as close as possible to the gridline position and subtract 0.0001 for precision, calculate ray Y-coordinate and offset coordinates for the ray moovement over the gridline
* If ray angle is smaller than PI / 2 or bigger than 3 * PI / 2 do the same as if ray angle > PI / 2 && < 3 * PI / 2 but add whatever the size of the map is to ray X-coordinate
* If ray angle is straight left or right set ray coordinates to camera coordinates and DOF to map size
* Loop only if DOF is smaller than map size:
* Calculate actual gridline coordinates
* If the grid cell at [X, Y] is a wall break out from the loop, save the current ray coordinates, calculate the distance between the camera and the wall
* Else update ray coordinates with the earlier calculated offsets
*
* If the vertical distance is smaller than the horizontal one update ray coordinates to the horizontal ones and set final distance to the horizontal one
* Else update ray coordinates to the vertical ones and set final distance to the vertical one
* Fix fisheye effect
* Add one radian to the ray angle
* Calculate line height by multiplying constant integer 400 by the map size and dividing that by the final distance
* Calculate line offset (to make it more centered) by subtracting half of the line height from constant integer 400
* Draw 8-pixels wide column at [ray index * 8, camera Z-offset + line offset] and [ray index * 8, camera Z-offset + line offset + line height] (the color doesn't matter i think)
*/
#include "../../LIB/wsgl.hpp"
#include "res/maths.hpp"
#include <memory>
using namespace std;
const int window_x = 640, window_y = 640;
float camera_x = 256, camera_y = 256, camera_z = 75;
float camera_a = 0.001;
int camera_fov = 80;
int map_x;
int map_y;
int map_s;
shared_ptr<int[]> map_w;
void controls()
{
if(wsgl::is_key_down(wsgl::key::w))
{
int mx = (camera_x + 30 * cos(camera_a)) / map_s;
int my = (camera_y + 30 * sin(camera_a)) / map_s;
int mp = my * map_x + mx;
if(mp >= 0 && mp < map_s && !map_w[mp])
{camera_x += 15 * cos(camera_a); camera_y += 15 * sin(camera_a);}
}
if(wsgl::is_key_down(wsgl::key::s))
{
int mx = (camera_x - 30 * cos(camera_a)) / map_s;
int my = (camera_y - 30 * sin(camera_a)) / map_s;
int mp = my * map_x + mx;
if(mp >= 0 && mp < map_s && !map_w[mp])
{camera_x -= 5 * cos(camera_a); camera_y -= 5 * sin(camera_a);}
}
if(wsgl::is_key_down(wsgl::key::a_left))
{camera_a = reset_ang(camera_a - 5 * RAD);}
if(wsgl::is_key_down(wsgl::key::a_right))
{camera_a = reset_ang(camera_a + 5 * RAD);}
if(wsgl::is_key_down(wsgl::key::a_up))
{camera_z += 15;}
if(wsgl::is_key_down(wsgl::key::a_down))
{camera_z -= 15;}
}
void renderer()
{
int map_x_pos, map_y_pos, map_cell, dof;
float ray_x, ray_y, ray_a = reset_ang(camera_a - deg_to_rad(camera_fov / 2));
float x_offset, y_offset, tangent, distance_h, distance_v, h_x, h_y, v_x, v_y;
float final_distance, line_height, line_offset;
wsgl::clear_window();
for(int i = 0; i < camera_fov; i++)
{
distance_h = 1000000, h_x = camera_x, h_y = camera_y;
tangent = -1 / tan(ray_a);
dof = 0;
if(ray_a > PI)
{ray_y = (((int)camera_y / map_s) * map_s) - 0.0001; ray_x = (camera_y - ray_y) * tangent + camera_x; y_offset = -map_s; x_offset = -y_offset * tangent;}
if(ray_a < PI)
{ray_y = (((int)camera_y / map_s) * map_s) + map_s; ray_x = (camera_y - ray_y) * tangent + camera_x; y_offset = map_s; x_offset = -y_offset * tangent;}
if(ray_a == 0 || ray_a == PI)
{ray_x = camera_x; ray_y = camera_y; dof = map_s;}
for(dof; dof < map_s; dof++)
{
map_x_pos = (int)(ray_x) / map_s;
map_y_pos = (int)(ray_y) / map_s;
map_cell = map_y_pos * map_x + map_x_pos;
if(map_cell >= 0 && map_cell < map_s && map_w[map_cell])
{dof = map_s; h_x = ray_x; h_y = ray_y; distance_h = distance(camera_x, camera_y, h_x, h_y);}
else
{ray_x += x_offset; ray_y += y_offset;}
}
distance_v = 1000000, v_x = camera_x, v_y = camera_y;
tangent = -tan(ray_a);
dof = 0;
if(ray_a > PI2 && ray_a < PI3)
{ray_x = (((int)camera_x / map_s) * map_s) - 0.0001; ray_y = (camera_x - ray_x) * tangent + camera_y; x_offset = -map_s; y_offset = -x_offset * tangent;}
if(ray_a < PI2 || ray_a > PI3)
{ray_x = (((int)camera_x / map_s) * map_s) + map_s; ray_y = (camera_x - ray_x) * tangent + camera_y; x_offset = map_s; y_offset = -x_offset * tangent;}
if(ray_a == PI2 || ray_a == PI3)
{ray_x = camera_x; ray_y = camera_y; dof = map_s;}
for(dof; dof < map_s; dof++)
{
map_x_pos = (int)(ray_x) / map_s;
map_y_pos = (int)(ray_y) / map_s;
map_cell = map_y_pos * map_x + map_x_pos;
if(map_cell >= 0 && map_cell < map_s && map_w[map_cell])
{dof = map_s; v_x = ray_x; v_y = ray_y; distance_v = distance(camera_x, camera_y, v_x, v_y);}
else
{ray_x += x_offset; ray_y += y_offset;}
}
if(distance_v < distance_h)
{ray_x = v_x; ray_y = v_y; final_distance = distance_v;}
else
{ray_x = h_x; ray_y = h_y; final_distance = distance_h;}
final_distance *= cos(reset_ang(camera_a - ray_a));
ray_a = reset_ang(ray_a + RAD);
line_height = (map_s * 400) / final_distance;
line_offset = 200 - line_height / 2;
wsgl::draw_line({i * 8, camera_z + line_offset}, {i * 8, camera_z + line_offset + line_height}, {0, 255 / (final_distance / 250 + 1), 0}, 8);
if(i == camera_fov / 2)
{wsgl::draw_text({0, 0}, {255, 255, 255}, L"Final distance: " + to_wstring(final_distance) + L" Line height: " + to_wstring(line_height) + L" X: " + to_wstring(camera_x) + L" Y: " + to_wstring(camera_y));}
}
wsgl::render_frame();
}
void load_map(wsgl::wide_str wstr, int cell_size = 1)
{
shared_ptr<wsgl::bmp> map = shared_ptr<wsgl::bmp>(wsgl::bmp::FromFile(wstr.c_str(), true));
map_x = map->GetWidth();
map_y = map->GetHeight();
map_s = map_x * map_y;
map_w = shared_ptr<int[]>(new int[map_s]);
wsgl::color color;
for(int y = 0; y < map_y; y += cell_size)
{
for(int x = 0; x < map_x; x += cell_size)
{
map->GetPixel(x, y, &color);
if(color.GetR() == 255 && color.GetG() == 255 && color.GetB() == 255)
{*(map_w.get() + ((y / cell_size) * map_x + (x / cell_size))) = 0;}
else
{*(map_w.get() + ((y / cell_size) * map_x + (x / cell_size))) = 1;}
}
}
}
int main()
{
wsgl::session sess = wsgl::startup(L"raycaster", {window_x, window_y});
load_map(L"res/map.png");
while(true)
{controls(); renderer();}
}
maths.hpp
#include <cmath>
const float PI = 3.14159265359;
const float PI2 = PI / 2;
const float PI3 = 3 * PI2;
const float RAD = PI / 180;
float deg_to_rad(float deg)
{return deg * RAD;}
float distance(float ax, float ay, float bx, float by)
{
float dx = bx - ax;
float dy = by - ay;
return sqrt(dx * dx + dy * dy);
}
float reset_ang(float ang)
{
if(ang < 0)
{ang += 2 * PI;}
if(ang > 2 * PI)
{ang -= 2 * PI;}
return ang;
}
If someone asks whats wsgl.hpp thats just my wrapper library over some WGDI routines and etc.
I think the problem lies here:
map_x_pos = (int)(ray_x) / map_s;
map_y_pos = (int)(ray_y) / map_s;
map_cell = map_y_pos * map_x + map_x_pos;
You need to change the order of operations:
map_x_pos = (int)(ray_x / map_s);
map_y_pos = (int)(ray_y / map_s);
map_cell = map_y_pos * map_x + map_x_pos;
With your current implementation, you first truncate ray_x and ray_y, then divide by map_s (which should probably be a floating point value, but is an integer in your current implementation), then truncate again to integer values. Your current implementation needlessly sacrifices precision and will be unpredictable for small map_s values.
Additionally, map_s seems incorrect. You set map_s to represent the total area of your map, but in the above code, you use it like it was the side length of the map.
To be correct, you would need something like
#include <cmath>
map_x_pos = (int)(ray_x / sqrtf(map_s));
map_y_pos = (int)(ray_y / sqrtf(map_s));
map_cell = map_y_pos * map_x + map_x_pos;
I'm studying the cordic. And I found the cordic gain. K=0.607XXX.
From CORDIC, K_i = cos(tan^-1(2^i)).
As I know the K is approched 0.607xxx.when I is going to infinity
this value come up with from all K multiplying.
I understand the reason of exist each k. But I am curioused Where does it used ? Why we use that value K=0.607xx?
The scale factor for the rotation mode of the circular variant of CORDIC can easily be established from first principles. The idea behind CORDIC is to take a point on the unit circle and rotate it, in steps, through the angle u whose sine and cosine we want to determine.
To that end we define a set of incremental angles a0, ..., an-1, such that ak = atan(0.5k). We sum these incremental angles appropriately into a partial sum of angles sk, such than sn ~= u. Let yk = cos(sk) and xk = sin(sk). If in a given step k we rotate by ak, we have
yk+1 = cos (sk+1) = cos (sk + ak)
xk+1 = sin (sk+1) = sin (sk + ak)
we can compute xk+1 and yk+1 from xk and yk as follows:
yk+1 = yk * cos (ak) - xk * sin (ak)
xk+1 = xk * cos (ak) + yk * sin (ak)
Considering that we may both add and subtract ak, and that tan(ak) = sin(ak)/cos(ak), we get:
yk+1 = cos (ak) * (yk ∓ xk * tan(ak)) = cos (sk+1)
xk+1 = cos (ak) * (xk ± yk * tan(ak)) = sin (sk+1)
To simplify this computation, we can leave out the multiplication with cos(ak) in every step, which gives us our CORDIC iteration scheme:
yk+1 = y ∓ xk * tan(ak)
xk+1 = x ± yk * tan(ak)
Because of our choice of ak, the multiplications with tan(ak) turn into simple right shifts if we compute in fixed-point arithmetic. Because we left off the factors cos(ak), we wind up with
yn ~= cos(u) * (1 / (cos (a0) * cos (a1) * ... * cos (an))
xn ~= sin(u) * (1 / (cos (a0) * cos (a1) * ... * cos (an))
The factor f = cos (a0) * cos (a1) * ... * cos (an) is 0.607..., as already noted. We incorporate it into the computation by setting the starting values
y0 = f * cos(0) = f
x0 = f * sin(0) = 0
Here is C code that shows the entire computation in action, using 16-bit fixed-point arithmetic. Input angles are scaled such that 360 degrees correspond to 216, while sine and cosine outputs are scaled such that 1 corresponds to 215.
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
/* round (atand (0.5**i) * 65536/360) */
static const short a[15] =
{
0x2000, 0x12e4, 0x09fb, 0x0511,
0x028b, 0x0146, 0x00a3, 0x0051,
0x0029, 0x0014, 0x000a, 0x0005,
0x0003, 0x0001, 0x0001
};
#define swap(a,b){a=a^b; b=b^a; a=a^b;}
void cordic (unsigned short u, short *s, short *c)
{
short x, y, oldx, oldy, q;
int i;
x = 0;
y = 0x4dba; /* 0.60725 */
oldx = x;
oldy = y;
q = u >> 14; /* quadrant */
u = u & 0x3fff; /* reduced angle */
u = -(short)u;
i = 0;
do {
if ((short)u < 0) {
x = x + oldy;
y = y - oldx;
u = u + a[i];
} else {
x = x - oldy;
y = y + oldx;
u = u - a[i];
}
oldx = x;
oldy = y;
i++;
/* right shift of signed negative number implementation defined in C */
oldx = (oldx < 0) ? (-((-oldx) >> i)) : (oldx >> i);
oldy = (oldy < 0) ? (-((-oldy) >> i)) : (oldy >> i);
} while (i < 15);
for (i = 0; i < q; i++) {
swap (x, y);
y = -y;
}
*s = x;
*c = y;
}
int main (void)
{
float angle;
unsigned short u;
short s, c;
printf ("angle in degrees [0,360): ");
scanf ("%f", &angle);
u = (unsigned short)(angle * 65536.0f / 360.0f + 0.5f);
cordic (u, &s, &c);
printf ("sin = % f (ref: % f) cos = % f (ref: % f)\n",
s/32768.0f, sinf(angle/360*2*3.14159265f),
c/32768.0f, cosf(angle/360*2*3.14159265f));
return EXIT_SUCCESS;
}
How do I calculate distance between two GPS coordinates (using latitude and longitude)?
Calculate the distance between two coordinates by latitude and longitude, including a Javascript implementation.
West and South locations are negative.
Remember minutes and seconds are out of 60 so S31 30' is -31.50 degrees.
Don't forget to convert degrees to radians. Many languages have this function. Or its a simple calculation: radians = degrees * PI / 180.
function degreesToRadians(degrees) {
return degrees * Math.PI / 180;
}
function distanceInKmBetweenEarthCoordinates(lat1, lon1, lat2, lon2) {
var earthRadiusKm = 6371;
var dLat = degreesToRadians(lat2-lat1);
var dLon = degreesToRadians(lon2-lon1);
lat1 = degreesToRadians(lat1);
lat2 = degreesToRadians(lat2);
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 earthRadiusKm * c;
}
Here are some examples of usage:
distanceInKmBetweenEarthCoordinates(0,0,0,0) // Distance between same
// points should be 0
0
distanceInKmBetweenEarthCoordinates(51.5, 0, 38.8, -77.1) // From London
// to Arlington
5918.185064088764
Look for haversine with Google; here is my solution:
#include <math.h>
#include "haversine.h"
#define d2r (M_PI / 180.0)
//calculate haversine distance for linear distance
double haversine_km(double lat1, double long1, double lat2, double long2)
{
double dlong = (long2 - long1) * d2r;
double dlat = (lat2 - lat1) * d2r;
double a = pow(sin(dlat/2.0), 2) + cos(lat1*d2r) * cos(lat2*d2r) * pow(sin(dlong/2.0), 2);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
double d = 6367 * c;
return d;
}
double haversine_mi(double lat1, double long1, double lat2, double long2)
{
double dlong = (long2 - long1) * d2r;
double dlat = (lat2 - lat1) * d2r;
double a = pow(sin(dlat/2.0), 2) + cos(lat1*d2r) * cos(lat2*d2r) * pow(sin(dlong/2.0), 2);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
double d = 3956 * c;
return d;
}
C# Version of Haversine
double _eQuatorialEarthRadius = 6378.1370D;
double _d2r = (Math.PI / 180D);
private int HaversineInM(double lat1, double long1, double lat2, double long2)
{
return (int)(1000D * HaversineInKM(lat1, long1, lat2, long2));
}
private double HaversineInKM(double lat1, double long1, double lat2, double long2)
{
double dlong = (long2 - long1) * _d2r;
double dlat = (lat2 - lat1) * _d2r;
double a = Math.Pow(Math.Sin(dlat / 2D), 2D) + Math.Cos(lat1 * _d2r) * Math.Cos(lat2 * _d2r) * Math.Pow(Math.Sin(dlong / 2D), 2D);
double c = 2D * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1D - a));
double d = _eQuatorialEarthRadius * c;
return d;
}
Here's a .NET Fiddle of this, so you can test it out with your own Lat/Longs.
Java Version of Haversine Algorithm based on Roman Makarov`s reply to this thread
public class HaversineAlgorithm {
static final double _eQuatorialEarthRadius = 6378.1370D;
static final double _d2r = (Math.PI / 180D);
public static int HaversineInM(double lat1, double long1, double lat2, double long2) {
return (int) (1000D * HaversineInKM(lat1, long1, lat2, long2));
}
public static double HaversineInKM(double lat1, double long1, double lat2, double long2) {
double dlong = (long2 - long1) * _d2r;
double dlat = (lat2 - lat1) * _d2r;
double a = Math.pow(Math.sin(dlat / 2D), 2D) + Math.cos(lat1 * _d2r) * Math.cos(lat2 * _d2r)
* Math.pow(Math.sin(dlong / 2D), 2D);
double c = 2D * Math.atan2(Math.sqrt(a), Math.sqrt(1D - a));
double d = _eQuatorialEarthRadius * c;
return d;
}
}
This is very easy to do with geography type in SQL Server 2008.
SELECT geography::Point(lat1, lon1, 4326).STDistance(geography::Point(lat2, lon2, 4326))
-- computes distance in meters using eliptical model, accurate to the mm
4326 is SRID for WGS84 elipsoidal Earth model
Here's a Haversine function in Python that I use:
from math import pi,sqrt,sin,cos,atan2
def haversine(pos1, pos2):
lat1 = float(pos1['lat'])
long1 = float(pos1['long'])
lat2 = float(pos2['lat'])
long2 = float(pos2['long'])
degree_to_rad = float(pi / 180.0)
d_lat = (lat2 - lat1) * degree_to_rad
d_long = (long2 - long1) * degree_to_rad
a = pow(sin(d_lat / 2), 2) + cos(lat1 * degree_to_rad) * cos(lat2 * degree_to_rad) * pow(sin(d_long / 2), 2)
c = 2 * atan2(sqrt(a), sqrt(1 - a))
km = 6367 * c
mi = 3956 * c
return {"km":km, "miles":mi}
I needed to calculate a lot of distances between the points for my project, so I went ahead and tried to optimize the code, I have found here. On average in different browsers my new implementation runs 2 times faster than the most upvoted answer.
function distance(lat1, lon1, lat2, lon2) {
var p = 0.017453292519943295; // Math.PI / 180
var c = Math.cos;
var a = 0.5 - c((lat2 - lat1) * p)/2 +
c(lat1 * p) * c(lat2 * p) *
(1 - c((lon2 - lon1) * p))/2;
return 12742 * Math.asin(Math.sqrt(a)); // 2 * R; R = 6371 km
}
You can play with my jsPerf and see the results here.
Recently I needed to do the same in python, so here is a python implementation:
from math import cos, asin, sqrt
def distance(lat1, lon1, lat2, lon2):
p = 0.017453292519943295
a = 0.5 - cos((lat2 - lat1) * p)/2 + cos(lat1 * p) * cos(lat2 * p) * (1 - cos((lon2 - lon1) * p)) / 2
return 12742 * asin(sqrt(a))
And for the sake of completeness: Haversine on wiki.
It depends on how accurate you need it to be. If you need pinpoint accuracy, it is best to look at an algorithm which uses an ellipsoid, rather than a sphere, such as Vincenty's algorithm, which is accurate to the mm.
Here it is in C# (lat and long in radians):
double CalculateGreatCircleDistance(double lat1, double long1, double lat2, double long2, double radius)
{
return radius * Math.Acos(
Math.Sin(lat1) * Math.Sin(lat2)
+ Math.Cos(lat1) * Math.Cos(lat2) * Math.Cos(long2 - long1));
}
If your lat and long are in degrees then divide by 180/PI to convert to radians.
PHP version:
(Remove all deg2rad() if your coordinates are already in radians.)
$R = 6371; // km
$dLat = deg2rad($lat2-$lat1);
$dLon = deg2rad($lon2-$lon1);
$lat1 = deg2rad($lat1);
$lat2 = deg2rad($lat2);
$a = sin($dLat/2) * sin($dLat/2) +
sin($dLon/2) * sin($dLon/2) * cos($lat1) * cos($lat2);
$c = 2 * atan2(sqrt($a), sqrt(1-$a));
$d = $R * $c;
A T-SQL function, that I use to select records by distance for a center
Create Function [dbo].[DistanceInMiles]
( #fromLatitude float ,
#fromLongitude float ,
#toLatitude float,
#toLongitude float
)
returns float
AS
BEGIN
declare #distance float
select #distance = cast((3963 * ACOS(round(COS(RADIANS(90-#fromLatitude))*COS(RADIANS(90-#toLatitude))+
SIN(RADIANS(90-#fromLatitude))*SIN(RADIANS(90-#toLatitude))*COS(RADIANS(#fromLongitude-#toLongitude)),15))
)as float)
return round(#distance,1)
END
I. Regarding "Breadcrumbs" method
Earth radius is different on different Lat. This must be taken into consideration in Haversine algorithm.
Consider Bearing change, which turns straight lines to arches (which are longer)
Taking Speed change into account will turn arches to spirals (which are longer or shorter than arches)
Altitude change will turn flat spirals to 3D spirals (which are longer again). This is very important for hilly areas.
Below see the function in C which takes #1 and #2 into account:
double calcDistanceByHaversine(double rLat1, double rLon1, double rHeading1,
double rLat2, double rLon2, double rHeading2){
double rDLatRad = 0.0;
double rDLonRad = 0.0;
double rLat1Rad = 0.0;
double rLat2Rad = 0.0;
double a = 0.0;
double c = 0.0;
double rResult = 0.0;
double rEarthRadius = 0.0;
double rDHeading = 0.0;
double rDHeadingRad = 0.0;
if ((rLat1 < -90.0) || (rLat1 > 90.0) || (rLat2 < -90.0) || (rLat2 > 90.0)
|| (rLon1 < -180.0) || (rLon1 > 180.0) || (rLon2 < -180.0)
|| (rLon2 > 180.0)) {
return -1;
};
rDLatRad = (rLat2 - rLat1) * DEGREE_TO_RADIANS;
rDLonRad = (rLon2 - rLon1) * DEGREE_TO_RADIANS;
rLat1Rad = rLat1 * DEGREE_TO_RADIANS;
rLat2Rad = rLat2 * DEGREE_TO_RADIANS;
a = sin(rDLatRad / 2) * sin(rDLatRad / 2) + sin(rDLonRad / 2) * sin(
rDLonRad / 2) * cos(rLat1Rad) * cos(rLat2Rad);
if (a == 0.0) {
return 0.0;
}
c = 2 * atan2(sqrt(a), sqrt(1 - a));
rEarthRadius = 6378.1370 - (21.3847 * 90.0 / ((fabs(rLat1) + fabs(rLat2))
/ 2.0));
rResult = rEarthRadius * c;
// Chord to Arc Correction based on Heading changes. Important for routes with many turns and U-turns
if ((rHeading1 >= 0.0) && (rHeading1 < 360.0) && (rHeading2 >= 0.0)
&& (rHeading2 < 360.0)) {
rDHeading = fabs(rHeading1 - rHeading2);
if (rDHeading > 180.0) {
rDHeading -= 180.0;
}
rDHeadingRad = rDHeading * DEGREE_TO_RADIANS;
if (rDHeading > 5.0) {
rResult = rResult * (rDHeadingRad / (2.0 * sin(rDHeadingRad / 2)));
} else {
rResult = rResult / cos(rDHeadingRad);
}
}
return rResult;
}
II. There is an easier way which gives pretty good results.
By Average Speed.
Trip_distance = Trip_average_speed * Trip_time
Since GPS Speed is detected by Doppler effect and is not directly related to [Lon,Lat] it can be at least considered as secondary (backup or correction) if not as main distance calculation method.
If you need something more accurate then have a look at this.
Vincenty's formulae are two related iterative methods used in geodesy
to calculate the distance between two points on the surface of a
spheroid, developed by Thaddeus Vincenty (1975a) They are based on the
assumption that the figure of the Earth is an oblate spheroid, and
hence are more accurate than methods such as great-circle distance
which assume a spherical Earth.
The first (direct) method computes the location of a point which is a
given distance and azimuth (direction) from another point. The second
(inverse) method computes the geographical distance and azimuth
between two given points. They have been widely used in geodesy
because they are accurate to within 0.5 mm (0.020″) on the Earth
ellipsoid.
If you're using .NET don't reivent the wheel. See System.Device.Location. Credit to fnx in the comments in another answer.
using System.Device.Location;
double lat1 = 45.421527862548828D;
double long1 = -75.697189331054688D;
double lat2 = 53.64135D;
double long2 = -113.59273D;
GeoCoordinate geo1 = new GeoCoordinate(lat1, long1);
GeoCoordinate geo2 = new GeoCoordinate(lat2, long2);
double distance = geo1.GetDistanceTo(geo2);
here is the Swift implementation from the answer
func degreesToRadians(degrees: Double) -> Double {
return degrees * Double.pi / 180
}
func distanceInKmBetweenEarthCoordinates(lat1: Double, lon1: Double, lat2: Double, lon2: Double) -> Double {
let earthRadiusKm: Double = 6371
let dLat = degreesToRadians(degrees: lat2 - lat1)
let dLon = degreesToRadians(degrees: lon2 - lon1)
let lat1 = degreesToRadians(degrees: lat1)
let lat2 = degreesToRadians(degrees: lat2)
let a = sin(dLat/2) * sin(dLat/2) +
sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2)
let c = 2 * atan2(sqrt(a), sqrt(1 - a))
return earthRadiusKm * c
}
This is version from "Henry Vilinskiy" adapted for MySQL and Kilometers:
CREATE FUNCTION `CalculateDistanceInKm`(
fromLatitude float,
fromLongitude float,
toLatitude float,
toLongitude float
) RETURNS float
BEGIN
declare distance float;
select
6367 * ACOS(
round(
COS(RADIANS(90-fromLatitude)) *
COS(RADIANS(90-toLatitude)) +
SIN(RADIANS(90-fromLatitude)) *
SIN(RADIANS(90-toLatitude)) *
COS(RADIANS(fromLongitude-toLongitude))
,15)
)
into distance;
return round(distance,3);
END;
This Lua code is adapted from stuff found on Wikipedia and in Robert Lipe's GPSbabel tool:
local EARTH_RAD = 6378137.0
-- earth's radius in meters (official geoid datum, not 20,000km / pi)
local radmiles = EARTH_RAD*100.0/2.54/12.0/5280.0;
-- earth's radius in miles
local multipliers = {
radians = 1, miles = radmiles, mi = radmiles, feet = radmiles * 5280,
meters = EARTH_RAD, m = EARTH_RAD, km = EARTH_RAD / 1000,
degrees = 360 / (2 * math.pi), min = 60 * 360 / (2 * math.pi)
}
function gcdist(pt1, pt2, units) -- return distance in radians or given units
--- this formula works best for points close together or antipodal
--- rounding error strikes when distance is one-quarter Earth's circumference
--- (ref: wikipedia Great-circle distance)
if not pt1.radians then pt1 = rad(pt1) end
if not pt2.radians then pt2 = rad(pt2) end
local sdlat = sin((pt1.lat - pt2.lat) / 2.0);
local sdlon = sin((pt1.lon - pt2.lon) / 2.0);
local res = sqrt(sdlat * sdlat + cos(pt1.lat) * cos(pt2.lat) * sdlon * sdlon);
res = res > 1 and 1 or res < -1 and -1 or res
res = 2 * asin(res);
if units then return res * assert(multipliers[units])
else return res
end
end
private double deg2rad(double deg)
{
return (deg * Math.PI / 180.0);
}
private double rad2deg(double rad)
{
return (rad / Math.PI * 180.0);
}
private double GetDistance(double lat1, double lon1, double lat2, double lon2)
{
//code for Distance in Kilo Meter
double theta = lon1 - lon2;
double dist = Math.Sin(deg2rad(lat1)) * Math.Sin(deg2rad(lat2)) + Math.Cos(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) * Math.Cos(deg2rad(theta));
dist = Math.Abs(Math.Round(rad2deg(Math.Acos(dist)) * 60 * 1.1515 * 1.609344 * 1000, 0));
return (dist);
}
private double GetDirection(double lat1, double lon1, double lat2, double lon2)
{
//code for Direction in Degrees
double dlat = deg2rad(lat1) - deg2rad(lat2);
double dlon = deg2rad(lon1) - deg2rad(lon2);
double y = Math.Sin(dlon) * Math.Cos(lat2);
double x = Math.Cos(deg2rad(lat1)) * Math.Sin(deg2rad(lat2)) - Math.Sin(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) * Math.Cos(dlon);
double direct = Math.Round(rad2deg(Math.Atan2(y, x)), 0);
if (direct < 0)
direct = direct + 360;
return (direct);
}
private double GetSpeed(double lat1, double lon1, double lat2, double lon2, DateTime CurTime, DateTime PrevTime)
{
//code for speed in Kilo Meter/Hour
TimeSpan TimeDifference = CurTime.Subtract(PrevTime);
double TimeDifferenceInSeconds = Math.Round(TimeDifference.TotalSeconds, 0);
double theta = lon1 - lon2;
double dist = Math.Sin(deg2rad(lat1)) * Math.Sin(deg2rad(lat2)) + Math.Cos(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) * Math.Cos(deg2rad(theta));
dist = rad2deg(Math.Acos(dist)) * 60 * 1.1515 * 1.609344;
double Speed = Math.Abs(Math.Round((dist / Math.Abs(TimeDifferenceInSeconds)) * 60 * 60, 0));
return (Speed);
}
private double GetDuration(DateTime CurTime, DateTime PrevTime)
{
//code for speed in Kilo Meter/Hour
TimeSpan TimeDifference = CurTime.Subtract(PrevTime);
double TimeDifferenceInSeconds = Math.Abs(Math.Round(TimeDifference.TotalSeconds, 0));
return (TimeDifferenceInSeconds);
}
i took the top answer and used it in a Scala program
import java.lang.Math.{atan2, cos, sin, sqrt}
def latLonDistance(lat1: Double, lon1: Double)(lat2: Double, lon2: Double): Double = {
val earthRadiusKm = 6371
val dLat = (lat2 - lat1).toRadians
val dLon = (lon2 - lon1).toRadians
val latRad1 = lat1.toRadians
val latRad2 = lat2.toRadians
val a = sin(dLat / 2) * sin(dLat / 2) + sin(dLon / 2) * sin(dLon / 2) * cos(latRad1) * cos(latRad2)
val c = 2 * atan2(sqrt(a), sqrt(1 - a))
earthRadiusKm * c
}
i curried the function in order to be able to easily produce functions that have one of the two locations fixed and require only a pair of lat/lon to produce distance.
Here's a Kotlin variation:
import kotlin.math.*
class HaversineAlgorithm {
companion object {
private const val MEAN_EARTH_RADIUS = 6371.008
private const val D2R = Math.PI / 180.0
}
private fun haversineInKm(lat1: Double, lon1: Double, lat2: Double, lon2: Double): Double {
val lonDiff = (lon2 - lon1) * D2R
val latDiff = (lat2 - lat1) * D2R
val latSin = sin(latDiff / 2.0)
val lonSin = sin(lonDiff / 2.0)
val a = latSin * latSin + (cos(lat1 * D2R) * cos(lat2 * D2R) * lonSin * lonSin)
val c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a))
return MEAN_EARTH_RADIUS * c
}
}
you can find a implementation of this (with some good explanation) in F# on fssnip
here are the important parts:
let GreatCircleDistance<[<Measure>] 'u> (R : float<'u>) (p1 : Location) (p2 : Location) =
let degToRad (x : float<deg>) = System.Math.PI * x / 180.0<deg/rad>
let sq x = x * x
// take the sin of the half and square the result
let sinSqHf (a : float<rad>) = (System.Math.Sin >> sq) (a / 2.0<rad>)
let cos (a : float<deg>) = System.Math.Cos (degToRad a / 1.0<rad>)
let dLat = (p2.Latitude - p1.Latitude) |> degToRad
let dLon = (p2.Longitude - p1.Longitude) |> degToRad
let a = sinSqHf dLat + cos p1.Latitude * cos p2.Latitude * sinSqHf dLon
let c = 2.0 * System.Math.Atan2(System.Math.Sqrt(a), System.Math.Sqrt(1.0-a))
R * c
I needed to implement this in PowerShell, hope it can help someone else.
Some notes about this method
Don't split any of the lines or the calculation will be wrong
To calculate in KM remove the * 1000 in the calculation of $distance
Change $earthsRadius = 3963.19059 and remove * 1000 in the calculation of $distance the to calulate the distance in miles
I'm using Haversine, as other posts have pointed out Vincenty's formulae is much more accurate
Function MetresDistanceBetweenTwoGPSCoordinates($latitude1, $longitude1, $latitude2, $longitude2)
{
$Rad = ([math]::PI / 180);
$earthsRadius = 6378.1370 # Earth's Radius in KM
$dLat = ($latitude2 - $latitude1) * $Rad
$dLon = ($longitude2 - $longitude1) * $Rad
$latitude1 = $latitude1 * $Rad
$latitude2 = $latitude2 * $Rad
$a = [math]::Sin($dLat / 2) * [math]::Sin($dLat / 2) + [math]::Sin($dLon / 2) * [math]::Sin($dLon / 2) * [math]::Cos($latitude1) * [math]::Cos($latitude2)
$c = 2 * [math]::ATan2([math]::Sqrt($a), [math]::Sqrt(1-$a))
$distance = [math]::Round($earthsRadius * $c * 1000, 0) #Multiple by 1000 to get metres
Return $distance
}
Scala version
def deg2rad(deg: Double) = deg * Math.PI / 180.0
def rad2deg(rad: Double) = rad / Math.PI * 180.0
def getDistanceMeters(lat1: Double, lon1: Double, lat2: Double, lon2: Double) = {
val theta = lon1 - lon2
val dist = Math.sin(deg2rad(lat1)) * Math.sin(deg2rad(lat2)) + Math.cos(deg2rad(lat1)) *
Math.cos(deg2rad(lat2)) * Math.cos(deg2rad(theta))
Math.abs(
Math.round(
rad2deg(Math.acos(dist)) * 60 * 1.1515 * 1.609344 * 1000)
)
}
Here's my implementation in Elixir
defmodule Geo do
#earth_radius_km 6371
#earth_radius_sm 3958.748
#earth_radius_nm 3440.065
#feet_per_sm 5280
#d2r :math.pi / 180
def deg_to_rad(deg), do: deg * #d2r
def great_circle_distance(p1, p2, :km), do: haversine(p1, p2) * #earth_radius_km
def great_circle_distance(p1, p2, :sm), do: haversine(p1, p2) * #earth_radius_sm
def great_circle_distance(p1, p2, :nm), do: haversine(p1, p2) * #earth_radius_nm
def great_circle_distance(p1, p2, :m), do: great_circle_distance(p1, p2, :km) * 1000
def great_circle_distance(p1, p2, :ft), do: great_circle_distance(p1, p2, :sm) * #feet_per_sm
#doc """
Calculate the [Haversine](https://en.wikipedia.org/wiki/Haversine_formula)
distance between two coordinates. Result is in radians. This result can be
multiplied by the sphere's radius in any unit to get the distance in that unit.
For example, multiple the result of this function by the Earth's radius in
kilometres and you get the distance between the two given points in kilometres.
"""
def haversine({lat1, lon1}, {lat2, lon2}) do
dlat = deg_to_rad(lat2 - lat1)
dlon = deg_to_rad(lon2 - lon1)
radlat1 = deg_to_rad(lat1)
radlat2 = deg_to_rad(lat2)
a = :math.pow(:math.sin(dlat / 2), 2) +
:math.pow(:math.sin(dlon / 2), 2) *
:math.cos(radlat1) * :math.cos(radlat2)
2 * :math.atan2(:math.sqrt(a), :math.sqrt(1 - a))
end
end
Dart Version
Haversine Algorithm.
import 'dart:math';
class GeoUtils {
static double _degreesToRadians(degrees) {
return degrees * pi / 180;
}
static double distanceInKmBetweenEarthCoordinates(lat1, lon1, lat2, lon2) {
var earthRadiusKm = 6371;
var dLat = _degreesToRadians(lat2-lat1);
var dLon = _degreesToRadians(lon2-lon1);
lat1 = _degreesToRadians(lat1);
lat2 = _degreesToRadians(lat2);
var a = sin(dLat/2) * sin(dLat/2) +
sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2);
var c = 2 * atan2(sqrt(a), sqrt(1-a));
return earthRadiusKm * c;
}
}
In Python, you can use the geopy library to compute the geodesic distance using the WGS84 ellipsoid:
from geopy.distance import geodesic
newport_ri = (41.49008, -71.312796)
cleveland_oh = (41.499498, -81.695391)
print(geodesic(newport_ri, cleveland_oh).km)
TypeScript Version
export const degreeToRadian = (degree: number) => {
return degree * Math.PI / 180;
}
export const distanceBetweenEarthCoordinatesInKm = (lat1: number, lon1: number, lat2: number, lon2: number) => {
const earthRadiusInKm = 6371;
const dLat = degreeToRadian(lat2 - lat1);
const dLon = degreeToRadian(lon2 - lon1);
lat1 = degreeToRadian(lat1);
lat2 = degreeToRadian(lat2);
const a = Math.sin(dLat / 2) * Math.sin(dLat / 2) + Math.sin(dLon / 2) * Math.sin(dLon / 2) * Math.cos(lat1) * Math.cos(lat2);
const c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
return earthRadiusInKm * c;
}
I think a version of the algorithm in R is still missing:
gpsdistance<-function(lat1,lon1,lat2,lon2){
# internal function to change deg to rad
degreesToRadians<- function (degrees) {
return (degrees * pi / 180)
}
R<-6371e3 #radius of Earth in meters
phi1<-degreesToRadians(lat1) # latitude 1
phi2<-degreesToRadians(lat2) # latitude 2
lambda1<-degreesToRadians(lon1) # longitude 1
lambda2<-degreesToRadians(lon2) # longitude 2
delta_phi<-phi1-phi2 # latitude-distance
delta_lambda<-lambda1-lambda2 # longitude-distance
a<-sin(delta_phi/2)*sin(delta_phi/2)+
cos(phi1)*cos(phi2)*sin(delta_lambda/2)*
sin(delta_lambda/2)
cc<-2*atan2(sqrt(a),sqrt(1-a))
distance<- R * cc
return(distance) # in meters
}
For java
public static double degreesToRadians(double degrees) {
return degrees * Math.PI / 180;
}
public static double distanceInKmBetweenEarthCoordinates(Location location1, Location location2) {
double earthRadiusKm = 6371;
double dLat = degreesToRadians(location2.getLatitude()-location1.getLatitude());
double dLon = degreesToRadians(location2.getLongitude()-location1.getLongitude());
double lat1 = degreesToRadians(location1.getLatitude());
double lat2 = degreesToRadians(location2.getLatitude());
double a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.sin(dLon/2) * Math.sin(dLon/2) * Math.cos(lat1) * Math.cos(lat2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
return earthRadiusKm * c;
}
For anyone searching for a Delphi/Pascal version:
function GreatCircleDistance(const Lat1, Long1, Lat2, Long2: Double): Double;
var
Lat1Rad, Long1Rad, Lat2Rad, Long2Rad: Double;
const
EARTH_RADIUS_KM = 6378;
begin
Lat1Rad := DegToRad(Lat1);
Long1Rad := DegToRad(Long1);
Lat2Rad := DegToRad(Lat2);
Long2Rad := DegToRad(Long2);
Result := EARTH_RADIUS_KM * ArcCos(Cos(Lat1Rad) * Cos(Lat2Rad) * Cos(Long1Rad - Long2Rad) + Sin(Lat1Rad) * Sin(Lat2Rad));
end;
I take no credit for this code, I originally found it posted by Gary William on a public forum.