求经纬度xy 到x1 y1 |x2 y2组成的直线的垂直距离

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;
    }