根据两点传的不同参数计算距离算法

根据两点间经纬度坐标（double值），计算两点间距离，单位为米

	/**
	 * 地球半径
	 */
	private static final double EARTH_RADIUS = 6378137;
	/**
	 * 弧度
	 */
	private static final double RAD = Math.PI / 180.0;

	/**
	 * 根据两点间经纬度坐标（double值），计算两点间距离，单位为米 *
	 */
	public static Double getDistance(Double lng1, Double lat1, Double lng2, Double lat2) {
		double radLat1 = lat1 * RAD;
		double radLat2 = lat2 * RAD;
		double a = radLat1 - radLat2;
		Double b = (lng1 - lng2) * RAD;
		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 = Math.round(s * 10000) / 10000.0;
		return s;
	}

其他方法

/**
 * @author LiuTao
 * @description:地球点算法
 * @date : 2020/10/19 11:54
 */
public class MapCalculationLogLatDistance {
    /**
     * 地球赤道半径(km)
     */
    /**
     * 地球赤道半径(米)
     */
    public final static double EARTH_RADIUS = 6378137;
    /**
     * 地球每度的弧长(米)
     */
    public final static double EARTH_ARC = 111199;

    /**
     * 转化为弧度(rad)
     */
    public static double rad(double d) {
        return d * Math.PI / 180.0;
    }

    /**
     * 求两经纬度距离
     *
     * @param lon1 第一点的经度
     * @param lat1 第一点的纬度
     * @param lon2 第二点的经度
     * @param lat2 第二点的纬度
     * @return 两点距离，单位km
     */
    public static double getDistanceOne(double lon1, double lat1, double lon2,
                                        double lat2) {
        double r1 = rad(lat1);
        double r2 = rad(lon1);
        double a = rad(lat2);
        double b = rad(lon2);
        double s = Math.acos(Math.cos(r1) * Math.cos(a) * Math.cos(r2 - b)
                + Math.sin(r1) * Math.sin(a))
                * EARTH_RADIUS*0.001;
        return s;
    }

    /**
     * 求两经纬度距离(google maps源码中)
     *
     * @param lon1 第一点的经度
     * @param lat1 第一点的纬度
     * @param lon2 第二点的经度
     * @param lat2 第二点的纬度
     * @return 两点距离，单位km
     */
    public static double getDistanceTwo(double lon1, double lat1, double lon2, double lat2) {
        double radLat1 = rad(lat1);
        double radLat2 = rad(lat2);
        double a = radLat1 - radLat2;
        double b = rad(lon1) - rad(lon2);
        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*0.001;
        return s;
    }

    /**
     * 求两经纬度距离(google maps源码中)
     *
     * @param lon1 第一点的经度
     * @param lat1 第一点的纬度
     * @param lon2 第二点的经度
     * @param lat2 第二点的纬度
     * @return 两点距离，单位m
     */
    public static double getDistanceTwoMi(double lon1, double lat1, double lon2,
                                          double lat2) {
        double radLat1 = rad(lat1);
        double radLat2 = rad(lat2);
        double a = radLat1 - radLat2;
        double b = rad(lon1) - rad(lon2);
        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;
        return s;
    }

    /**
     * 求两经纬度距离
     *
     * @param lon1 第一点的经度
     * @param lat1 第一点的纬度
     * @param lon2 第二点的经度
     * @param lat2 第二点的纬度
     * @return 两点距离，单位km
     */
    public static double getDistanceThree(double lon1, double lat1,
                                          double lon2, double lat2) {
        double radLat1 = rad(lat1);
        double radLat2 = rad(lat2);
        double radLon1 = rad(lon1);
        double radLon2 = rad(lon2);
        if (radLat1 < 0) {
            radLat1 = Math.PI / 2 + Math.abs(radLat1);// south
        }
        if (radLat1 > 0) {
            radLat1 = Math.PI / 2 - Math.abs(radLat1);// north
        }
        if (radLon1 < 0) {
            radLon1 = Math.PI * 2 - Math.abs(radLon1);// west
        }
        if (radLat2 < 0) {
            radLat2 = Math.PI / 2 + Math.abs(radLat2);// south
        }
        if (radLat2 > 0) {
            radLat2 = Math.PI / 2 - Math.abs(radLat2);// north
        }
        if (radLon2 < 0) {
            radLon2 = Math.PI * 2 - Math.abs(radLon2);// west
        }
        double x1 = Math.cos(radLon1) * Math.sin(radLat1);
        double y1 = Math.sin(radLon1) * Math.sin(radLat1);
        double z1 = Math.cos(radLat1);

        double x2 = Math.cos(radLon2) * Math.sin(radLat2);
        double y2 = Math.sin(radLon2) * Math.sin(radLat2);
        double z2 = Math.cos(radLat2);

        double d = Math.pow((x1 - x2), 2) + Math.pow((y1 - y2), 2)
                + Math.pow((z1 - z2), 2);
        // // 余弦定理求夹角
        // double theta = Math.acos((2 - d) / 2);

        d = Math.pow(EARTH_RADIUS, 2) * d;
        // //余弦定理求夹角
        double theta = Math.acos((2 * Math.pow(EARTH_RADIUS, 2) - d)
                / (2 * Math.pow(EARTH_RADIUS, 2)));

        double dist = theta * EARTH_RADIUS;
        return dist;
    }

