How do I calculate the PDF of a bivariate normal distribution? - c++

I have a set of points and extract a small subset of them for calculating a bivariate normal distribution. Afterwards I check all other points if they fit in this distribution by calculating the PDF for every point and rejecting points with a value below some threshold.
So much about the theory...
The PDF has according to wikipedia the formula:
σ is the standard deviation and μ is the mean, calculated as following:
cv::Scalar mean;
cv::Scalar stdDev;
dataPoints = dataPoints.reshape(3); // convert 3 columns to 3 channels
cv::meanStdDev(dataPoints, mean, stdDev);
dataPoints = dataPoints.reshape(1); // convert back
meanX = mean.val[0];
meanY = mean.val[1];
sigmaX = stdDev.val[0];
sigmaY = stdDev.val[1];
dataPoints is a cv::Mat with 3 columns of floats (x, y, index).
ρ is the correlation coefficient which I calculate like this:
cv::matchTemplate(dataPoints.col(0), dataPoints.col(1), rho, cv::TM_CCOEFF_NORMED);
The last step is calculating the the probability for each point using this:
double p = (1. / (2. * M_PI * sigmaX * sigmaY * sqrt(1. - pow(rho, 2))));
double e = exp((-1. / 2.) * D(x, y, rho));
double ret = p * e;
And D() should be as far as I know the Mahalanobis Distance, but the formula from OpenCV cv::Mahalanobis(x, y, rho) returns another value than when I calculate it myself:
double cX = (x - meanX) / sigmaX;
double cY = (y - meanY) / sigmaY;
double a = (1. / (1. - pow(rho, 2)));
double b = (pow(cX, 2) + pow(cY, 2) - 2. * rho * cX * cY);
double ret = a * b;
So and now my Problem:
As far as I know the integral over the PDF should be 1 and the maximum value of the PDF should be at (meanX, meanY), so when σ would be 0 the PDF at mean should be 1. But with the computations above I can get values over 1. What do I get wrong?

Related

Finding Perpendicular Points Given An Angle With Piece-wise Hermite Splines

I am given a Hermite spline from which I want to create another spline with every point on that spline being exactly x distance away.
Here's an example of what I want to do:
.
I can find every derivative and point on the original spline. I also know all the coefficients of each polynomial.
Here's the code that I've came up with that does this for every control point of the original spline. Where controlPath[i] is a vector of control points that makeup the spline, and Point is a struct representing a 2D point with its facing angle.
double x, y, a;
a = controlPath[i].Angle + 90;
x = x * cosf(a * (PI / 180)) + controlPath[i].X;
y = x * sinf(a * (PI / 180)) + controlPath[i].Y;
Point l(x, y, a - 90);
a = controlPath[i].Angle - 90;
x = x * cosf(a * (PI / 180)) + controlPath[i].X;
y = x * sinf(a * (PI / 180)) + controlPath[i].Y;
Point r(x, y, a + 90);
This method work to an extent, but its results are subpar.
Result of this method using input:
The inaccuracy is not good. How do I confront this issue?
If you build normals of given length in every point of Hermite spline and connect endpoint of these normals, resulting curve (so-called parallel curve) is not Hermit spline in general case. The same is true for Bezier curve and the most of pther curve (only circle arc generates self-similar curve and some exotic curves).
So to generate reliable result, it is worth to subdivide curve into small pieces, build normals in all intermediate points and generate smooth piecewise splines through "parallel points"
Also note doubtful using x in the right part of formulas - should be some distance.
Also you don't need to calculate sin/cos twice
double x, y, a, d, c, s;
a = controlPath[i].Angle + 90;
c = d * cosf(a * (PI / 180));
s = d * sinf(a * (PI / 180))
x = c + controlPath[i].X;
y = s + controlPath[i].Y;
Point l(x, y, controlPath[i].Angle);
x = -c + controlPath[i].X;
y = -s + controlPath[i].Y;
Point l(x, y, controlPath[i].Angle);

How to fit a 2D ellipse to given points

