Java计算WGS84坐标系两个点之间的距离，距离可能会较大，
算法 球形模型1直接采用球形的模型。
@Deprecated
public static double distance(double lat1, double lat2, double lon1,
double lon2, double el1, double el2) {
final int R = 6371; // Radius of the earth
double latDistance = Math.toRadians(lat2 - lat1);
double lonDistance = Math.toRadians(lon2 - lon1);
double a = Math.sin(latDistance / 2) * Math.sin(latDistance / 2)
+ Math.cos(Math.toRadians(lat1)) * Math.cos(Math.toRadians(lat2))
* Math.sin(lonDistance / 2) * Math.sin(lonDistance / 2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
double distance = R * c * 1000; // convert to meters
double height = el1 - el2;
distance = Math.pow(distance, 2) + Math.pow(height, 2);
return Math.sqrt(distance);
}
球形模型2
public static double calculateDistance(Point2D.Double fromPoint, Point2D.Double toPoint) {
return distanceWithLljk(fromPoint.getX(), fromPoint.getY(), toPoint.getX(), toPoint.getY()) / 1000.0;
}
private static double distanceWithLljk(double fromLon, double fromLat, double toLon, double toLat) {
//地球半径以及塔台和无人机的经纬高赋值
double r0 = 6371393.0;
double t1 = fromLon;
double t2 = fromLat;
double t3 = 0;
double a1 = toLon;
double a2 = toLat;
double a3 = 0;
//角度转弧度以及距地心距离计算
t1 = t1 / 180.0 * Math.PI;
t2 = t2 / 180.0 * Math.PI;
t3 = t3 + r0;
a1 = a1 / 180.0 * Math.PI;
a2 = a2 / 180.0 * Math.PI;
a3 = a3 + r0;
//地球坐标系
double t11 = t3 * Math.cos(t2) * Math.cos(t1);
double t12 = t3 * Math.cos(t2) * Math.sin(t1);
double t13 = t3 * Math.sin(t2);
double a11 = a3 * Math.cos(a2) * Math.cos(a1);
double a12 = a3 * Math.cos(a2) * Math.sin(a1);
double a13 = a3 * Math.sin(a2);
double x1 = a11 - t11;
double x2 = a12 - t12;
double x3 = a13 - t13;
double r1 = (-Math.sin(t1)) * x1 + Math.cos(t1) * x2;
double r2 = (-Math.sin(t2)) * Math.cos(t1) * x1 + (-Math.sin(t2) * Math.sin(t1)) * x2 + Math.cos(t2) * x3;
double r3 = Math.cos(t2) * Math.cos(t1) * x1 + Math.cos(t2) * Math.sin(t1) * x2 + Math.sin(t2) * x3;
return Math.sqrt(r1 * r1 + r2 * r2 + r3 * r3);
}
坐标变换
@Deprecated
public static double calculateDistanceWithUTM(Point2D.Double fromPoint, Point2D.Double toPoint) {
double[] s = SpatialUtils.wgs84ToUTM(fromPoint.getX(), fromPoint.getY());
double[] t = SpatialUtils.wgs84ToUTM(toPoint.getX(), toPoint.getY());
return Math.sqrt(Math.pow((s[0] - t[0]), 2) + Math.pow((s[1] - t[1]), 2)) / 1000.0;
}
public static double[] wgs84ToUTM(double lon, double lat) {
double[] xy = new double[2];
double radPerDegree = MPI / 180;
int zone = (int) (lon / 6) + 31;
double a = 6378137;
double finv = 298.257223563;
double f = 1 / finv;
double eDouble = 1 - (1 - f) * (1 - f);
double koDouble = 0.9996;
double eoDouble = 500000;
double noDouble = 0;
double lcm;
double lam;
double latAfter;
lcm = (zone * 6 - 183) * radPerDegree;
lam = lon * radPerDegree - lcm;
latAfter = lat * radPerDegree;
double rnDouble;
double h2;
double t;
double n;
rnDouble = a / pow(1 - eDouble * sin(latAfter) * sin(latAfter), 0.5);
h2 = eDouble * cos(latAfter) * cos(latAfter) / (1 - eDouble);
t = tan(latAfter);
n = f / (2 - f);
double a0;
double a2;
double a4;
double a6;
double a8;
double resultS;
a0 = 1 + n * n / 4 + pow(n, 4) / 64;
a2 = 3.0 / 2 * (n - pow(n, 3) / 8);
a4 = 15.0 / 16 * (n * n - pow(n, 4) / 4);
a6 = 35.0 / 48 * pow(n, 3);
a8 = 315.0 / 512 * pow(n, 4);
resultS = a / (1 + n) * (a0 * latAfter - a2 * sin(2 * latAfter) + a4 * sin(4 * latAfter) - a6 * sin(6 * latAfter) + a8 * sin(8 * latAfter));
double e1;
double e2;
double e3;
double e4;
double resultE;
e1 = lam * cos(latAfter);
e2 = pow(lam, 3) * pow(cos(latAfter), 3) / 6 * (1 - t * t + h2);
e3 = pow(lam, 5) * pow(cos(latAfter), 5) / 120 * (5 - 18 * t * t + pow(t, 4) + 14 * h2 - 58 * t * t * h2 + 13 * h2 * h2 + 4 * pow(h2, 3) - 64 * t * t * h2 * h2 - 24 * t * t * pow(h2, 3));
e4 = pow(lam, 7) * pow(cos(latAfter), 7) / 5040 * (61 - 479 * t * t + 179 * pow(t, 4) - pow(t, 6));
resultE = eoDouble + koDouble * rnDouble * (e1 + e2 + e3 + e4);
double n1;
double n2;
double n3;
double n4;
double n5;
double nResult;
n1 = resultS / rnDouble;
n2 = lam * lam / 2 * sin(latAfter) * cos(latAfter);
n3 = pow(lam, 4) / 24 * sin(latAfter) * pow(cos(latAfter), 3) * (5 - t * t + 9 * h2 + 4 * h2 * h2);
n4 = pow(lam, 6) / 720 * sin(latAfter) * pow(cos(latAfter), 5) * (61 - 58 * t * t + pow(t, 4) + 270 * h2 - 330 * t * t * h2 + 445 * h2 * h2 + 324 * pow(h2, 3) - 680 * t * t * h2 * h2 + 88 * pow(h2, 4) - 600 * t * t * pow(h2, 3) - 192 * t * t * pow(h2, 4));
n5 = pow(lam, 8) / 40320 * sin(latAfter) * pow(cos(latAfter), 7) * (1385 - 311 * t * t + 543 * pow(t, 4) - pow(t, 6));
nResult = noDouble + koDouble * rnDouble * (n1 + n2 + n3 + n4 + n5);
xy[0] = resultE;
xy[1] = nResult;
return xy;
}
org.gavaghan.geodesy库
https://github.com/mgavaghan/geodesy，这个库也有一个js版本所以前端也可以用。
@Deprecated
public static double calculateWithSdk(Point2D.Double fromPoint, Point2D.Double toPoint) {
GlobalCoordinates from = new GlobalCoordinates(fromPoint.getX(), fromPoint.getY());
GlobalCoordinates to = new GlobalCoordinates(toPoint.getX(), toPoint.getY());
return new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, from, to).getEllipsoidalDistance() / 1000.0;
}
public GeodeticCurve calculateGeodeticCurve(Ellipsoid ellipsoid, GlobalCoordinates start, GlobalCoordinates end) {
double a = ellipsoid.getSemiMajorAxis();
double b = ellipsoid.getSemiMinorAxis();
double f = ellipsoid.getFlattening();
double phi1 = Angle.toRadians(start.getLatitude());
double lambda1 = Angle.toRadians(start.getLongitude());
double phi2 = Angle.toRadians(end.getLatitude());
double lambda2 = Angle.toRadians(end.getLongitude());
double a2 = a * a;
double b2 = b * b;
double a2b2b2 = (a2 - b2) / b2;
double omega = lambda2 - lambda1;
double tanphi1 = Math.tan(phi1);
double tanU1 = (1.0D - f) * tanphi1;
double U1 = Math.atan(tanU1);
double sinU1 = Math.sin(U1);
double cosU1 = Math.cos(U1);
double tanphi2 = Math.tan(phi2);
double tanU2 = (1.0D - f) * tanphi2;
double U2 = Math.atan(tanU2);
double sinU2 = Math.sin(U2);
double cosU2 = Math.cos(U2);
double sinU1sinU2 = sinU1 * sinU2;
double cosU1sinU2 = cosU1 * sinU2;
double sinU1cosU2 = sinU1 * cosU2;
double cosU1cosU2 = cosU1 * cosU2;
double lambda = omega;
double A = 0.0D;
double B = 0.0D;
double sigma = 0.0D;
double deltasigma = 0.0D;
boolean converged = false;
for(int i = 0; i < 20; ++i) {
double lambda0 = lambda;
double sinlambda = Math.sin(lambda);
double coslambda = Math.cos(lambda);
double sin2sigma = cosU2 * sinlambda * cosU2 * sinlambda + (cosU1sinU2 - sinU1cosU2 * coslambda) * (cosU1sinU2 - sinU1cosU2 * coslambda);
double sinsigma = Math.sqrt(sin2sigma);
double cossigma = sinU1sinU2 + cosU1cosU2 * coslambda;
sigma = Math.atan2(sinsigma, cossigma);
double sinalpha = sin2sigma == 0.0D ? 0.0D : cosU1cosU2 * sinlambda / sinsigma;
double alpha = Math.asin(sinalpha);
double cosalpha = Math.cos(alpha);
double cos2alpha = cosalpha * cosalpha;
double cos2sigmam = cos2alpha == 0.0D ? 0.0D : cossigma - 2.0D * sinU1sinU2 / cos2alpha;
double u2 = cos2alpha * a2b2b2;
double cos2sigmam2 = cos2sigmam * cos2sigmam;
A = 1.0D + u2 / 16384.0D * (4096.0D + u2 * (-768.0D + u2 * (320.0D - 175.0D * u2)));
B = u2 / 1024.0D * (256.0D + u2 * (-128.0D + u2 * (74.0D - 47.0D * u2)));
deltasigma = B * sinsigma * (cos2sigmam + B / 4.0D * (cossigma * (-1.0D + 2.0D * cos2sigmam2) - B / 6.0D * cos2sigmam * (-3.0D + 4.0D * sin2sigma) * (-3.0D + 4.0D * cos2sigmam2)));
double C = f / 16.0D * cos2alpha * (4.0D + f * (4.0D - 3.0D * cos2alpha));
lambda = omega + (1.0D - C) * f * sinalpha * (sigma + C * sinsigma * (cos2sigmam + C * cossigma * (-1.0D + 2.0D * cos2sigmam2)));
double change = Math.abs((lambda - lambda0) / lambda);
if (i > 1 && change < 1.0E-13D) {
converged = true;
break;
}
}
double s = b * A * (sigma - deltasigma);
double alpha1;
double alpha2;
if (!converged) {
if (phi1 > phi2) {
alpha1 = 180.0D;
alpha2 = 0.0D;
} else if (phi1 < phi2) {
alpha1 = 0.0D;
alpha2 = 180.0D;
} else {
alpha1 = 0.0D / 0.0;
alpha2 = 0.0D / 0.0;
}
} else {
double radians = Math.atan2(cosU2 * Math.sin(lambda), cosU1sinU2 - sinU1cosU2 * Math.cos(lambda));
if (radians < 0.0D) {
radians += 6.283185307179586D;
}
alpha1 = Angle.toDegrees(radians);
radians = Math.atan2(cosU1 * Math.sin(lambda), -sinU1cosU2 + cosU1sinU2 * Math.cos(lambda)) + 3.141592653589793D;
if (radians < 0.0D) {
radians += 6.283185307179586D;
}
alpha2 = Angle.toDegrees(radians);
}
if (alpha1 >= 360.0D) {
alpha1 -= 360.0D;
}
if (alpha2 >= 360.0D) {
alpha2 -= 360.0D;
}
return new GeodeticCurve(s, alpha1, alpha2);
}
单元测试
package com.zw.se2.platform.utils.spatial;
import org.junit.Test;
import java.awt.geom.Point2D;
import static org.junit.Assert.*;
public class SpatialUtilsTest {
@Test
public void calculateDistance() {
Point2D.Double from=new Point2D.Double(5.998,20.0);
Point2D.Double to=new Point2D.Double(6.002,19.99999);
double v = SpatialUtils.calculateDistance(from, to);
assertEquals(0.417,v,0.01);
}
@Test
public void calculateDistanceWithSame() {
Point2D.Double from=new Point2D.Double(5.998,20.0);
double v = SpatialUtils.calculateDistance(from, from);
assertEquals(v,0,0.0001);
}
@Test
public void calculateDistanceWithLocal() {
Point2D.Double from=new Point2D.Double(117,39);
Point2D.Double to=new Point2D.Double(118,39);
double v = SpatialUtils.calculateDistance(from, to);
assertEquals(86.36,v,1);
}
@Test
public void calculateDistanceWithTdt() {
Point2D.Double from=new Point2D.Double(112.67,38.76);
Point2D.Double to=new Point2D.Double(115.37,37.22);
double v = SpatialUtils.calculateDistance(from, to);;
assertEquals(292.3,v,0.1);
}
}
结论
采用SDK的方式最为准确，与Cesium计算结果高度一致，球形模型2的次之，在跨带后UTM输出结果没法使用，所以建议使用geodesy这个库来实现。