    /**
     * 求两经纬度方向角
     *
     * @param lon1 第一点的经度
     * @param lat1 第一点的纬度
     * @param lon2 第二点的经度
     * @param lat2 第二点的纬度
     * @return 方位角，角度（单位：°）
     */
    public static double getAzimuth(double lon1, double lat1, double lon2,
                                    double lat2) {
        lat1 = rad(lat1);
        lat2 = rad(lat2);
        lon1 = rad(lon1);
        lon2 = rad(lon2);
        double azimuth = Math.sin(lat1) * Math.sin(lat2) + Math.cos(lat1)
                * Math.cos(lat2) * Math.cos(lon2 - lon1);
        azimuth = Math.sqrt(1 - azimuth * azimuth);
        azimuth = Math.cos(lat2) * Math.sin(lon2 - lon1) / azimuth;
        azimuth = Math.asin(azimuth) * 180 / Math.PI;
        if (Double.isNaN(azimuth)) {
            if (lon1 < lon2) {
                azimuth = 90.0;
            } else {
                azimuth = 270.0;
            }
        }
        return azimuth;
    }

    /**
     * 已知一点经纬度，方位角，距离，求另一点经纬度。
     *
     * @param latLng
     * @param distance 米
     * @param bearing
     * @return
     */
    public static LatLng getBearingLatLng(LatLng latLng, double distance, double bearing) {
        double R = 6371.393 * 1000;

        double δ = distance / R;
        double θ = toRadius(bearing);

        double φ1 = toRadius(latLng.latitude);
        double λ1 = toRadius(latLng.longitude);

        double sinφ2 = Math.sin(φ1) * Math.cos(δ) + Math.cos(φ1) * Math.sin(δ) * Math.cos(θ);
        double φ2 = Math.asin(sinφ2);

        double y = Math.sin(θ) * Math.sin(δ) * Math.cos(φ1);
        double x = Math.cos(δ) - Math.sin(φ1) * sinφ2;
        double λ2 = λ1 + Math.atan2(y, x);

        double lat = toDegree(φ2);
        double lng = toDegree(λ2);

        return new LatLng(lat, lng);
    }
    /**
     * 已知两点 和到点1距离的另一点
     * <p>
     * 外扩
     *
     * @param Lng1
     * @param Lat1
     * @param Lng2
     * @param Lat2
     * @param distance
     * @return
     */
    public static LatLng getDistanceExpansionPoint(double Lng1, double Lat1, double Lng2, double Lat2, double distance) {
        double azimuth = getAzimuthAngle(Lat1, Lng1, Lat2, Lng2);
        LogUtil.e("我的位置", "----角度-----" + azimuth);

        LatLng gpslatLng = MapCalculationLogLatDistance.convertDistanceToLogLat(Lng2, Lat2, distance, azimuth);
//        LatLng gpslatLng = MapCalculationLogLatDistance.getBearingLatLng(new LatLng(Lat2, Lng2), distance, azimuth);
        return gpslatLng;
    }
    /**
     * 已知一点经纬度A，和与另一点B的距离和方位角，求B的经纬度
     *
     * @param lng1     A的经度
     * @param lat1     A的纬度
     * @param distance AB距离（单位：米）
     * @param azimuth  AB方位角
     * @return B的经纬度
     */
    public static LatLng convertDistanceToLogLat(double lng1, double lat1, double distance, double azimuth) {
        azimuth = rad(azimuth);
        // 将距离转换成经度的计算公式
        double lon = lng1 + (distance * Math.sin(azimuth)) / (EARTH_ARC * Math.cos(rad(lat1)));
        // 将距离转换成纬度的计算公式
        double lat = lat1 + (distance * Math.cos(azimuth)) / EARTH_ARC;
        return new LatLng(lat, lon);
    }

