DCPA和TCPA的Java 实现方法

public static double getCpaTime(Map map) {

        System.out.println("1111");
        double v0 = Double.parseDouble(String.valueOf(map.get("sog")));//A船航速
        double v1 = Double.parseDouble(String.valueOf(map.get("msog")));//B船航速
        double vr = 0;//初始化相对速度
        double c0 = Double.parseDouble(String.valueOf(map.get("cog")));//A船航向
        double c1 = Double.parseDouble(String.valueOf(map.get("mcog")));//B船航向
        double lon = Double.parseDouble(String.valueOf(map.get("lon")));//A船经度
        double lat = Double.parseDouble(String.valueOf(map.get("lat")));//A船纬度
        double mlon = Double.parseDouble(String.valueOf(map.get("mlon")));//B船经度
        double mlat = Double.parseDouble(String.valueOf(map.get("mlat")));//B船纬度

        //两船夹角计算
        double B =  getAngle3(lon,lat,mlon,mlat);
        System.out.println("夹角:"+B);

        //相对方位角
        if(B >= 180 ){
            B = B - c0;
        }else{
            B = B + c0;
        }
        System.out.println("方位角:"+B);
        // 两船距离计算
        double D = GetDistance(lon,lat,mlon,mlat);
        System.out.println("两船距离计算:"+D);
        // 相对速度计算
        double  m = c0 - c1;
        System.out.println("m:"+m);

        vr = Math.pow(v0,2) + Math.pow(v1,2) - (2*v0*v1*Math.cos(m*Math.PI/180));
        System.out.println(vr);
        vr = Math.sqrt(vr);
        System.out.println("相对速度：+"+vr);
        double cr = 0;//初始化相对航向
        double d;
        double q = Math.acos((Math.pow(vr,2) + Math.pow(v0,2) - Math.pow(v1,2))/(2*vr*v0));

        if(m>=0){
            cr = c0 + q;
        }else{
            cr = c0 - q;
        }
        System.out.println("cr:"+cr);
        // 相对舷角计算 b
        double qr = B - cr ;
        System.out.println("qr:"+qr);
        // DCPA 和TCPA 的计算
//        double tcpa = D * Math.cos(qr*Math.PI/180)/vr;
//        System.out.println("预计碰撞时间:"+tcpa);

        double dcpa = D * Math.sin(qr*Math.PI/180);
        System.out.println("预计碰撞距离:"+dcpa);

        double time1 = dcpa/v0;
        System.out.println("A船到达碰撞点的时间："+time1);
        double time2 = dcpa/v1;
        System.out.println("B船到达碰撞点的时间："+time2);
        double time = time1 - time2;
        if (time == 0){
            System.out.println("两船到达碰撞点的时间："+time2);
            return time2;
        }else if(Math.abs(time)< 0.1) {
            if (time1 > time2){

                System.out.println("两船到达碰撞点的时间："+time2);
                return time2;
            }
            else {
                System.out.println("两船到达碰撞点的时间："+time1);
                return time1;
            }

        }else {
            System.out.println("两船不会相撞");
            return 9999;
        }

    }



    private  static double getAngle3(double startX, double startY, double endX, double endY)
    {
        double mathPI = 3.1415926535897931;
        double tmpValue = 0;
        double latStart = startY * Math.PI / 180;
        double lngStart = startX * Math.PI / 180;
        double latEnd = endY * Math.PI / 180;
        double lngEnd = endX * Math.PI / 180;
        if (startX == endX || startY == endY)
        {
            if (startX == endX)
            {
                /// 经度相同
                if (endY >= startY)
                {
                    return 0;
                }
                else
                {
                    return 180;
                }
            }
            else
            {
                /// 纬度相同
                if (endX >= startX)
                {
                    return 90;
                }
                else
                {
                    return 270;
                }
            }
        }

        tmpValue = Math.sin(latStart) * Math.sin(latEnd) + Math.cos(latStart) * Math.cos(latEnd) * Math.cos(lngEnd - lngStart);
        tmpValue = Math.sqrt(1 - tmpValue * tmpValue);
        tmpValue = Math.cos(latEnd) * Math.sin(lngEnd - lngStart) / tmpValue;
        double resultAngle = Math.abs(Math.asin(tmpValue) * 180 / mathPI);

        if (endX > startX)
        {
            if (endY >= startY)
            {
                /// 第一象限
                return resultAngle;
            }
            else
            {
                /// 第二象限
                return 180 - resultAngle;
            }
        }
        else
        {
            /// 第四象限
            if (endY >= startY)
            {
                return 360 - resultAngle;
            }
            else
            {
                /// 第三象限
                return 180 + resultAngle;
            }
        }
    }

    private static final  double EARTH_RADIUS = 6378137;//赤道半径
    private static double rad(double d){
        return d * Math.PI / 180.0;
    }
    public static double GetDistance(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;//单位米
    }

仅供参考