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<Long, double[]> 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位小数
|
}
|
}
|
