mirror of
https://github.com/herizon1054/openpilot.git
synced 2026-06-08 09:54:13 +08:00
115 lines
4.1 KiB
Python
115 lines
4.1 KiB
Python
"""
|
|
Copyright (c) 2026, Rick Lan
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
of this software and associated documentation files (the "Software"), to deal
|
|
in the Software without restriction, including without limitation the rights
|
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
|
for non-commercial purposes only, subject to the following conditions:
|
|
|
|
- The above copyright notice and this permission notice shall be included in
|
|
all copies or substantial portions of the Software.
|
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
|
generate revenue) is prohibited without explicit written permission from
|
|
the copyright holder.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
|
|
WGS-84 <-> GCJ-02 coordinate transformations.
|
|
|
|
GCJ-02 (Chinese: 火星坐标系 "Mars Coordinates") is the coordinate system
|
|
mandated for maps in China. All maps in China must use GCJ-02, including
|
|
AMap (Gaode/高德) and Baidu Maps.
|
|
|
|
GPS devices output WGS-84 coordinates, which must be converted to GCJ-02
|
|
for use with Chinese map services, and vice versa.
|
|
"""
|
|
|
|
import math
|
|
|
|
# Krasovsky 1940 ellipsoid parameters
|
|
_A = 6378245.0 # Semi-major axis
|
|
_EE = 0.00669342162296594323 # Eccentricity squared
|
|
|
|
|
|
def _out_of_china(lat: float, lon: float) -> bool:
|
|
"""Check if coordinates are outside China's approximate bounds."""
|
|
return not (72.004 <= lon <= 137.8347 and 0.8293 <= lat <= 55.8271)
|
|
|
|
|
|
def _transform_lat(x: float, y: float) -> float:
|
|
"""Transform latitude offset."""
|
|
ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * math.sqrt(abs(x))
|
|
ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
|
|
ret += (20.0 * math.sin(y * math.pi) + 40.0 * math.sin(y / 3.0 * math.pi)) * 2.0 / 3.0
|
|
ret += (160.0 * math.sin(y / 12.0 * math.pi) + 320 * math.sin(y * math.pi / 30.0)) * 2.0 / 3.0
|
|
return ret
|
|
|
|
|
|
def _transform_lon(x: float, y: float) -> float:
|
|
"""Transform longitude offset."""
|
|
ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * math.sqrt(abs(x))
|
|
ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
|
|
ret += (20.0 * math.sin(x * math.pi) + 40.0 * math.sin(x / 3.0 * math.pi)) * 2.0 / 3.0
|
|
ret += (150.0 * math.sin(x / 12.0 * math.pi) + 300.0 * math.sin(x / 30.0 * math.pi)) * 2.0 / 3.0
|
|
return ret
|
|
|
|
|
|
def wgs84_to_gcj02(lat: float, lon: float) -> tuple[float, float]:
|
|
"""
|
|
Convert WGS-84 coordinates to GCJ-02.
|
|
|
|
Args:
|
|
lat: WGS-84 latitude
|
|
lon: WGS-84 longitude
|
|
|
|
Returns:
|
|
Tuple of (GCJ-02 latitude, GCJ-02 longitude)
|
|
"""
|
|
if _out_of_china(lat, lon):
|
|
return lat, lon
|
|
|
|
dlat = _transform_lat(lon - 105.0, lat - 35.0)
|
|
dlon = _transform_lon(lon - 105.0, lat - 35.0)
|
|
|
|
radlat = lat / 180.0 * math.pi
|
|
magic = math.sin(radlat)
|
|
magic = 1 - _EE * magic * magic
|
|
sqrtmagic = math.sqrt(magic)
|
|
|
|
dlat = (dlat * 180.0) / ((_A * (1 - _EE)) / (magic * sqrtmagic) * math.pi)
|
|
dlon = (dlon * 180.0) / (_A / sqrtmagic * math.cos(radlat) * math.pi)
|
|
|
|
return lat + dlat, lon + dlon
|
|
|
|
|
|
def gcj02_to_wgs84(lat: float, lon: float) -> tuple[float, float]:
|
|
"""
|
|
Convert GCJ-02 coordinates to WGS-84 (approximate inverse).
|
|
|
|
Uses iterative approach for better accuracy (~0.5m error).
|
|
|
|
Args:
|
|
lat: GCJ-02 latitude
|
|
lon: GCJ-02 longitude
|
|
|
|
Returns:
|
|
Tuple of (WGS-84 latitude, WGS-84 longitude)
|
|
"""
|
|
if _out_of_china(lat, lon):
|
|
return lat, lon
|
|
|
|
# Iterative approach for better accuracy
|
|
wgs_lat, wgs_lon = lat, lon
|
|
for _ in range(5):
|
|
gcj_lat, gcj_lon = wgs84_to_gcj02(wgs_lat, wgs_lon)
|
|
wgs_lat += lat - gcj_lat
|
|
wgs_lon += lon - gcj_lon
|
|
|
|
return wgs_lat, wgs_lon
|