I would like to fit a 2D array by an elliptic function: (x / a)² + (y / b)² = 1 ----> (and so get the a and b)
And then, be able to replot it on my graph.
I found many examples on internet, but no one with this simple Cartesian equation. I probably have searched badly ! I think a basic solution for this problem could help many people.
Here is an example of the data:
Sadly, I can not put the values... So let's assume that I have an X,Y arrays defining the coordinates of each of those points.
This can be solved directly using least squares. You can frame this as minimizing the sum of squares of quantity (alpha * x_i^2 + beta * y_i^2 - 1) where alpha is 1/a^2 and beta is 1/b^2. You have all the x_i's in X and the y_i's in Y so you can find the minimizer of ||Ax - b||^2 where A is an Nx2 matrix (i.e. [X^2, Y^2]), x is the column vector [alpha; beta] and b is column vector of all ones.
The following code solves the more general problem for an ellipse of the form Ax^2 + Bxy + Cy^2 + Dx +Ey = 1 though the idea is exactly the same. The print statement gives 0.0776x^2 + 0.0315xy+0.125y^2+0.00457x+0.00314y = 1 and the image of the ellipse generated is also below
import numpy as np
import matplotlib.pyplot as plt
alpha = 5
beta = 3
N = 500
DIM = 2
np.random.seed(2)
# Generate random points on the unit circle by sampling uniform angles
theta = np.random.uniform(0, 2*np.pi, (N,1))
eps_noise = 0.2 * np.random.normal(size=[N,1])
circle = np.hstack([np.cos(theta), np.sin(theta)])
# Stretch and rotate circle to an ellipse with random linear tranformation
B = np.random.randint(-3, 3, (DIM, DIM))
noisy_ellipse = circle.dot(B) + eps_noise
# Extract x coords and y coords of the ellipse as column vectors
X = noisy_ellipse[:,0:1]
Y = noisy_ellipse[:,1:]
# Formulate and solve the least squares problem ||Ax - b ||^2
A = np.hstack([X**2, X * Y, Y**2, X, Y])
b = np.ones_like(X)
x = np.linalg.lstsq(A, b)[0].squeeze()
# Print the equation of the ellipse in standard form
print('The ellipse is given by {0:.3}x^2 + {1:.3}xy+{2:.3}y^2+{3:.3}x+{4:.3}y = 1'.format(x[0], x[1],x[2],x[3],x[4]))
# Plot the noisy data
plt.scatter(X, Y, label='Data Points')
# Plot the original ellipse from which the data was generated
phi = np.linspace(0, 2*np.pi, 1000).reshape((1000,1))
c = np.hstack([np.cos(phi), np.sin(phi)])
ground_truth_ellipse = c.dot(B)
plt.plot(ground_truth_ellipse[:,0], ground_truth_ellipse[:,1], 'k--', label='Generating Ellipse')
# Plot the least squares ellipse
x_coord = np.linspace(-5,5,300)
y_coord = np.linspace(-5,5,300)
X_coord, Y_coord = np.meshgrid(x_coord, y_coord)
Z_coord = x[0] * X_coord ** 2 + x[1] * X_coord * Y_coord + x[2] * Y_coord**2 + x[3] * X_coord + x[4] * Y_coord
plt.contour(X_coord, Y_coord, Z_coord, levels=[1], colors=('r'), linewidths=2)
plt.legend()
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
Following the suggestion by ErroriSalvo, here is the complete process of fitting an ellipse using the SVD. The arrays x, y are coordinates of the given points, let's say there are N points. Then U, S, V are obtained from the SVD of the centered coordinate array of shape (2, N). So, U is a 2 by 2 orthogonal matrix (rotation), S is a vector of length 2 (singular values), and V, which we do not need, is an N by N orthogonal matrix.
The linear map transforming the unit circle to the ellipse of best fit is
sqrt(2/N) * U * diag(S)
where diag(S) is the diagonal matrix with singular values on the diagonal. To see why the factor of sqrt(2/N) is needed, imagine that the points x, y are taken uniformly from the unit circle. Then sum(x**2) + sum(y**2) is N, and so the coordinate matrix consists of two orthogonal rows of length sqrt(N/2), hence its norm (the largest singular value) is sqrt(N/2). We need to bring this down to 1 to have the unit circle.
N = 300
t = np.linspace(0, 2*np.pi, N)
x = 5*np.cos(t) + 0.2*np.random.normal(size=N) + 1
y = 4*np.sin(t+0.5) + 0.2*np.random.normal(size=N)
plt.plot(x, y, '.') # given points
xmean, ymean = x.mean(), y.mean()
x -= xmean
y -= ymean
U, S, V = np.linalg.svd(np.stack((x, y)))
tt = np.linspace(0, 2*np.pi, 1000)
circle = np.stack((np.cos(tt), np.sin(tt))) # unit circle
transform = np.sqrt(2/N) * U.dot(np.diag(S)) # transformation matrix
fit = transform.dot(circle) + np.array([[xmean], [ymean]])
plt.plot(fit[0, :], fit[1, :], 'r')
plt.show()
But if you assume that there is no rotation, then np.sqrt(2/N) * S is all you need; these are a and b in the equation of the ellipse.
You could try a Singular Value Decomposition of the data matrix.
https://docs.scipy.org/doc/numpy-1.13.0/reference/generated/numpy.linalg.svd.html
First center the data by subtracting mean values of X,Y from each column respectively.
X=X-np.mean(X)
Y=Y-np.mean(Y)
D=np.vstack(X,Y)
Then, apply SVD and extract
-eigenvalues (members of s) -> axis length
-eigenvectors(U) -> axis orientation
U, s, V = np.linalg.svd(D, full_matrices=True)
This should be a least-squares fit.
Of course, things can get more complicated than this, please see
https://www.emis.de/journals/BBMS/Bulletin/sup962/gander.pdf

Integrate function

