最近连个点连线的垂直距离(三点连线的垂直距离)

   ***************************************************************************
    计算是否在计划线路行驶偏航，偏航条件最近连个点连线的垂直距离(三点连线的垂直距离)

   ***************************************************************************

注释：使用该算法留下评论

    private static final double EARTH_RADIUS = 6371000; // 地球半径（单位：米）

    /**
     * 计算两个点之间的距离
     * @param point1
     * @param point2
     * @return
     */
    public static double calculateDistance(LatLngDTO point1, LatLngDTO point2) {
        double lat1 = Math.toRadians(point1.getLatitude());
        double lon1 = Math.toRadians(point1.getLongitude());
        double lat2 = Math.toRadians(point2.getLatitude());
        double lon2 = Math.toRadians(point2.getLongitude());

        double dlat = lat2 - lat1;
        double dlon = lon2 - lon1;

        double a = Math.sin(dlat / 2) * Math.sin(dlat / 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.sin(dlon / 2) * Math.sin(dlon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));

        return EARTH_RADIUS * c;
    }

    /**
     * 找到最近的两个点
     * @param point
     * @param points
     * @return
     */
    public static LatLngDTO[] findNearestPoints(LatLngDTO point, List<LatLngDTO> points) {
        PriorityQueue<LatLngDTO> pq = new PriorityQueue<>((p1, p2) -> {
            double d1 = calculateDistance(point, p1);
            double d2 = calculateDistance(point, p2);
            return Double.compare(d1, d2);
        });

        pq.addAll(points);

        LatLngDTO[] nearestPoints = new LatLngDTO[2];
        nearestPoints[0] = pq.poll();
        nearestPoints[1] = pq.poll();

        return nearestPoints;
    }


    /**
     * 计算点到两个点连线的最短垂直距离
     * @param point
     * @param point1
     * @param point2
     * @return
     */
    public static double shortestDistanceToLine(LatLngDTO point, LatLngDTO point1, LatLngDTO point2) {
        // 点到两点连线的垂直距离可以通过点到两点连线的垂直平分线的距离来计算
        double distance1 = calculateDistance(point, point1);
        double distance2 = calculateDistance(point, point1);

        // 如果两点相同，那么垂直距离就是点到那个点的距离
        if (point1.getLatitude() == point2.getLatitude() && point1.getLongitude() == point2.getLongitude()) {
            return distance1;
        }

        // 如果两点不同，垂直距离是点到两点连线的距离
        double baseDistance = calculateDistance(point1, point2);
        return Math.sqrt(Math.pow(distance1, 2) + Math.pow(distance2, 2)) / baseDistance * baseDistance;
    }


    public static void main(String[] args) {

        // 示例数据
        LatLngDTO point = new LatLngDTO(31.59343487, 124.01874686); // 给定点
        List<LatLngDTO> points = new ArrayList<>();
        points.add(new LatLngDTO(28.96208288, 123.10455866));
        points.add(new LatLngDTO(31.57343487, 124.01174686));
        points.add(new LatLngDTO(33.10630063, 124.81813649));
        points.add(new LatLngDTO(33.61144584, 125.17093191));
        // 找到最近的两个点
        LatLngDTO[] nearestPoints = findNearestPoints(point, points);
        System.out.println("Nearest Points: " + nearestPoints[0] + " and " + nearestPoints[1]);
        // 计算两点距离的公式 单位海里，1海里=1852
        // 计算点到直线的距离
        double distanceToLine = shortestDistanceToLine(point, nearestPoints[0], nearestPoints[1]);

        System.out.println("Distance to line: " + distanceToLine/1852 + "NM");

    }