package com.pojo.common.core.utils;
import java.math.BigDecimal;
import java.time.LocalDateTime;
public class CaculateDistanceUtil {
private final static double EARTH_RADIUS = 6378.137;
private final static double a = 6378137.0D;
private final static double f = 0.003352810681;
private final static double b = 6356752.314141519D;
private final static double e2 = 2 * f - f * f;
private static double rad(double d) {
return d * Math.PI / 180.0;
}
/**
* 根据两点间经纬度坐标（double值），计算两点间距离，单位为米
*/
public static double getDistance(double lat1, double lng1, double lat2, double lng2) {
double radLat1 = rad(lat1);
double radLat2 = rad(lat2);
double a = radLat1 - radLat2;
double b = rad(lng1) - rad(lng2);
double s = 2 * Math.asin(Math.sqrt(
Math.pow(Math.sin(a / 2), 2) + Math.cos(radLat1) * Math.cos(radLat2) * Math.pow(Math.sin(b / 2), 2)));
s = s * EARTH_RADIUS;
s = (s * 10000) / 10;
return s;
}
public static double getDistance(BigDecimal lat1, BigDecimal lng1, BigDecimal lat2, BigDecimal lng2) {
return getDistance(lat1.doubleValue(), lng1.doubleValue(), lat2.doubleValue(), lng2.doubleValue());
}
public static double[] WGS84ToECEF(double longitude, double latitude, double h) {
latitude = latitude / 180 * Math.PI; // converting to radians
longitude = longitude / 180 * Math.PI; // converting to radians
double chi = Math.sqrt(1 - e2 * Math.pow(Math.sin(latitude), 2));
double X = (a / chi + h) * Math.cos(latitude) * Math.cos(longitude);
double Y = (a / chi + h) * Math.cos(latitude) * Math.sin(longitude);
double Z = (a * (1 - e2) / chi + h) * Math.sin(latitude);
return new double[]{X, Y, Z};
}
public static double[] ECEFToENU(double Xr, double Yr, double Zr, double X, double Y, double Z) {
double phiP = Math.atan2(Zr, Math.sqrt(Xr * Xr + Yr * Yr));
double lambda = Math.atan2(Yr, Xr);
double e = -Math.sin(lambda) * (X - Xr) + Math.cos(lambda) * (Y - Yr);
double n = -Math.sin(phiP) * Math.cos(lambda) * (X - Xr) - Math.sin(phiP) * Math.sin(lambda) * (Y - Yr) + Math.cos(phiP) * (Z - Zr);
double u = Math.cos(phiP) * Math.cos(lambda) * (X - Xr) + Math.cos(phiP) * Math.sin(lambda) * (Y - Yr) + Math.sin(phiP) * (Z - Zr);
return new double[]{e, n, u};
}
public static boolean checkDistance(double e1, double n1, double e2, double n2) {
if (Math.abs(e2 - e1) < 1 || Math.abs(n2 - n1) < 1) {
return true;
} else {
double distance = Math.abs(e1 - n1 * (e2 - e1) / (n2 - n1)) / Math.sqrt(1 + Math.pow((e2 - e1) / (n2 - n1), 2));
if (distance < 100) {
return true;
} else {
return false;
}
}
}
public static double verticalDistance(double e1, double n1, double e2, double n2) {
if (Math.abs(e2 - e1) < 1 || Math.abs(n2 - n1) < 1) {
//表示点在直线上
return 0;
} else {
double distance = Math.abs(e1 - n1 * (e2 - e1) / (n2 - n1)) / Math.sqrt(1 + Math.pow((e2 - e1) / (n2 - n1), 2));
return distance;
}
}
/**
* 求 给定经纬度 （lng，lat） 到 （lng1，lat1| lng2,lat2组成的直线的垂直距离）
*
* @param lat1
* @param lng1
* @param lat2
* @param lng2
* @param lat
* @param lng
* @return
*/
public static double verticalDistance(BigDecimal lat1, BigDecimal lng1, BigDecimal lat2, BigDecimal lng2, BigDecimal lat, BigDecimal lng) {
double lng1d = lng1.doubleValue();
double lat1d = lat1.doubleValue();
double alt1d = 0.0;
double lng2d = lng2.doubleValue();
double lat2d = lat2.doubleValue();
double alt2d = 0.0;
double lngd = lng.doubleValue();
double latd = lat.doubleValue();
double altd = 0.0;
// Convert WGS84 to ECEF
double[] XYZ1 = WGS84ToECEF(lng1d, lat1d, alt1d);
double[] XYZ2 = WGS84ToECEF(lng2d, lat2d, alt2d);
double[] XYZ = WGS84ToECEF(lngd, latd, altd);
// Convert ECEF to ENU
double[] enu1 = ECEFToENU(XYZ1[0], XYZ1[1], XYZ1[2], XYZ[0], XYZ[1], XYZ[2]);
double[] enu2 = ECEFToENU(XYZ2[0], XYZ2[1], XYZ2[2], XYZ[0], XYZ[1], XYZ[2]);
// Check distance
double result = verticalDistance(enu1[0], enu1[1], enu2[0], enu2[1]);
return result;
}
public static void main(String[] args) {
// Test data
//
// double Long1 = 116.502846000000000;
// double Lat1 = 31.860445000000000;
// double Alt1 = 0.0;
// double Long2 =116.582846000000000;
// double Lat2 =31.860445000000000;
// double Alt2 = 0;
// double Long = 116.522846000000000;
// double Lat = 31.860445000000000;
// double Alt = 0;
//
//
//
// // Convert WGS84 to ECEF
// double[] XYZ1 = WGS84ToECEF(Long1, Lat1, Alt1);
// double[] XYZ2 = WGS84ToECEF(Long2, Lat2, Alt2);
// double[] XYZ = WGS84ToECEF(Long, Lat, Alt);
//
// // Convert ECEF to ENU
// double[] enu1 = ECEFToENU(XYZ1[0], XYZ1[1], XYZ1[2], XYZ[0], XYZ[1], XYZ[2]);
// double[] enu2 = ECEFToENU(XYZ2[0], XYZ2[1], XYZ2[2], XYZ[0], XYZ[1], XYZ[2]);
//
// // Check distance
// double result = verticalDistance(enu1[0], enu1[1], enu2[0], enu2[1]);
// System.out.println(result);
BigDecimal x1 = new BigDecimal(31.860445000000000);
BigDecimal x2 = new BigDecimal(31.860445000000000);
BigDecimal x = new BigDecimal(31.860445000000000);
BigDecimal y1 = new BigDecimal(116.502846000000000);
BigDecimal y2 = new BigDecimal(116.582846000000000);
BigDecimal y = new BigDecimal(116.582846000000000);
//求经纬度xy 到x1 y1 |x2 y2组成的直线的垂直距离
double result = verticalDistance(x1, y1, x2, y2, x, y);
System.out.println(result);
// System.out.println(StringUtils.station("K275+650"));
// System.out.println(LocalDateUtil.localDateTimeToString(LocalDateTime.now(),LocalDateUtil.DATE_PATTERN_yyyyMMdd));
// System.out.println(LocalDateUtil.getMonthEndTime());
// System.out.println(LocalDateUtil.getWeekStartTime());
// System.out.println(LocalDateUtil.getWeekEndTime());
// System.out.println(LocalDateUtil.getTodayStart());
// System.out.println(LocalDateUtil.getTodayEnd());
// System.out.println(getDistance(31.817077442754957,117.16533299864935,31.816731016717863,117.16559952340532));
}
}
/**
* 勾股定理 求斜边长度
*
* @param a 直角边长度
* @param b 直角边长度
* @return
*/
public static double hypotenuse(double a, double b) {
double root = Math.sqrt(a * a + b * b);
return root;
}
/**
* 勾股定理 求直角边长度
*
* @param ht 斜边长度
* @param b 直角边长度
* @return
*/
public static double angledEdge(double ht, double b) {
double root = Math.sqrt(ht * ht - b * b);
return root;
}