import numpy as np import math from typing import SupportsFloat, Union from numpy.typing import NDArray _MINUTE = 1 / 60 _SECOND = 1 / 3600 _EARTH_LONG_AXIS = 6378137 # m class LocationCode(): _DELTA_LON = np.array([6, 0.5, 15 * _MINUTE, _MINUTE, 4 * _SECOND, 2 * _SECOND, _SECOND / 4, _SECOND / 32, _SECOND / 256, _SECOND / 2048]) _DELTA_LAT = np.array([4, 0.5, 10 * _MINUTE, _MINUTE, 4 * _SECOND, 2 * _SECOND, _SECOND / 4, _SECOND / 32, _SECOND / 256, _SECOND / 2048]) _POLAR_DELTA_LON = np.array([360, 60, 30, _MINUTE, 4 * _SECOND, 2 * _SECOND, _SECOND / 4, _SECOND / 32, _SECOND / 256, _SECOND / 2048]) _POLAR_DELTA_LAT = np.array([2, 0.5, 15 * _MINUTE, _MINUTE, 4 * _SECOND, 2 * _SECOND, _SECOND / 4, _SECOND / 32, _SECOND / 256, _SECOND / 2048]) _SPLIT_NUMS = np.array([90, 8, 3, 10, 15, 2, 8, 8, 8, 8]) _CODE_LENGTH = [4, 6, 7, 9, 11, 12, 14, 16, 18, 20] _HEIGHT_CODE_BIT_LENGTH = np.array([1, 6, 3, 1, 4, 4, 1, 3, 3, 3, 3], dtype=int) _THETA0 = np.pi / 180 @classmethod def to_code(cls, lon: SupportsFloat, lat: SupportsFloat, alt: SupportsFloat = None, level: int = 10, dimension: int = 2) -> str: """ 根据大地坐标系坐标, 计算二/三维网格码 Args: lon(SupportsFloat): 经度(deg), Range: [-180, 180] lat(SupportsFloat): 纬度(deg), Range: [-90, 90] alt(SupportsFloat): 高程(m), Range: [-6378137, inf], Defaults: None level(int): 网格层级, Range: [1, 10], Defaults: 10 dimension(int): 网格维度, Range: 2 or 3, Defaults: 2 Returns: str: 二/三维网格码 """ if lat < 88: code = cls._coordinate_to_code(lon, lat, level) else: code = cls._polar_to_code(lon, lat, level) if dimension == 3: if alt is None: raise ValueError("Altitude must input when the dimension is 3.") height_code = cls._height_to_code(alt, level) code += height_code elif dimension != 2: raise ValueError("Dimension should be 2 or 3.") return code @classmethod def to_coordinate(cls, code: str, dimension: int = 2) -> NDArray: """ 根据二/三维网格码, 计算大地坐标系坐标 Args: code(str): 二/三维网格码 dimension(int): 网格码维度, Range: 2 or 3, Defaults: 2 Returns NDArray: - NDArray[float, float]: 经度(deg), 纬度(deg) - NDArray[float, float, float]: 经度(deg), 纬度(deg), 高程(m) """ if dimension == 2: if code[1:4] == '000': coordinate = cls._code_to_polar(code) else: coordinate = cls._code_to_coordinate(code) elif dimension == 3: localation_code = code[:-12] height_code = code[-12: ] level = cls._CODE_LENGTH.index(len(localation_code)) + 1 if code[1:4] == '000': lon, lat = cls._code_to_polar(code) else: lon, lat = cls._code_to_coordinate(code) alt = cls._code_to_height(height_code, level) coordinate = np.array([lon, lat, alt]) else: raise ValueError("Dimension should be 2 or 3.") return coordinate @classmethod def _coordinate_to_code(cls, lon: SupportsFloat, lat: SupportsFloat, level: int) -> str: """ 根据经纬度坐标, 计算北斗网格位置码 Args: lon(SupportsFloat): 经度(deg) lat(SupportsFloat): 纬度(deg) level(int): 网格层级 Returns: str: 北斗地理网格码 """ # 输入检测 if level > 11 or level < 1: raise ValueError("Level should be an integer from 1 to 10.") if lon > 180 or lon < -180: raise ValueError("Longitude should belong to [-180, 180].") if lat > 90 or lat < -90: raise ValueError("Latitude should belong to [-90, 90].") # 初始化变量 a = np.zeros(level, dtype=int) # 经向网格码 b = np.zeros(level, dtype=int) # 纬向网格码 lambda_array = np.zeros(level, dtype=float) # 定位角经度 phi_array = np.zeros(level, dtype=float) # 定位角纬度 # 判断半球 hemisphere = 'N' if lat < 0: hemisphere = 'S' lat *= -1 # 计算L1网格 a[0] = int(lon // cls._DELTA_LON[0]) + 30 b[0] = int(lat // cls._DELTA_LAT[0]) lambda_array[0] = - 180 + a[0] * cls._DELTA_LON[0] #L1网格定位角经度 if lon < 0: lambda_array[0] = -lambda_array[0] - cls._DELTA_LON[0] lon *= -1 phi_array[0] = b[0] * cls._DELTA_LAT[0] #L1网格定位角纬度 # 计算L2-L8网格 for i in range(1, level): a[i] = int((lon - lambda_array[i - 1]) // cls._DELTA_LON[i]) b[i] = int((lat - phi_array[i - 1]) // cls._DELTA_LAT[i]) lambda_array[i] = lambda_array[i - 1] + a[i] * cls._DELTA_LON[i] phi_array[i] = phi_array[i - 1] + b[i] * cls._DELTA_LAT[i] # 格式化编码 code = hemisphere for i, (ai, bi) in enumerate(zip(a, b)): if i == 0: code += f"{ai + 1:02d}{chr(65 + bi)}" elif i == 2 or i == 5: code += f"{bi * 2 + ai}" else: code += f"{ai:X}{bi:X}" return code @classmethod #TODO def _polar_to_code(cls, lon: SupportsFloat, lat: SupportsFloat, level: int) -> str: lambda_array = np.zeros(level, dtype=float) # 定位角经度 phi_array = np.zeros(level, dtype=float) # 定位角纬度 a = np.zeros(level, dtype=int) # 经向网格码 b = np.zeros(level, dtype=int) # 纬向网格码 # 判断半球 hemisphere = 'N' if lat < 0: hemisphere = 'S' lat *= -1 # L1网格 code_num_array = np.zeros(level, dtype=int) # L2网格 #TODO if level >= 2: # P0 if lat >= 90 - 30 * _MINUTE: phi_array[1] = 90 - 30 * _MINUTE code_num_array[1] = 0 # P1 elif lat >= 90 - 60 * _MINUTE: phi_array[1] = 90 - 60 * _MINUTE if lon >= 0 and lon < 120: code_num_array[1] = 1 elif lon < 0 and lon > -120: code_num_array[1] = 3 lon *= -1 else: code_num_array[1] = 2 # P2 elif lat >= 90 - 90 * _MINUTE: phi_array[1] = 90 - 90 * _MINUTE if lon >= 0: if lon < 60: code_num_array[1] = 12 lambda_array[1] = 0 elif lon < 120: code_num_array[1] = 13 lambda_array[1] = 60 elif lon < 180: code_num_array[1] = 22 lambda_array[1] = 120 else: lon *= -1 if lon < 60: code_num_array[1] = 32 lambda_array[1] = 0 elif lon < 120: code_num_array[1] = 33 lambda_array[1] = 60 elif lon < 180: code_num_array[1] = 23 lambda_array[1] = 120 # P3 else: phi_array[1] = 90 - 120 * _MINUTE if lon >= 0: if lon < 60: code_num_array[1] = 10 lambda_array[1] = 0 elif lon < 120: code_num_array[1] = 11 lambda_array[1] = 60 elif lon < 180: code_num_array[1] = 20 lambda_array[1] = 120 else: lon *= -1 if lon < 60: code_num_array[1] = 30 lambda_array[1] = 0 elif lon < 120: code_num_array[1] = 31 lambda_array[1] = 60 elif lon < 180: code_num_array[1] = 21 lambda_array[1] = 120 # 计算L3网格 #TODO if level >= 3: if code_num_array[1] == 0: if lat > 90 - 15 * _MINUTE: phi_array[2] = 90 - 15 * _MINUTE else: code_num_array[2] = (lon + 360) % 360 // 120 + 1 phi_array[2] = 90 - 30 * _MINUTE elif code_num_array[1] < 10: if code_num_array[1] == 2: if lat < 90 - 45 * _MINUTE: phi_array[2] = 90 - 45 * _MINUTE code_num_array[2] = 0 else: phi_array[2] = 90 - 60 * _MINUTE code_num_array[2] = 1 else: a[2] = (abs(lon) - lambda_array[1]) // cls._POLAR_DELTA_LON[2] b[2] = (lat - phi_array[1]) // cls._POLAR_DELTA_LAT[2] code_num_array[2] = b[2] * 2 + a[2] code = f"{hemisphere}000{code_num_array[1]:0>2}{code_num_array[2]}" return code.ljust(cls._CODE_LENGTH[level - 1] - len(code), '0') # # 计算L4网格 # if level >= 4: # if code_num_array.sum() == 0 and lat >= 90 - cls._DELTA_LAT[3]: #TODO # a[3] = 0 # b[3] = 14 # else: # delta_lon = 24 # a[3] = (lon + 360) % 360 // 24 # b[3] = (lat - phi_array[2]) // cls._DELTA_LAT[3] # lambda_array[3] = a * 24 # phi_array[3] = phi_array[2] + b * cls._DELTA_LAT[3] # # 计算L5网格 # if level >= 5: # if code_num_array.sum() == 0: # if lat > 90 - cls._DELTA_LAT[4]: # phi_array[4] = 90 - cls._DELTA_LAT[4] # code_num_array[4] = 0 # else: # delta_lon = 24 # a = (lon + 360) % 360 // delta_lon # b = (lat - phi_array[3]) // cls._DELTA_LAT[4] # else: # delta_lon = 24 / 15 # if code_num_array[3] == 7: # a = ((lon + 360) % 360 - 7 * 24) // delta_lon # else: # a = (abs(lon) - lambda_array[3]) // delta_lon # b = (lat - phi_array) // cls._DELTA_LAT[4] @classmethod def _height_to_code(cls, alt: SupportsFloat, level: int) -> str: """ 根据高程, 计算高度码 Args: alt(SupportsFloat): 地理高度(m) level(int): 网格层级 Returns: str: 北斗高度码 """ n = np.int32(1 / cls._DELTA_LAT[level - 1] * math.log((alt + _EARTH_LONG_AXIS) / _EARTH_LONG_AXIS, 1 + cls._THETA0)) shift_length = cls._HEIGHT_CODE_BIT_LENGTH[level + 1:].sum() n = n >> shift_length a = np.zeros(11, dtype=int) # 高度码 for i in range(level + 1): bit_length = cls._HEIGHT_CODE_BIT_LENGTH[level - i] a[level - i] = n & ((1 << bit_length) - 1) n = n >> bit_length code = "" for i, code_num in enumerate(a): if i == 1: code += f"{code_num:02d}" else: code += f"{code_num:X}" return code @classmethod def _code_to_coordinate(cls, code:str) -> NDArray: """ 根据北斗网格位置, 计算经纬度坐标 Args: code(str): 北斗网格位置码码 Returns: NDArray: 经度(deg), 纬度(deg) """ # 输入检测 code_length = len(code) if code_length not in cls._CODE_LENGTH: raise ValueError("Code length error.") level = cls._CODE_LENGTH.index(code_length) + 1 # 网格层级 hemisphere = code[0] # 坐标半球 a = np.zeros(10, dtype=int) # 经向网格码 b = np.zeros(10, dtype=int) # 纬向网格码 # 解码 for i in range(level): if i == 0: a[0] = abs(int(code[1:3]) - 31) b[0] = ord(code[3]) - 65 elif i == 2 or i == 5: a[i] = int(code[cls._CODE_LENGTH[i] - 1]) % 2 b[i] = int(code[cls._CODE_LENGTH[i] - 1]) // 2 else: a[i] = int(code[cls._CODE_LENGTH[i - 1]], 16) b[i] = int(code[cls._CODE_LENGTH[i - 1] + 1], 16) # 计算经纬度 lon = np.dot(a, cls._DELTA_LON) if int(code[1:3]) < 31: lon = -lon + cls._DELTA_LON[0] lat = np.dot(b, cls._DELTA_LAT) if hemisphere == 'S': lat *= -1 # 计算网格中心点 lon += np.sign(lon) * cls._DELTA_LON[level - 1] / 2 lat += np.sign(lat) * cls._DELTA_LAT[level - 1] / 2 return np.array([lon, lat]) @classmethod def _code_to_polar(cls, code:str) -> NDArray: hemisphere = 1 if code[0] == 'N' else -1 if code[4] == '0': if code[5] == '0': if code[6] == '0': lon, lat = 0, 90 else: lat = 90 - 22.5 * _MINUTE lon = int(code[5]) * 120 - 60 lon = lon if lon < 180 else lon - 360 else: lat = 90 + (int(code[6]) * 15 - 60) * _MINUTE lon = int(code[5]) * 120 - 60 lon = lon if lon < 180 else lon - 360 else: code6_num = int(code[5]) code7_lon = int(code[6]) % 2 code7_lat = int(code[6]) // 2 lat = 90 + (code7_lat * 15 - 120 + 7.5) * _MINUTE if code6_num > 1: lat += 30 * _MINUTE code6_num -= 2 if code[4] == '3': lon = -60 * code6_num - code7_lon * 30 - 15 elif code[4] == '1': lon = code6_num * 60 + code7_lon * 30 + 15 else: if code6_num == 0: lon = 120 + code7_lon * 30 + 15 else: lon = -(120 + code7_lon * 30 + 15) return np.array([lon, lat * hemisphere]) @classmethod def _code_to_height(cls, code: str, level: int) -> float: """ 根据高度码, 计算高程 Args: code(str): 北斗高度码 level(int): 网格层级 Returns: float: 高程(m) """ # 输入检测 if level > 11 or level < 1: raise ValueError("Level should be an integer from 1 to 10.") if len(code) != 12: raise ValueError("Code length should be 12.") n = np.int32(0) # 初始高程 shift_length = 32 # 初始化移位位数 # 解码 for i in range(11): if i == 0: code_num = int(code[0]) elif i == 1: code_num = int(code[1: 3]) else: code_num = int(code[i + 1], 16) shift_length -= cls._HEIGHT_CODE_BIT_LENGTH[i] n = n | (code_num << shift_length) # 计算高程 alt = (1 + cls._THETA0) ** (n * cls._DELTA_LAT[level - 1]) * _EARTH_LONG_AXIS - _EARTH_LONG_AXIS return alt