mirror of
https://gitlab.com/oeffi/public-transport-enabler.git
synced 2025-07-19 00:39:58 +00:00
116 lines
4.6 KiB
Java
116 lines
4.6 KiB
Java
/*
|
|
* Copyright the original author or authors.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
package de.schildbach.pte;
|
|
|
|
import de.schildbach.pte.dto.Point;
|
|
|
|
/**
|
|
* @author Andreas Schildbach
|
|
*/
|
|
public final class LocationUtils {
|
|
public static float computeDistance(final Point p1, final Point p2) {
|
|
return computeDistance(p1.getLatAsDouble(), p1.getLonAsDouble(), p2.getLatAsDouble(), p2.getLonAsDouble());
|
|
}
|
|
|
|
/**
|
|
* @param lat1
|
|
* latitude of origin point in decimal degrees
|
|
* @param lon1
|
|
* longitude of origin point in decimal degrees
|
|
* @param lat2
|
|
* latitude of destination point in decimal degrees
|
|
* @param lon2
|
|
* longitude of destination point in decimal degrees
|
|
*
|
|
* @return distance in meters
|
|
*/
|
|
public static float computeDistance(double lat1, double lon1, double lat2, double lon2) {
|
|
// Based on http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
|
|
// using the "Inverse Formula" (section 4)
|
|
|
|
final int MAXITERS = 20;
|
|
// Convert lat/long to radians
|
|
lat1 *= Math.PI / 180.0;
|
|
lat2 *= Math.PI / 180.0;
|
|
lon1 *= Math.PI / 180.0;
|
|
lon2 *= Math.PI / 180.0;
|
|
|
|
final double a = 6378137.0; // WGS84 major axis
|
|
final double b = 6356752.3142; // WGS84 semi-major axis
|
|
final double f = (a - b) / a;
|
|
final double aSqMinusBSqOverBSq = (a * a - b * b) / (b * b);
|
|
|
|
final double L = lon2 - lon1;
|
|
double A = 0.0;
|
|
final double U1 = Math.atan((1.0 - f) * Math.tan(lat1));
|
|
final double U2 = Math.atan((1.0 - f) * Math.tan(lat2));
|
|
|
|
final double cosU1 = Math.cos(U1);
|
|
final double cosU2 = Math.cos(U2);
|
|
final double sinU1 = Math.sin(U1);
|
|
final double sinU2 = Math.sin(U2);
|
|
final double cosU1cosU2 = cosU1 * cosU2;
|
|
final double sinU1sinU2 = sinU1 * sinU2;
|
|
|
|
double sigma = 0.0;
|
|
double deltaSigma = 0.0;
|
|
double cosSqAlpha = 0.0;
|
|
double cos2SM = 0.0;
|
|
double cosSigma = 0.0;
|
|
double sinSigma = 0.0;
|
|
double cosLambda = 0.0;
|
|
double sinLambda = 0.0;
|
|
|
|
double lambda = L; // initial guess
|
|
for (int iter = 0; iter < MAXITERS; iter++) {
|
|
final double lambdaOrig = lambda;
|
|
cosLambda = Math.cos(lambda);
|
|
sinLambda = Math.sin(lambda);
|
|
final double t1 = cosU2 * sinLambda;
|
|
final double t2 = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda;
|
|
final double sinSqSigma = t1 * t1 + t2 * t2; // (14)
|
|
sinSigma = Math.sqrt(sinSqSigma);
|
|
cosSigma = sinU1sinU2 + cosU1cosU2 * cosLambda; // (15)
|
|
sigma = Math.atan2(sinSigma, cosSigma); // (16)
|
|
final double sinAlpha = (sinSigma == 0) ? 0.0 : cosU1cosU2 * sinLambda / sinSigma; // (17)
|
|
cosSqAlpha = 1.0 - sinAlpha * sinAlpha;
|
|
cos2SM = (cosSqAlpha == 0) ? 0.0 : cosSigma - 2.0 * sinU1sinU2 / cosSqAlpha; // (18)
|
|
|
|
final double uSquared = cosSqAlpha * aSqMinusBSqOverBSq; // defn
|
|
A = 1 + (uSquared / 16384.0) * // (3)
|
|
(4096.0 + uSquared * (-768 + uSquared * (320.0 - 175.0 * uSquared)));
|
|
final double B = (uSquared / 1024.0) * // (4)
|
|
(256.0 + uSquared * (-128.0 + uSquared * (74.0 - 47.0 * uSquared)));
|
|
final double C = (f / 16.0) * cosSqAlpha * (4.0 + f * (4.0 - 3.0 * cosSqAlpha)); // (10)
|
|
final double cos2SMSq = cos2SM * cos2SM;
|
|
deltaSigma = B * sinSigma * // (6)
|
|
(cos2SM + (B / 4.0) * (cosSigma * (-1.0 + 2.0 * cos2SMSq)
|
|
- (B / 6.0) * cos2SM * (-3.0 + 4.0 * sinSigma * sinSigma) * (-3.0 + 4.0 * cos2SMSq)));
|
|
|
|
lambda = L + (1.0 - C) * f * sinAlpha
|
|
* (sigma + C * sinSigma * (cos2SM + C * cosSigma * (-1.0 + 2.0 * cos2SM * cos2SM))); // (11)
|
|
|
|
final double delta = (lambda - lambdaOrig) / lambda;
|
|
|
|
if (Math.abs(delta) < 1.0e-12)
|
|
break;
|
|
}
|
|
|
|
return (float) (b * A * (sigma - deltaSigma));
|
|
}
|
|
}
|