462 lines
16 KiB
Python
462 lines
16 KiB
Python
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
|