| | |
| | | } |
| | | |
| | | public static String distance2(double lon_y0, double lat_x0, double lon, double lat) { |
| | | System.out.println(lon_y0); |
| | | System.out.println(lat_x0); |
| | | System.out.println(lon); |
| | | System.out.println(lat); |
| | | double[] xycs = initiize_ublox_zeropoint(lat_x0, lon_y0, 0.0); |
| | | double x0 = xycs[0]; |
| | | double y0 = xycs[1]; |
| | |
| | | double disd = Math.sqrt(xy[0] * xy[0] + xy[1] * xy[1]) * 100.0; |
| | | return disd + ""; |
| | | } |
| | | public static double[] ubloxraw2xy(double lat, double lon) { |
| | | double[] xy=new double[3]; |
| | | double lond = ublox_dm2d(lon); |
| | | double latd = ublox_dm2d(lat); |
| | | double[] dxy = utm(latd, lond); |
| | | xy[0]= dxy[0]* 1000; |
| | | xy[1]= dxy[1]* 1000; |
| | | xy[2]= dxy[2]; |
| | | return xy; |
| | | |
| | | } |
| | | /**初始化原点位置和方向 |
| | | * 输出原点以米为单位的xy坐标, cos值, sin值*/ |
| | | public static double[] initiize_ublox_zeropoint(double lat, double lon, double th){ |
| | |
| | | } |
| | | |
| | | /**输入ublox的度分经纬度,输出xy,单位米*/ |
| | | public static double[] ubloxraw2xy(double lat, double lon) { |
| | | double[] xy=new double[3]; |
| | | double lond = ublox_dm2d(lon); |
| | | double latd = ublox_dm2d(lat); |
| | | double[] dxy = utm(latd, lond); |
| | | xy[0]= dxy[0]* 1000; |
| | | xy[1]= dxy[1]* 1000; |
| | | xy[2]= dxy[2]; |
| | | return xy; |
| | | |
| | | } |
| | | public static double ublox_dm2d(double ddmm_dot_m) { |
| | | double d0=Math.floor(ddmm_dot_m/100);//取整 |
| | | double d=d0+(ddmm_dot_m-d0*100)/60; |