package publicsWay; // 包名：公共路径相关类 import dell_map.Dell_Map; // 导入地图数据类 import targets.Csxy; // 导入坐标系统类 import java.text.DecimalFormat; // 格式化数字工具 import java.util.Locale; // 本地化工具 import java.util.Map; // 映射接口 import java.util.concurrent.ConcurrentHashMap; // 线程安全哈希映射 public class CoordinateTranslator { // 坐标转换工具类 static final double a = 6378.137; // WGS84椭球体长半轴(公里) static final double e = 0.0818192; // 第一偏心率 static final double k0 = 0.9996; // UTM比例因子 static final double e2 = e * e; // 偏心率的平方 static final double e4 = e2 * e2; // 偏心率的四次方 static final double e6 = e4 * e2; // 偏心率的六次方 static final double E0 = 500; // UTM东向偏移值(公里) static final double N0 = 0; // UTM北向偏移值(公里) static DecimalFormat df = new DecimalFormat("0.0000000"); // 7位小数格式化器 // UTM坐标缓存<经纬度组合键, [东坐标, 北坐标, 带号]> private static final Map UTM_CACHE = new ConcurrentHashMap<>(); // 最近计算结果的缓存 private static double lastLat = Double.NaN; // 上次计算的纬度 private static double lastLon = Double.NaN; // 上次计算的经度 private static int[] lastResult = null; // 上次转换结果 // 计算UTM坐标（纬度,经度 -> [东坐标, 北坐标, 带号]） public static double[] utm(double lat, double lon) { double[] dxy = new double[3]; // 返回数组[东坐标,北坐标,带号] double zonenum1 = lon / 6; // 计算基础带号 zonenum1 = (zonenum1 > 0) ? Math.floor(zonenum1) : Math.ceil(zonenum1); // 取整处理 double Zonenum = zonenum1 + 31; // 实际带号（+31偏移） double lambda0 = ((Zonenum - 1) * 6 - 180 + 3) * Math.PI / 180; // 中央经线(弧度) double phi = lat * Math.PI / 180; // 纬度转弧度 double lambda = lon * Math.PI / 180; // 经度转弧度 double v = 1 / Math.sqrt((1 - e2 * Math.pow(Math.sin(phi), 2))); // 曲率半径 double A = (lambda - lambda0) * Math.cos(phi); // 经差投影 double A2 = A * A; // A的平方 double A3 = A2 * A; // A的立方 double A4 = A3 * A; // A的四次方 double A5 = A4 * A; // A的五次方 double A6 = A5 * A; // A的六次方 double T = Math.tan(phi) * Math.tan(phi); // tan²φ double T2 = T * T; // tan⁴φ double C = e2 * Math.cos(phi) * Math.cos(phi) / (1 - e2); // 辅助系数 double C2 = C * C; // C的平方 double s = (1 - e2/4 - 3*e4/64 - 5*e6/256) * phi - // 子午线弧长 (3*e2/8 + 3*e4/32 + 45*e6/1024) * Math.sin(2*phi) + (15*e4/256 + 45*e6/1024) * Math.sin(4* phi) - 35*e6/3072 * Math.sin(6*phi); dxy[0] = E0 + k0 * a * v * (A + (1 - T + C) * A3/6 + (5 - 18*T + T2) * A5/120); // 东坐标计算 dxy[1] = N0 + k0 * a * (s + v * Math.tan(phi) * (A2/2 + (5 - T + 9*C + 4*C2) * A4/24 + (61 - 58*T + T2) * A6/720)); // 北坐标计算 dxy[2] = Zonenum; // 带号 return dxy; // 返回UTM坐标数组 } // 度分格式转十进制度 (ddmm.mmmm -> dd.dddddd) public static double ublox_dm2d(double ddmm_dot_m) { double d0 = Math.floor(ddmm_dot_m / 100); // 提取度数 return d0 + (ddmm_dot_m - d0 * 100) / 60; // 转换分值为度 } // 度分格式转字符串 (ddmm.mmmm -> "dd.dddddd") public static String ublox_dm2dstr(double ddmm_dot_m) { double d0 = Math.floor(ddmm_dot_m / 100); // 提取度数 double d = d0 + (ddmm_dot_m - d0 * 100) / 60; // 转换分值为度 return df.format(d); // 格式化为字符串 } // 初始化原点坐标系 [原点X, 原点Y, cosθ, sinθ] public static double[] initiize_ublox_zeropoint(double lat, double lon, double th) { double[] xycs = new double[4]; // 返回数组[原点X,原点Y,cosθ,sinθ] double[] utm = ubloxraw2xy(lat, lon); // 获取UTM坐标 xycs[0] = utm[0]; // 原点X(米) xycs[1] = utm[1]; // 原点Y(米) double th1 = th * Math.PI / 180; // 角度转弧度 xycs[2] = Math.cos(th1); // 旋转矩阵cos分量 xycs[3] = Math.sin(th1); // 旋转矩阵sin分量 return xycs; // 返回坐标系参数 } // 通过两点初始化坐标系（简易版）[原点X,原点Y,cosθ,sinθ,角度,带号] public static double[] initiize_ublox_AB( double lat0, // 原点纬度 double lon0, // 原点经度 double latB, // B点纬度 double lonB, // B点经度 double xB, // B点局部坐标X double yB) { // B点局部坐标Y double[] xycstf = new double[6]; // 返回数组[原点X,原点Y,cosθ,sinθ,角度,带号] double[] a = ubloxraw2xy(lat0, lon0); // 原点UTM坐标 double[] b = ubloxraw2xy(latB, lonB); // B点UTM坐标 double[] ab = {b[0] - a[0], b[1] - a[1]}; // UTM坐标差向量 double the = Math.atan2(ab[0], ab[1]); // UTM向量角度 double thp = Math.atan2(yB, xB); // 局部坐标向量角度 double th = the - thp; // 旋转角差 xycstf[0] = a[0]; // 原点X(米) xycstf[1] = a[1]; // 原点Y(米) xycstf[2] = Math.cos(th); // 旋转矩阵cos分量 xycstf[3] = Math.sin(th); // 旋转矩阵sin分量 xycstf[4] = th * 180 / Math.PI; // 旋转角度(度) xycstf[5] = a[2]; // UTM带号 return xycstf; // 返回坐标系参数 } // 通过两点初始化坐标系（完整版）[原点X,原点Y,cosθ,sinθ,角度,带号] /** * 通过两个已知点计算坐标转换参数 * [原点X(m), 原点Y(m), cosθ, sinθ, 旋转角度(°), UTM带号] * * @param latA 点A纬度 (度分格式) * @param lonA 点A经度 (度分格式) * @param latB 点B纬度 (度分格式) * @param lonB 点B经度 (度分格式) * @param localXA 点A局部X坐标 (cm) * @param localYA 点A局部Y坐标 (cm) * @param localXB 点B局部X坐标 (cm) * @param localYB 点B局部Y坐标 (cm) * @return 转换参数数组 [x0, y0, cosθ, sinθ, θ_deg, zone] */ public static double[] computeTransformParamsWithTwoPoints( double latA, double lonA, double latB, double lonB, double localXA, double localYA, double localXB, double localYB) { // 1. 单位转换 (cm → m) double xA = localXA / 100; double yA = localYA / 100; double xB = localXB / 100; double yB = localYB / 100; // 2. 获取UTM坐标 double[] utmA = ubloxraw2xy(latA, lonA); double[] utmB = ubloxraw2xy(latB, lonB); // 3. 计算UTM向量差 double dxUTM = utmB[0] - utmA[0]; double dyUTM = utmB[1] - utmA[1]; // 4. 计算局部坐标向量差 double dxLocal = xB - xA; double dyLocal = yB - yA; // 5. 计算旋转角度差 double thetaUTM = Math.atan2(dxUTM, dyUTM); // UTM向量角度 double thetaLocal = Math.atan2(dyLocal, dxLocal); // 局部向量角度 double theta = thetaUTM - thetaLocal; // 旋转角差 // 6. 计算旋转矩阵分量 double cosTheta = Math.cos(theta); double sinTheta = Math.sin(theta); // 7. 计算实际原点坐标 double rotatedXA = cosTheta * xA - sinTheta * yA; double rotatedYA = sinTheta * xA + cosTheta * yA; double originX = utmA[0] - rotatedXA; double originY = utmA[1] - rotatedYA; return new double[] { originX, // 实际原点X (m) originY, // 实际原点Y (m) cosTheta, // 旋转矩阵cos分量 sinTheta, // 旋转矩阵sin分量 theta * 180 / Math.PI, // 旋转角度(°) utmA[2] // UTM带号 }; } // 原始格式转UTM坐标（带缓存）[东坐标(米), 北坐标(米), 带号] public static double[] ubloxraw2xy(double lat, double lon) { // 生成缓存键（1e7精度约1.1cm） long key = ((long)(lat * 1e7) << 32) | ((long)(lon * 1e7)) & 0xFFFFFFFFL; return UTM_CACHE.computeIfAbsent(key, k -> { // 缓存查询/计算 double lond = ublox_dm2d(lon); // 度分转度 double latd = ublox_dm2d(lat); // 度分转度 double[] dxy = utm(latd, lond); // 计算UTM坐标 return new double[] { // 返回结果(米) dxy[0] * 1000, // 公里转米 dxy[1] * 1000, // 公里转米 dxy[2] // 带号不变 }; }); } // GPS转局部坐标 [局部Y(cm), 局部X(cm), 高度, 楼层] public static int[] run_gps2xy(double lat, double lon) { // 检查缓存是否可用 if (Double.compare(lat, lastLat) == 0 && Double.compare(lon, lastLon) == 0 && lastResult != null) { return lastResult; // 直接返回缓存结果 } // 1. 计算当前点UTM坐标 double[] currentUtm = ubloxraw2xy(lat, lon); double currentX = currentUtm[0]; // 当前点东坐标(米) double currentY = currentUtm[1]; // 当前点北坐标(米) // 2. 查找最近基站 double minDistanceSq = Double.MAX_VALUE; // 最小距离平方 Csxy nearestCsxy = null; // 最近基站对象 for (Csxy cs : Dell_Map.csxyvc) { // 遍历基站集合 double dx = currentX - cs.getOriginUtmX(); // X方向差 double dy = currentY - cs.getOriginUtmY(); // Y方向差 double distanceSq = dx * dx + dy * dy; // 距离平方 if (distanceSq < minDistanceSq) { // 找到更近基站 minDistanceSq = distanceSq; nearestCsxy = cs; // 更新最近基站 } } if (nearestCsxy == null) return null; // 无基站时返回空 // 3. 坐标转换 double[] xycs = nearestCsxy.getXycs(); // 获取基站坐标系参数 double x0 = xycs[0]; // 基站原点X double y0 = xycs[1]; // 基站原点Y double c = xycs[2]; // 旋转cos double s = xycs[3]; // 旋转sin double x = currentX - x0; // 平移X double y = currentY - y0; // 平移Y // 4. 旋转变换 double rotatedX = c * x - s * y; // 新坐标系X double rotatedY = s * x + c * y; // 新坐标系Y // 5. 更新缓存 lastLat = lat; // 缓存纬度 lastLon = lon; // 缓存经度 lastResult = new int[] { // 转换结果(cm) (int)(rotatedY * 100), // Y坐标转厘米 (int)(rotatedX * 100), // X坐标转厘米 150, // 固定高度值 nearestCsxy.getFloorInt() // 楼层信息 }; return lastResult; // 返回转换结果 } // 字符串转双精度（保留1位小数） public static double stringToDouble(String a) { double b = Double.valueOf(a); // 字符串转double DecimalFormat df = new DecimalFormat("#.0"); // 1位小数格式化 return Double.valueOf(df.format(b)); // 格式化后返回 } // GPS转局部坐标（双点标定版）[局部Y(cm), 局部X(cm)] public static int[] gps_xy( String[] A, // A点参数[经,纬,X,Y] String[] B, // B点参数[经,纬,X,Y] String jd1, // 当前经度 String wd1) { // 当前纬度 double lon_a1 = Double.parseDouble(A[0]); // A点经度 double lat_a1 = Double.parseDouble(A[1]); // A点纬度 double xa1 = 0; // A点局部X默认值 double ya1 = 0; // A点局部Y默认值 int size2 = A.length; // A点参数数量 if (size2 == 4) { // 如果有局部坐标 xa1 = Double.parseDouble(A[2]); // A点局部X(cm) ya1 = Double.parseDouble(A[3]); // A点局部Y(cm) } double lon_b1 = Double.parseDouble(B[0]); // B点经度 double lat_b1 = Double.parseDouble(B[1]); // B点纬度 double xb1 = Double.parseDouble(B[2]); // B点局部X(cm) double yb1 = Double.parseDouble(B[3]); // B点局部Y(cm) double lat = Double.parseDouble(wd1); // 当前纬度 double lon = Double.parseDouble(jd1); // 当前经度 int[] realxy = new int[2]; // 返回结果[Y,X](cm) double[] xycs; // 坐标系参数 if (size2 == 4) { // 完整标定模式 xycs = computeTransformParamsWithTwoPoints(lat_a1, lon_a1, lat_b1, lon_b1, xa1, ya1, xb1, yb1); } else { // 简易标定模式 xycs = initiize_ublox_AB(lat_a1, lon_a1, lat_b1, lon_b1, xb1, yb1); } double x0 = xycs[0]; // 原点X(米) double y0 = xycs[1]; // 原点Y(米) double c = xycs[2]; // 旋转cos double s = xycs[3]; // 旋转sin double x = ubloxraw2xy(lat, lon)[0] - x0; // 平移X(米) double y = ubloxraw2xy(lat, lon)[1] - y0; // 平移Y(米) double rotatedX = c * x - s * y; // 旋转后X(米) double rotatedY = s * x + c * y; // 旋转后Y(米) realxy[0] = (int)(rotatedX * 100); //X转厘米 realxy[1] = (int)(rotatedY * 100); //Y转厘米 return realxy; // 返回结果 } // 计算两点距离(cm)（字符串参数） public static int distance(String jd0, String wd0, String jd1, String wd1) { double lat_x0 = Double.parseDouble(wd0); // 点1纬度 double lon_y0 = Double.parseDouble(jd0); // 点1经度 double lat = Double.parseDouble(wd1); // 点2纬度 double lon = Double.parseDouble(jd1); // 点2经度 double[] xycs = initiize_ublox_zeropoint(lat_x0, lon_y0, 0); // 初始化原点坐标系 double x0 = xycs[0]; // 原点X(米) double y0 = xycs[1]; // 原点Y(米) double c = xycs[2]; // 旋转cos double s = xycs[3]; // 旋转sin double x = ubloxraw2xy(lat, lon)[0] - x0; // 平移X(米) double y = ubloxraw2xy(lat, lon)[1] - y0; // 平移Y(米) double rotatedX = c * x - s * y; // 旋转后X(米) double rotatedY = s * x + c * y; // 旋转后Y(米) double disd = Math.sqrt(rotatedX * rotatedX + rotatedY * rotatedY) * 100; // 计算距离(cm) return (int)disd; // 返回整型距离 } // 计算两点距离(cm)（双精度参数） public static int distance2(double lon_y0, double lat_x0, double lon, double lat) { double[] xycs = initiize_ublox_zeropoint(lat_x0, lon_y0, 0); // 初始化原点坐标系 double x0 = xycs[0]; // 原点X(米) double y0 = xycs[1]; // 原点Y(米) double c = xycs[2]; // 旋转cos double s = xycs[3]; // 旋转sin double x = ubloxraw2xy(lat, lon)[0] - x0; // 平移X(米) double y = ubloxraw2xy(lat, lon)[1] - y0; // 平移Y(米) double rotatedX = c * x - s * y; // 旋转后X(米) double rotatedY = s * x + c * y; // 旋转后Y(米) double disd = Math.sqrt(rotatedX * rotatedX + rotatedY * rotatedY) * 100; // 计算距离(cm) return (int)disd; // 返回整型距离 } // GPS转局部坐标（原点标定版）[局部Y(cm), 局部X(cm)] public static int[] run_gps2xyurt(double lat0, double lon0, double lat, double lon) { double[] xycs = initiize_ublox_zeropoint(lat0, lon0, 0); // 初始化原点坐标系 double x0 = xycs[0]; // 原点X(米) double y0 = xycs[1]; // 原点Y(米) double c = xycs[2]; // 旋转cos double s = xycs[3]; // 旋转sin double x = ubloxraw2xy(lat, lon)[0] - x0; // 平移X(米) double y = ubloxraw2xy(lat, lon)[1] - y0; // 平移Y(米) double rotatedX = c * x - s * y; // 旋转后X(米) double rotatedY = s * x + c * y; // 旋转后Y(米) return new int[] { // 返回结果(cm) (int)(rotatedY * 100), // Y转厘米 (int)(rotatedX * 100) // X转厘米 }; } // 计算字符串格式坐标的距离 public static double disab(String jwd) { String[] bb = jwd.split(";"); // 分割字符串 double jd0 = Double.parseDouble(bb[1]); // 点1经度 double wd0 = Double.parseDouble(bb[0]); // 点1纬度 double jd1 = Double.parseDouble(bb[3]); // 点2经度 double wd1 = Double.parseDouble(bb[2]); // 点2纬度 int[] a = run_gps2xyurt(wd0, jd0, wd1, jd1); // 坐标转换 double disa = a[0] * a[0] + a[1] * a[1]; // 距离平方 return Math.sqrt(disa); // 返回实际距离(cm) } // 度分秒转十进制度 public static String todufen(String Du, String Fen, String Miao) { Float strDu = Float.valueOf(Du); // 度 Float strFen = Float.valueOf(Fen) / 60; // 分转度 Float strMiao = Float.valueOf(Miao) / 3600; // 秒转度 Float dufenmiao = strDu + strFen + strMiao; // 总和 return String.format(Locale.US, "%.6f", dufenmiao); // 格式化为6位小数 } }