    /**
     * 已知两点 和到点1距离的另一点
     *
     * @param droneLng       gps坐标
     * @param droneLat       gps坐标
     * @param targetLng      gps坐标
     * @param targetLat      gps坐标
     * @param percentagedata 两点距离的百分比
     * @return latlng gps坐标
     */
    public static LatLng getPercentageDividePoint(double droneLng, double droneLat, double targetLng, double targetLat, double percentagedata) {
        double azimuth = MapCalculationLogLatDistance.getAzimuth(targetLng, targetLat, droneLng, droneLat);
        double azimuth2 = getAzimuth2(targetLat, targetLng, droneLat, droneLng);
        LogUtil.e("我的位置", "----角度-----" + azimuth);
        LogUtil.e("我的位置", "----角度-----" + azimuth2);
        //两点距离
        double distance = MapCalculationLogLatDistance.getDistanceTwo(droneLng, droneLat, targetLng, targetLat);
        double parcentDistance = distance * percentagedata;
        if (parcentDistance >= 2) {
            parcentDistance = 1;
        }
        //0.5m  0.0005km
        //计算起始点 GPS坐标
        LatLng gpslatLng = MapCalculationLogLatDistance.convertDistanceToLogLat(targetLng, targetLat, parcentDistance, azimuth2);
        return gpslatLng;
    }

    /**
     * 我的点
     *
     * @param lngA 纬度1
     * @param latA 经度1
     *
     *             <p>
     *             <p>
     *             对着的 目标点/兴趣点
     * @param latB 纬度2
     * @param lngB 经度2
     *             <p>
     *             逆时针
     * @return
     */
    public static double getAzimuth2(double latA, double lngA, double latB, double lngB) {
        double angle = computeAzimuth(new LatLng(latA, lngA), new LatLng(latB, lngB));
        if (Double.isNaN(angle)) {
            return 0;
        } else {
            return angle;
        }
    }

    private static double computeAzimuth(LatLng la1, LatLng la2) {
        double lat1 = la1.latitude, lon1 = la1.longitude, lat2 = la2.latitude,
                lon2 = la2.longitude;
        double result = 0.0;

        int ilat1 = (int) (0.50 + lat1 * 360000.0);
        int ilat2 = (int) (0.50 + lat2 * 360000.0);
        int ilon1 = (int) (0.50 + lon1 * 360000.0);
        int ilon2 = (int) (0.50 + lon2 * 360000.0);

        lat1 = Math.toRadians(lat1);
        lon1 = Math.toRadians(lon1);
        lat2 = Math.toRadians(lat2);
        lon2 = Math.toRadians(lon2);

        if ((ilat1 == ilat2) && (ilon1 == ilon2)) {
            return result;
        } else if (ilon1 == ilon2) {
            if (ilat1 > ilat2) {
                result = 180.0;
            }
        } else {
            double c = Math
                    .acos(Math.sin(lat2) * Math.sin(lat1) + Math.cos(lat2)
                            * Math.cos(lat1) * Math.cos((lon2 - lon1)));
            double a = Math.asin(Math.cos(lat2) * Math.sin((lon2 - lon1))
                    / Math.sin(c));
            result = Math.toDegrees(a);
            if ((ilat2 > ilat1) && (ilon2 > ilon1)) {
            } else if ((ilat2 < ilat1) && (ilon2 < ilon1)) {
                result = 180.0 - result;
            } else if ((ilat2 < ilat1) && (ilon2 > ilon1)) {
                result = 180.0 - result;
            } else if ((ilat2 > ilat1) && (ilon2 < ilon1)) {
                result += 360.0;
            }
        }
        return result;
    }

    /**
     * 得到方向角
     * 我的点
     *
     * @param latA 纬度1
     * @param lngA 经度1
     *
     *             <p>
     *             <p>
     *             对着的 方位角
     * @param latB 纬度2
     * @param lngB 经度2
     *             <p>
     *             逆时针
     * @return
     */
    public static double getAzimuthAngle(double latA, double lngA, double latB, double lngB) {
        double angle = computeAzimuth(new LatLng(latA, lngA), new LatLng(latB, lngB));
        if (Double.isNaN(angle)) {
            return 0;
        } else {
            return angle;
        }
    }

    public static double getTriangleAngle(double x1, double y1, double x2, double y2, double x3, double y3) {

        double c = Math.abs(Math.sqrt(Math.pow((x1 - x2), 2) + Math.pow((y1 - y2), 2)));

        double a = Math.abs(Math.sqrt(Math.pow((x3 - x2), 2) + Math.pow((y3 - y2), 2)));

        double b = Math.abs(Math.sqrt(Math.pow((x1 - x3), 2) + Math.pow((y1 - y3), 2)));

        double cosb = (Math.pow(a, 2) + Math.pow(c, 2) - Math.pow(b, 2)) / (2 * a * c);

        return Math.toDegrees(Math.acos(cosb));

    }
    /**
     * 求两个经纬度的中点
     *
     * @param l1
     * @param l2
     * @return
     */
    public static LatLng getMiddleLatLng(LatLng l1, LatLng l2) {
        return new LatLng((l1.latitude + l2.latitude) / 2, (l1.longitude + l2.longitude) / 2);
    }