I have this function to reach a certain 1 dimensional value accelerated and damped with overshoot. That is: given an inital value, a velocity and a acceleration (force/mass), the target value is attained by accelerating to it and gets increasingly damped while getting closer to the target value.
This all works fine, howver If i want to know what the TotalAngle is after time 't' I have to run this function say N steps with a 'small' dt to find the 'limit'.
I was wondering If i can (and how) to intergrate over dt so that the TotalAngle can be determined given a time 't' initially.
Regards, Tanks for any help.
dt = delta time step per frame
input = 1
TotalAngle = 0 at t=0
Velocity = 0 at t=0
void FAccelDampedWithOvershoot::Update(float dt, float input, float& Velocity, float& TotalAngle)
{
const float Force = 500000.f;
const float DampForce = 5000.f;
const float MaxAngle = 45.f;
const float InvMass = 1.f / 162400.f;
float target = MaxAngle * input;
float ratio = (target - TotalAngle) / MaxAngle;
float fMove = Force * ratio;
float fDamp = -Velocity * DampForce;
Velocity += (fMove + fDamp) * invMass * dt;
TotalAngle += Velocity * dt;
}
Updated with fixed bugs in math
Originally I've lost mass and MaxAngle a few times. This is why you should first solve it on a paper and then enter to the SO rather than trying to solve it in the text editor.
Anyway, I've fixed the math and now it seems to work reasonably well. I put fixed solution just over previous one.
Well, this looks like a Newtonian mechanics which means differential equations. Let's try to solve them.
SO is not very friendly to math formulas and I'm a bit bored to type characters so here is what I use:
F = Force
Fd = DampForce
MA = MaxAngle
A= TotalAngle
v = Velocity
m = 1 / InvMass
' for derivative i.e. something' is 1-st derivative of something by t and something'' is 2-nd derivative
if I divide you last two lines of code by dt and merge in all the other lines I can get (I also assume that input = 1 as other case is obviously symmetrical)
v' = ([F * (1 - A / MA)] - v * Fd) / m
and applying A' = v we get
m * A'' = F(1 - A/MA) - Fd * A'
or moving to one side we get a simple 2-nd order differential equation
m * A'' + Fd * A' + F/MA * A = F
IIRC, the way to solve it is to first solve characteristic equation which here is
m * x^2 + Fd * x + F/MA = 0
x[1,2] = (-Fd +/- sqrt(Fd^2 - 4*F*m/MA))/ (2*m)
I expect that part under sqrt i.e. (Fd^2 - 4*F*m/MA) is negative thus solution should be of the following form. Let
Dm = Fd/(2*m)
K = sqrt(F/MA/m - Dm^2)
(note the negated value under sqrt so it works now) then
A(t) = e^(-Dm*t) * [P * sin(K*t) + Q * cos(K*t)] + C
where P, Q and C are some constants.
The solution is easier to find as a sum of two solutions: some specific solution for
m * A'' + Fd * A' + F/MA * A = F
and a general solution for homogeneou
m * A'' + Fd * A' + F/MA * A = 0
that makes original conditions fit. Obviously specific solution A(t) = MA works and thus C = MA. So now we need to fit P and Q of general solution to match starting conditions. To find them we need
A(0) = - MA
A'(0) = V(0) = 0
Given that e^0 = 1, sin(0) = 0 and cos(0) = 1 you get something like
Q = -MA
P = 0
or
P = 0
Q = - MA
C = MA
thus
A(t) = MA * [1 - e^(-Dm*t) * cos(K*t)]
where
Dm = Fd/(2*m)
K = sqrt(F/MA/m - Dm^2)
which kind of makes sense given your task.
Note also that this equation assumes that everything happens in radians rather than degrees (i.e. derivative of [sin(t)]' is just cos(t)) so you should transform all your constants accordingly or transform the solution.
const float Force = 500000.f * M_PI / 180;
const float DampForce = 5000.f * M_PI / 180;
const float MaxAngle = M_PI_4;
which on my machine produces
Dm = 0.000268677541
K = 0.261568546
This seems to be similar to original funcion is I step with dt = 0.01f and the main obstacle seems to be precision loss because of float
Hope this helps!
This is not a full answer and I am sure someone else can work it out, but there is no room in the comments and it may help you find a better solution.
The image below shows the velocity (blue) as your function integrates at time steps 1. The red shows the function below that calculates the value for time t
The function F(t)
F(t) = sin((t / f) * pi * 2) * (1 / (((t / f) + a) ^ c)) * b
With f = 23.7, a = 1.4, c = 2, and b= 50 that give the red plot in the image above
All the values are just approximation.
f determines the frequency and is close to a match,
a,b,c control the falloff in amplitude and are a by eye guestimate.
If it does not matter that you have a perfect match then this will work for you. totalAngle uses the same function but t has 0.25 added to it. Unfortunately I did not get any values for a,b,c for totalAngle and I did notice that it was offset so you will have to add the offset value d (I normalised everything so have no idea what the range of totalAngle was)
Function F(t) for totalAngle
F(t) = sin(((t+0.25) / f) * pi * 2) * (1 / ((((t+0.25) / f) + a) ^ c)) * b + d
Sorry only have f = 23.7, c= 2, a~1.4 nothing for b=? d=?

Distance between 2 decimal points [duplicate]

How do I calculate the distance between two points specified by latitude and longitude?
For clarification, I'd like the distance in kilometers; the points use the WGS84 system and I'd like to understand the relative accuracies of the approaches available.
This link might be helpful to you, as it details the use of the Haversine formula to calculate the distance.
Excerpt:
This script [in Javascript] calculates great-circle distances between the two points –
that is, the shortest distance over the earth’s surface – using the
‘Haversine’ formula.
function getDistanceFromLatLonInKm(lat1,lon1,lat2,lon2) {
var R = 6371; // Radius of the earth in km
var dLat = deg2rad(lat2-lat1); // deg2rad below
var dLon = deg2rad(lon2-lon1);
var 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)
;
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c; // Distance in km
return d;
}
function deg2rad(deg) {
return deg * (Math.PI/180)
}
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, pi
def distance(lat1, lon1, lat2, lon2):
p = pi/180
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)) #2*R*asin...
And for the sake of completeness: Haversine on Wikipedia.
Here is a C# Implementation:
static class DistanceAlgorithm
{
const double PIx = 3.141592653589793;
const double RADIUS = 6378.16;
/// <summary>
/// Convert degrees to Radians
/// </summary>
/// <param name="x">Degrees</param>
/// <returns>The equivalent in radians</returns>
public static double Radians(double x)
{
return x * PIx / 180;
}
/// <summary>
/// Calculate the distance between two places.
/// </summary>
/// <param name="lon1"></param>
/// <param name="lat1"></param>
/// <param name="lon2"></param>
/// <param name="lat2"></param>
/// <returns></returns>
public static double DistanceBetweenPlaces(
double lon1,
double lat1,
double lon2,
double lat2)
{
double dlon = Radians(lon2 - lon1);
double dlat = Radians(lat2 - lat1);
double a = (Math.Sin(dlat / 2) * Math.Sin(dlat / 2)) + Math.Cos(Radians(lat1)) * Math.Cos(Radians(lat2)) * (Math.Sin(dlon / 2) * Math.Sin(dlon / 2));
double angle = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
return angle * RADIUS;
}
}
Here is a java implementation of the Haversine formula.
public final static double AVERAGE_RADIUS_OF_EARTH_KM = 6371;
public int calculateDistanceInKilometer(double userLat, double userLng,
double venueLat, double venueLng) {
double latDistance = Math.toRadians(userLat - venueLat);
double lngDistance = Math.toRadians(userLng - venueLng);
double a = Math.sin(latDistance / 2) * Math.sin(latDistance / 2)
+ Math.cos(Math.toRadians(userLat)) * Math.cos(Math.toRadians(venueLat))
* Math.sin(lngDistance / 2) * Math.sin(lngDistance / 2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
return (int) (Math.round(AVERAGE_RADIUS_OF_EARTH_KM * c));
}
Note that here we are rounding the answer to the nearest km.
Thanks very much for all this. I used the following code in my Objective-C iPhone app:
const double PIx = 3.141592653589793;
const double RADIO = 6371; // Mean radius of Earth in Km
double convertToRadians(double val) {
return val * PIx / 180;
}
-(double)kilometresBetweenPlace1:(CLLocationCoordinate2D) place1 andPlace2:(CLLocationCoordinate2D) place2 {
double dlon = convertToRadians(place2.longitude - place1.longitude);
double dlat = convertToRadians(place2.latitude - place1.latitude);
double a = ( pow(sin(dlat / 2), 2) + cos(convertToRadians(place1.latitude))) * cos(convertToRadians(place2.latitude)) * pow(sin(dlon / 2), 2);
double angle = 2 * asin(sqrt(a));
return angle * RADIO;
}
Latitude and Longitude are in decimal. I didn't use min() for the asin() call as the distances that I'm using are so small that they don't require it.
It gave incorrect answers until I passed in the values in Radians - now it's pretty much the same as the values obtained from Apple's Map app :-)
Extra update:
If you are using iOS4 or later then Apple provide some methods to do this so the same functionality would be achieved with:
-(double)kilometresBetweenPlace1:(CLLocationCoordinate2D) place1 andPlace2:(CLLocationCoordinate2D) place2 {
MKMapPoint start, finish;
start = MKMapPointForCoordinate(place1);
finish = MKMapPointForCoordinate(place2);
return MKMetersBetweenMapPoints(start, finish) / 1000;
}
This is a simple PHP function that will give a very reasonable approximation (under +/-1% error margin).
<?php
function distance($lat1, $lon1, $lat2, $lon2) {
$pi80 = M_PI / 180;
$lat1 *= $pi80;
$lon1 *= $pi80;
$lat2 *= $pi80;
$lon2 *= $pi80;
$r = 6372.797; // mean radius of Earth in km
$dlat = $lat2 - $lat1;
$dlon = $lon2 - $lon1;
$a = sin($dlat / 2) * sin($dlat / 2) + cos($lat1) * cos($lat2) * sin($dlon / 2) * sin($dlon / 2);
$c = 2 * atan2(sqrt($a), sqrt(1 - $a));
$km = $r * $c;
//echo '<br/>'.$km;
return $km;
}
?>
As said before; the earth is NOT a sphere. It is like an old, old baseball that Mark McGwire decided to practice with - it is full of dents and bumps. The simpler calculations (like this) treat it like a sphere.
Different methods may be more or less precise according to where you are on this irregular ovoid AND how far apart your points are (the closer they are the smaller the absolute error margin). The more precise your expectation, the more complex the math.
For more info: wikipedia geographic distance
I post here my working example.
List all points in table having distance between a designated point (we use a random point - lat:45.20327, long:23.7806) less than 50 KM, with latitude & longitude, in MySQL (the table fields are coord_lat and coord_long):
List all having DISTANCE<50, in Kilometres (considered Earth radius 6371 KM):
SELECT denumire, (6371 * acos( cos( radians(45.20327) ) * cos( radians( coord_lat ) ) * cos( radians( 23.7806 ) - radians(coord_long) ) + sin( radians(45.20327) ) * sin( radians(coord_lat) ) )) AS distanta
FROM obiective
WHERE coord_lat<>''
AND coord_long<>''
HAVING distanta<50
ORDER BY distanta desc
The above example was tested in MySQL 5.0.95 and 5.5.16 (Linux).
In the other answers an implementation in r is missing.
Calculating the distance between two point is quite straightforward with the distm function from the geosphere package:
distm(p1, p2, fun = distHaversine)
where:
p1 = longitude/latitude for point(s)
p2 = longitude/latitude for point(s)
# type of distance calculation
fun = distCosine / distHaversine / distVincentySphere / distVincentyEllipsoid
As the earth is not perfectly spherical, the Vincenty formula for ellipsoids is probably the best way to calculate distances. Thus in the geosphere package you use then:
distm(p1, p2, fun = distVincentyEllipsoid)
Off course you don't necessarily have to use geosphere package, you can also calculate the distance in base R with a function:
hav.dist <- function(long1, lat1, long2, lat2) {
R <- 6371
diff.long <- (long2 - long1)
diff.lat <- (lat2 - lat1)
a <- sin(diff.lat/2)^2 + cos(lat1) * cos(lat2) * sin(diff.long/2)^2
b <- 2 * asin(pmin(1, sqrt(a)))
d = R * b
return(d)
}
The haversine is definitely a good formula for probably most cases, other answers already include it so I am not going to take the space. But it is important to note that no matter what formula is used (yes not just one). Because of the huge range of accuracy possible as well as the computation time required. The choice of formula requires a bit more thought than a simple no brainer answer.
This posting from a person at nasa, is the best one I found at discussing the options
http://www.cs.nyu.edu/visual/home/proj/tiger/gisfaq.html
For example, if you are just sorting rows by distance in a 100 miles radius. The flat earth formula will be much faster than the haversine.
HalfPi = 1.5707963;
R = 3956; /* the radius gives you the measurement unit*/
a = HalfPi - latoriginrad;
b = HalfPi - latdestrad;
u = a * a + b * b;
v = - 2 * a * b * cos(longdestrad - longoriginrad);
c = sqrt(abs(u + v));
return R * c;
Notice there is just one cosine and one square root. Vs 9 of them on the Haversine formula.
There could be a simpler solution, and more correct: The perimeter of earth is 40,000Km at the equator, about 37,000 on Greenwich (or any longitude) cycle. Thus:
pythagoras = function (lat1, lon1, lat2, lon2) {
function sqr(x) {return x * x;}
function cosDeg(x) {return Math.cos(x * Math.PI / 180.0);}
var earthCyclePerimeter = 40000000.0 * cosDeg((lat1 + lat2) / 2.0);
var dx = (lon1 - lon2) * earthCyclePerimeter / 360.0;
var dy = 37000000.0 * (lat1 - lat2) / 360.0;
return Math.sqrt(sqr(dx) + sqr(dy));
};
I agree that it should be fine-tuned as, I myself said that it's an ellipsoid, so the radius to be multiplied by the cosine varies. But it's a bit more accurate. Compared with Google Maps and it did reduce the error significantly.
pip install haversine
Python implementation
Origin is the center of the contiguous United States.
from haversine import haversine, Unit
origin = (39.50, 98.35)
paris = (48.8567, 2.3508)
haversine(origin, paris, unit=Unit.MILES)
To get the answer in kilometers simply set unit=Unit.KILOMETERS (that's the default).
There is some errors in the code provided, I've fixed it below.
All the above answers assumes the earth is a sphere. However, a more accurate approximation would be that of an oblate spheroid.
a= 6378.137#equitorial radius in km
b= 6356.752#polar radius in km
def Distance(lat1, lons1, lat2, lons2):
lat1=math.radians(lat1)
lons1=math.radians(lons1)
R1=(((((a**2)*math.cos(lat1))**2)+(((b**2)*math.sin(lat1))**2))/((a*math.cos(lat1))**2+(b*math.sin(lat1))**2))**0.5 #radius of earth at lat1
x1=R1*math.cos(lat1)*math.cos(lons1)
y1=R1*math.cos(lat1)*math.sin(lons1)
z1=R1*math.sin(lat1)
lat2=math.radians(lat2)
lons2=math.radians(lons2)
R2=(((((a**2)*math.cos(lat2))**2)+(((b**2)*math.sin(lat2))**2))/((a*math.cos(lat2))**2+(b*math.sin(lat2))**2))**0.5 #radius of earth at lat2
x2=R2*math.cos(lat2)*math.cos(lons2)
y2=R2*math.cos(lat2)*math.sin(lons2)
z2=R2*math.sin(lat2)
return ((x1-x2)**2+(y1-y2)**2+(z1-z2)**2)**0.5
I don't like adding yet another answer, but the Google maps API v.3 has spherical geometry (and more). After converting your WGS84 to decimal degrees you can do this:
<script src="http://maps.google.com/maps/api/js?sensor=false&libraries=geometry" type="text/javascript"></script>
distance = google.maps.geometry.spherical.computeDistanceBetween(
new google.maps.LatLng(fromLat, fromLng),
new google.maps.LatLng(toLat, toLng));
No word about how accurate Google's calculations are or even what model is used (though it does say "spherical" rather than "geoid". By the way, the "straight line" distance will obviously be different from the distance if one travels on the surface of the earth which is what everyone seems to be presuming.
You can use the build in CLLocationDistance to calculate this:
CLLocation *location1 = [[CLLocation alloc] initWithLatitude:latitude1 longitude:longitude1];
CLLocation *location2 = [[CLLocation alloc] initWithLatitude:latitude2 longitude:longitude2];
[self distanceInMetersFromLocation:location1 toLocation:location2]
- (int)distanceInMetersFromLocation:(CLLocation*)location1 toLocation:(CLLocation*)location2 {
CLLocationDistance distanceInMeters = [location1 distanceFromLocation:location2];
return distanceInMeters;
}
In your case if you want kilometers just divide by 1000.
As pointed out, an accurate calculation should take into account that the earth is not a perfect sphere. Here are some comparisons of the various algorithms offered here:
geoDistance(50,5,58,3)
Haversine: 899 km
Maymenn: 833 km
Keerthana: 897 km
google.maps.geometry.spherical.computeDistanceBetween(): 900 km
geoDistance(50,5,-58,-3)
Haversine: 12030 km
Maymenn: 11135 km
Keerthana: 10310 km
google.maps.geometry.spherical.computeDistanceBetween(): 12044 km
geoDistance(.05,.005,.058,.003)
Haversine: 0.9169 km
Maymenn: 0.851723 km
Keerthana: 0.917964 km
google.maps.geometry.spherical.computeDistanceBetween(): 0.917964 km
geoDistance(.05,80,.058,80.3)
Haversine: 33.37 km
Maymenn: 33.34 km
Keerthana: 33.40767 km
google.maps.geometry.spherical.computeDistanceBetween(): 33.40770 km
Over small distances, Keerthana's algorithm does seem to coincide with that of Google Maps. Google Maps does not seem to follow any simple algorithm, suggesting that it may be the most accurate method here.
Anyway, here is a Javascript implementation of Keerthana's algorithm:
function geoDistance(lat1, lng1, lat2, lng2){
const a = 6378.137; // equitorial radius in km
const b = 6356.752; // polar radius in km
var sq = x => (x*x);
var sqr = x => Math.sqrt(x);
var cos = x => Math.cos(x);
var sin = x => Math.sin(x);
var radius = lat => sqr((sq(a*a*cos(lat))+sq(b*b*sin(lat)))/(sq(a*cos(lat))+sq(b*sin(lat))));
lat1 = lat1 * Math.PI / 180;
lng1 = lng1 * Math.PI / 180;
lat2 = lat2 * Math.PI / 180;
lng2 = lng2 * Math.PI / 180;
var R1 = radius(lat1);
var x1 = R1*cos(lat1)*cos(lng1);
var y1 = R1*cos(lat1)*sin(lng1);
var z1 = R1*sin(lat1);
var R2 = radius(lat2);
var x2 = R2*cos(lat2)*cos(lng2);
var y2 = R2*cos(lat2)*sin(lng2);
var z2 = R2*sin(lat2);
return sqr(sq(x1-x2)+sq(y1-y2)+sq(z1-z2));
}
Here is a typescript implementation of the Haversine formula
static getDistanceFromLatLonInKm(lat1: number, lon1: number, lat2: number, lon2: number): number {
var deg2Rad = deg => {
return deg * Math.PI / 180;
}
var r = 6371; // Radius of the earth in km
var dLat = deg2Rad(lat2 - lat1);
var dLon = deg2Rad(lon2 - lon1);
var 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);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
var d = r * c; // Distance in km
return d;
}
Here is the SQL Implementation to calculate the distance in km,
SELECT UserId, ( 3959 * acos( cos( radians( your latitude here ) ) * cos( radians(latitude) ) *
cos( radians(longitude) - radians( your longitude here ) ) + sin( radians( your latitude here ) ) *
sin( radians(latitude) ) ) ) AS distance FROM user HAVING
distance < 5 ORDER BY distance LIMIT 0 , 5;
For further details in the implementation by programming langugage, you can just go through the php script given here
This script [in PHP] calculates distances between the two points.
public static function getDistanceOfTwoPoints($source, $dest, $unit='K') {
$lat1 = $source[0];
$lon1 = $source[1];
$lat2 = $dest[0];
$lon2 = $dest[1];
$theta = $lon1 - $lon2;
$dist = sin(deg2rad($lat1)) * sin(deg2rad($lat2)) + cos(deg2rad($lat1)) * cos(deg2rad($lat2)) * cos(deg2rad($theta));
$dist = acos($dist);
$dist = rad2deg($dist);
$miles = $dist * 60 * 1.1515;
$unit = strtoupper($unit);
if ($unit == "K") {
return ($miles * 1.609344);
}
else if ($unit == "M")
{
return ($miles * 1.609344 * 1000);
}
else if ($unit == "N") {
return ($miles * 0.8684);
}
else {
return $miles;
}
}
here is an example in postgres sql (in km, for miles version, replace 1.609344 by 0.8684 version)
CREATE OR REPLACE FUNCTION public.geodistance(alat float, alng float, blat
float, blng float)
RETURNS float AS
$BODY$
DECLARE
v_distance float;
BEGIN
v_distance = asin( sqrt(
sin(radians(blat-alat)/2)^2
+ (
(sin(radians(blng-alng)/2)^2) *
cos(radians(alat)) *
cos(radians(blat))
)
)
) * cast('7926.3352' as float) * cast('1.609344' as float) ;
RETURN v_distance;
END
$BODY$
language plpgsql VOLATILE SECURITY DEFINER;
alter function geodistance(alat float, alng float, blat float, blng float)
owner to postgres;
Java implementation in according Haversine formula
double calculateDistance(double latPoint1, double lngPoint1,
double latPoint2, double lngPoint2) {
if(latPoint1 == latPoint2 && lngPoint1 == lngPoint2) {
return 0d;
}
final double EARTH_RADIUS = 6371.0; //km value;
//converting to radians
latPoint1 = Math.toRadians(latPoint1);
lngPoint1 = Math.toRadians(lngPoint1);
latPoint2 = Math.toRadians(latPoint2);
lngPoint2 = Math.toRadians(lngPoint2);
double distance = Math.pow(Math.sin((latPoint2 - latPoint1) / 2.0), 2)
+ Math.cos(latPoint1) * Math.cos(latPoint2)
* Math.pow(Math.sin((lngPoint2 - lngPoint1) / 2.0), 2);
distance = 2.0 * EARTH_RADIUS * Math.asin(Math.sqrt(distance));
return distance; //km value
}
I made a custom function in R to calculate haversine distance(km) between two spatial points using functions available in R base package.
custom_hav_dist <- function(lat1, lon1, lat2, lon2) {
R <- 6371
Radian_factor <- 0.0174533
lat_1 <- (90-lat1)*Radian_factor
lat_2 <- (90-lat2)*Radian_factor
diff_long <-(lon1-lon2)*Radian_factor
distance_in_km <- 6371*acos((cos(lat_1)*cos(lat_2))+
(sin(lat_1)*sin(lat_2)*cos(diff_long)))
rm(lat1, lon1, lat2, lon2)
return(distance_in_km)
}
Sample output
custom_hav_dist(50.31,19.08,54.14,19.39)
[1] 426.3987
PS: To calculate distances in miles, substitute R in function (6371) with 3958.756 (and for nautical miles, use 3440.065).
To calculate the distance between two points on a sphere you need to do the Great Circle calculation.
There are a number of C/C++ libraries to help with map projection at MapTools if you need to reproject your distances to a flat surface. To do this you will need the projection string of the various coordinate systems.
You may also find MapWindow a useful tool to visualise the points. Also as its open source its a useful guide to how to use the proj.dll library, which appears to be the core open source projection library.
Here is my java implementation for calculation distance via decimal degrees after some search. I used mean radius of world (from wikipedia) in km. İf you want result miles then use world radius in miles.
public static double distanceLatLong2(double lat1, double lng1, double lat2, double lng2)
{
double earthRadius = 6371.0d; // KM: use mile here if you want mile result
double dLat = toRadian(lat2 - lat1);
double dLng = toRadian(lng2 - lng1);
double a = Math.pow(Math.sin(dLat/2), 2) +
Math.cos(toRadian(lat1)) * Math.cos(toRadian(lat2)) *
Math.pow(Math.sin(dLng/2), 2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
return earthRadius * c; // returns result kilometers
}
public static double toRadian(double degrees)
{
return (degrees * Math.PI) / 180.0d;
}
Here's the accepted answer implementation ported to Java in case anyone needs it.
package com.project529.garage.util;
/**
* Mean radius.
*/
private static double EARTH_RADIUS = 6371;
/**
* Returns the distance between two sets of latitudes and longitudes in meters.
* <p/>
* Based from the following JavaScript SO answer:
* http://stackoverflow.com/questions/27928/calculate-distance-between-two-latitude-longitude-points-haversine-formula,
* which is based on https://en.wikipedia.org/wiki/Haversine_formula (error rate: ~0.55%).
*/
public double getDistanceBetween(double lat1, double lon1, double lat2, double lon2) {
double dLat = toRadians(lat2 - lat1);
double dLon = toRadians(lon2 - lon1);
double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
Math.cos(toRadians(lat1)) * Math.cos(toRadians(lat2)) *
Math.sin(dLon / 2) * Math.sin(dLon / 2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
double d = EARTH_RADIUS * c;
return d;
}
public double toRadians(double degrees) {
return degrees * (Math.PI / 180);
}
For those looking for an Excel formula based on WGS-84 & GRS-80 standards:
=ACOS(COS(RADIANS(90-Lat1))*COS(RADIANS(90-Lat2))+SIN(RADIANS(90-Lat1))*SIN(RADIANS(90-Lat2))*COS(RADIANS(Long1-Long2)))*6371
Source
there is a good example in here to calculate distance with PHP http://www.geodatasource.com/developers/php :
function distance($lat1, $lon1, $lat2, $lon2, $unit) {
$theta = $lon1 - $lon2;
$dist = sin(deg2rad($lat1)) * sin(deg2rad($lat2)) + cos(deg2rad($lat1)) * cos(deg2rad($lat2)) * cos(deg2rad($theta));
$dist = acos($dist);
$dist = rad2deg($dist);
$miles = $dist * 60 * 1.1515;
$unit = strtoupper($unit);
if ($unit == "K") {
return ($miles * 1.609344);
} else if ($unit == "N") {
return ($miles * 0.8684);
} else {
return $miles;
}
}
Here is the implementation VB.NET, this implementation will give you the result in KM or Miles based on an Enum value you pass.
Public Enum DistanceType
Miles
KiloMeters
End Enum
Public Structure Position
Public Latitude As Double
Public Longitude As Double
End Structure
Public Class Haversine
Public Function Distance(Pos1 As Position,
Pos2 As Position,
DistType As DistanceType) As Double
Dim R As Double = If((DistType = DistanceType.Miles), 3960, 6371)
Dim dLat As Double = Me.toRadian(Pos2.Latitude - Pos1.Latitude)
Dim dLon As Double = Me.toRadian(Pos2.Longitude - Pos1.Longitude)
Dim a As Double = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) + Math.Cos(Me.toRadian(Pos1.Latitude)) * Math.Cos(Me.toRadian(Pos2.Latitude)) * Math.Sin(dLon / 2) * Math.Sin(dLon / 2)
Dim c As Double = 2 * Math.Asin(Math.Min(1, Math.Sqrt(a)))
Dim result As Double = R * c
Return result
End Function
Private Function toRadian(val As Double) As Double
Return (Math.PI / 180) * val
End Function
End Class
I condensed the computation down by simplifying the formula.
Here it is in Ruby:
include Math
earth_radius_mi = 3959
radians = lambda { |deg| deg * PI / 180 }
coord_radians = lambda { |c| { :lat => radians[c[:lat]], :lng => radians[c[:lng]] } }
# from/to = { :lat => (latitude_in_degrees), :lng => (longitude_in_degrees) }
def haversine_distance(from, to)
from, to = coord_radians[from], coord_radians[to]
cosines_product = cos(to[:lat]) * cos(from[:lat]) * cos(from[:lng] - to[:lng])
sines_product = sin(to[:lat]) * sin(from[:lat])
return earth_radius_mi * acos(cosines_product + sines_product)
end
function getDistanceFromLatLonInKm(lat1,lon1,lat2,lon2,units) {
var R = 6371; // Radius of the earth in km
var dLat = deg2rad(lat2-lat1); // deg2rad below
var dLon = deg2rad(lon2-lon1);
var 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)
;
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c;
var miles = d / 1.609344;
if ( units == 'km' ) {
return d;
} else {
return miles;
}}
Chuck's solution, valid for miles also.
In Mysql use the following function pass the parameters as using POINT(LONG,LAT)
CREATE FUNCTION `distance`(a POINT, b POINT)
RETURNS double
DETERMINISTIC
BEGIN
RETURN
GLength( LineString(( PointFromWKB(a)), (PointFromWKB(b)))) * 100000; -- To Make the distance in meters
END;

Line-Circle Algorithm not quite working as expected

First, see:
https://math.stackexchange.com/questions/105180/positioning-a-widget-involving-intersection-of-line-and-a-circle
I have an algorithm that solves for the height of an object given a circle and an offset.
It sort of works but the height is always off:
Here is the formula:
and here is a sketch of what it is supposed to do:
And here is sample output from the application:
In the formula, offset = 10 and widthRatio is 3. This is why it is (1 / 10) because (3 * 3) + 1 = 10.
The problem, as you can see is the height of the blue rectangle is not correct. I set the bottom left offsets to be the desired offset (in this case 10) so you can see the bottom left corner is correct. The top right corner is wrong because from the top right corner, I should only have to go 10 pixels until I touch the circle.
The code I use to set the size and location is:
void DataWidgetsHandler::resize( int w, int h )
{
int tabSz = getProportions()->getTableSize() * getProportions()->getScale();
int r = tabSz / 2;
agui::Point tabCenter = agui::Point(
w * getProportions()->getTableOffset().getX(),
h * getProportions()->getTableOffset().getY());
float widthRatio = 3.0f;
int offset = 10;
int height = solveHeight(offset,widthRatio,tabCenter.getX(),tabCenter.getY(),r);
int width = height * widthRatio;
int borderMargin = height;
m_frame->setLocation(offset,
h - height - offset);
m_frame->setSize(width,height);
m_borderLayout->setBorderMargins(0,0,borderMargin,borderMargin);
}
I can assert that the table radius and table center location are correct.
This is my implementation of the formula:
int DataWidgetsHandler::solveHeight( int offset, float widthRatio, float h, float k, float r ) const
{
float denom = (widthRatio * widthRatio) + 1.0f;
float rSq = denom * r * r;
float eq = widthRatio * offset - offset - offset + h - (widthRatio * k);
eq *= eq;
return (1.0f / denom) *
((widthRatio * h) + k - offset - (widthRatio * (offset + offset)) - sqrt(rSq - eq) );
}
It uses the quadratic formula to find what the height should be so that the distance between the top right of the rectangle, bottom left, amd top left are = offset.
Is there something wrong with the formula or implementation? The problem is the height is never long enough.
Thanks
Well, here's my solution, which looks to resemble your solveHeight function. There might be some arithmetic errors in the below, but the method is sound.
You can think in terms of matching the coordinates at the point of the circle across
from the rectangle (P).
Let o_x,o_y be the lower left corner offset distances, w and h be the
height of the rectangle, w_r be the width ratio, dx be the desired
distance between the top right hand corner of the rectangle and the
circle (moving horizontally), c_x and c_y the coordinates of the
circle's centre, theta the angle, and r the circle radius.
Labelling it is half the work! Simply write down the coordinates of the point P:
P_x = o_x + w + dx = c_x + r cos(theta)
P_y = o_y + h = c_y + r sin(theta)
and we know w = w_r * h.
To simplify the arithmetic, let's collect some of the constant terms, and let X = o_x + dx - c_x and Y = o_y - c_y. Then we have
X + w_r * h = r cos(theta)
Y + h = r sin(theta)
Squaring and summing gives a quadratic in h:
(w_r^2 + 1) * h^2 + 2 (X*w_r + Y) h + (X^2+Y^2-r^2) == 0
If you compare this with your effective quadratic, then as long as we made different mistakes :-), you might be able to figure out what's going on.
To be explicit: we can solve this using the quadratic formula, setting
a = (w_r^2 + 1)
b = 2 (X*w_r + Y)
c = (X^2+Y^2-r^2)