    public static void main(String[] args) {
        double lon1 = 121.469156;
        double lat1 = 31.232307;

        double lon2 = 121.469120;
        double lat2 = 31.233205;

        double distance = getDistanceTwo(lon1, lat1, lon2, lat2);
        double azimuth = getAzimuth(lon1, lat1, lon2, lat2);
        System.out.println("经纬度为(" + lon1 + "," + lat1 + ")的点与经纬度为(" + lon2
                + "," + lat2 + ")相距：" + distance + "千米," + "方位角：" + azimuth
                + "°");

        System.out.println("距经纬度为(" + lon1 + "," + lat1 + ")的点" + distance
                + "千米,方位角为" + azimuth + "°的另一点经纬度为("
                + convertDistanceToLogLat(lon1, lat1, distance, azimuth) + ")");
    }


    private static double toRadius(double value) {
        return value * Math.PI / 180;
    }

    private static double toDegree(double value) {
        return value * 180 / Math.PI;
    }

    /**
     * 判断是否有交点
     * 最快算法+跨立实验
     *
     * @param aa
     * @param bb
     * @param cc
     * @param dd
     * @return
     */
    public static boolean intersect(LatLng aa, LatLng bb, LatLng cc, LatLng dd) {
        if (Math.max(aa.longitude, bb.longitude) <= Math.min(cc.longitude, dd.longitude)) {
            return false;
        }
        if (Math.max(aa.latitude, bb.latitude) <= Math.min(cc.latitude, dd.latitude)) {
            return false;
        }
        if (Math.max(cc.longitude, dd.longitude) <= Math.min(aa.longitude, bb.longitude)) {
            return false;
        }
        if (Math.max(cc.latitude, dd.latitude) <= Math.min(aa.latitude, bb.latitude)) {
            return false;
        }
        if (mult(cc, bb, aa) * mult(bb, dd, aa) <= 0) {
            return false;
        }
        if (mult(aa, dd, cc) * mult(dd, bb, cc) <= 0) {
            return false;
        }

        return true;
    }

    /**
     * 跨立实验
     *
     * @param a
     * @param b
     * @param c
     * @return
     */
    private static double mult(LatLng a, LatLng b, LatLng c) {
        return (b.longitude - a.longitude) * (c.latitude - a.latitude) - (b.latitude - a.latitude) * (c.longitude - a.longitude);
    }

    /**
     * 计算直线的交点
     */
    public static double[] intersection(double p0x, double p0y, double p1x, double p1y, double p2x, double p2y, double p3x, double p3y) {
        double x = p0x;
        double y = p0y;
        double t = ((p0x - p2x) * (p3y - p2y) - (p0y - p2y) * (p3x - p2x))
                / ((p0x - p1x) * (p3y - p2y) - (p0y - p1y) * (p3x - p2x));
        x += (p1x - p0x) * t;
        y += (p1y - p0y) * t;
        if (!Double.isNaN(x) || !Double.isNaN(y)) {
            return new double[]{x, y};
        } else {
            return null;
        }
    }

    /**
     * 计算线段的交点（不包括端点）
     *
     * @param p0_x
     * @param p0_y
     * @param p1_x
     * @param p1_y
     * @param p2_x
     * @param p2_y
     * @param p3_x
     * @param p3_y
     * @return
     */
    public static double[] getLineIntersection(double p0_x, double p0_y, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y) {
        double s02_x, s02_y, s10_x, s10_y, s32_x, s32_y, s_numer, t_numer, denom, t;
        s10_x = p1_x - p0_x;
        s10_y = p1_y - p0_y;
        s32_x = p3_x - p2_x;
        s32_y = p3_y - p2_y;

        denom = s10_x * s32_y - s32_x * s10_y;
        //平行或共线
        if (denom == 0) {
            return null; // Collinear
        }
        boolean denomPositive = denom > 0;
        s02_x = p0_x - p2_x;
        s02_y = p0_y - p2_y;
        s_numer = s10_x * s02_y - s10_y * s02_x;
        //参数是大于等于0且小于等于1的，分子分母必须同号且分子小于等于分母
        if ((s_numer < 0) == denomPositive) {
            return null; // No collision
        }

        t_numer = s32_x * s02_y - s32_y * s02_x;
        if ((t_numer < 0) == denomPositive) {
            return null; // No collision
        }
        if (Math.abs(s_numer) > Math.abs(denom) || Math.abs(t_numer) > Math.abs(denom)) {
            return null; // No collision
        }
        // Collision detected
        t = t_numer / denom;
        double ix = p0_x + (t * s10_x);
        double iy = p0_y + (t * s10_y);
        if (!Double.isNaN(ix) || !Double.isNaN(iy)) {
            return new double[]{ix, iy};
        } else {
            return null;
        }
    }

}