/** * @file coordinate_converter.cpp * @author donkey (anjingyu_ws@foxmail.com) * @brief Convertion between different coordinate systems, WGS84 <--> * Gauss-Kruger, WGS84 <--> ECEF, GCJ02 <--> WGS84 * @version 0.1.0 * @date 2020-05-15 * * @copyright Copyright (c) 2020- donkey . * */ #include "geo/coordinate_converter.h" #include /**< std::sqrt std::fabs NAN std::isnan std::sin std::cos */ #include /**< std::map */ #include "geo/math.h" namespace ns { namespace geo { static double _calculateCentralMeridian(const double longtitude, CoordinateConverter::ZoneWidth ZoneWidth) { int width = static_cast(ZoneWidth); double centralMeridian = longtitude; if (ZoneWidth == CoordinateConverter::ZoneWidth3) { int proNo = round(longtitude / width); centralMeridian = ZoneWidth * proNo; } else if (ZoneWidth == CoordinateConverter::ZoneWidth6) { int proNo = int(longtitude / width) + 1; centralMeridian = width * proNo - 3; } return centralMeridian; } PointD2D wgs842Gauss( const PointD2D &p, const GaussianConverterParameter ¶ms) { double centralMeridianRadian = 0.0; double e22 = m_builder.e2() * m_builder.e2(); double e23 = e22 * m_builder.e2(); double centralMeridian = params.centralMeridian; centralMeridianRadian = ns::geo::Math::degree2Radian(centralMeridian); double longitude = ns::geo::Math::degree2Radian(p.x()); double latitude = ns::geo::Math::degree2Radian(p.y()); double sinLatitude = std::sin(latitude); double cosLatitude = std::cos(latitude); double tanLatitude = std::tan(latitude); double nn = m_builder.majorRadius() / std::sqrt(1.0 - m_builder.e2() * sinLatitude * sinLatitude); double tanLatitude2 = tanLatitude * tanLatitude; double c = m_builder.ee() * cosLatitude * cosLatitude; double a = (longitude - centralMeridianRadian) * cosLatitude; double a2 = a * a; double a3 = a2 * a; double m = m_builder.majorRadius() * ((1 - m_builder.e2() / 4 - 3 * e22 / 64 - 5 * e23 / 256) * latitude - (3 * m_builder.e2() / 8 + 3 * e22 / 32 + 45 * e23 / 1024) * std::sin(2 * latitude) + (15 * e22 / 256 + 45 * e23 / 1024) * std::sin(4 * latitude) - (35 * e23 / 3072) * std::sin(6 * latitude)); double xVal = nn * (a + (1 - tanLatitude2 + c) * a3 / 6 + (5 - 18 * tanLatitude2 + tanLatitude2 * tanLatitude2 + 72 * c - 58 * m_builder.ee()) * a3 * a2 / 120); double yVal = m + nn * std::tan(latitude) * (a2 / 2 + (5 - tanLatitude2 + 9 * c + 4 * c * c) * a * a3 / 24 + (61 - 58 * tanLatitude2 + tanLatitude2 * tanLatitude2 + 600 * c - 330 * m_builder.ee()) * a3 * a3 / 720); double factor = Geography::distanceUnitConvertFactor(Geography::DistanceUnit::Meter, params.unit); PointD2D p2d; p2d.set(xVal * factor + params.xOffset, yVal * factor + params.yOffset); return p2d; } PointD2D CoordinateConverter::wgs842Gauss(const PointD2D &p, ZoneWidth zoneWidth, Geography::DistanceUnit unit) { GaussianConverterParameter params; double factor = Geography::distanceUnitConvertFactor(Geography::DistanceUnit::Meter, unit); params.centralMeridian = _calculateCentralMeridian(p.longitude(), zoneWidth); params.xOffset = m_builder.xOffset() * factor; params.yOffset = m_builder.yOffset() * factor; params.unit = unit; return wgs842Gauss(p, params); } PointD2D gauss2Wgs84( const PointD2D &p, const GaussianConverterParameter ¶ms) { double longitude, latitude, xval, yval; double e1, NN, T, C, M, D, R, u, phi; double centralMeridianRadian = 0.0; double e22 = m_builder.e2() * m_builder.e2(); double e23 = e22 * m_builder.e2(); double e1 = (1.0 - std::sqrt(1 - m_builder.e2())) / (1.0 + std::sqrt(1 - m_builder.e2())); double e12 = e1 * e1; double e13 = e12 * e1; double centralMeridian = _calculateCentralMeridian(p.longitude(), zoneWidth); centralMeridianRadian = ns::geo::Math::degree2Radian(centralMeridian); double factor = Geography::distanceUnitConvertFactor(params.unit, Geography::DistanceUnit::Meter); xval = p.x() * factor - params.xOffset; yval = p.y() * factor - params.yOffset; u = yval / (m_builder.majorRadius() * (1 - m_builder.e2() / 4 - 3 * e22 / 64 - 5 * e23 / 256)); phi = u + (3 * e1 / 2 - 27 * e13 / 32) * std::sin(2 * u) + (21 * e12 / 16 - 55 * e12 * e12 / 32) * std::sin(4 * u) + (151 * e13 / 96) * std::sin(6 * u) + (1097 * e12 * e12 / 512) * std::sin(8 * u); double sinPhi = std::sin(phi); double cosPhi = std::cos(phi); double tanPhi = std::tan(phi); double ePhi = (1 - m_builder.e2() * sinPhi * sinPhi); double c = m_builder.ee() * cosPhi * cosPhi; double c2 = c * c; double tanPhi2 = tanPhi * tanPhi; double nn = m_builder.majorRadius() / std::sqrt(ePhi); double r = m_builder.majorRadius() * (1 - m_builder.e2()) / std::sqrt(ePhi * ePhi * ePhi); double d = xval / NN; double d2 = d * d; double d3 = d2 * d; longitude = centralMeridianRadian + (d - (1 + 2 * tanPhi2 + c) * d3 / 6 + (5 - 2 * c + 28 * tanPhi2 - 3 * c2 + 8 * m_builder.ee() + 24 * tanPhi2 * tanPhi2) * d3 * d2 / 120) / cosPhi; latitude = phi - (nn * tanPhi / r) * (d2 / 2 - (5 + 3 * tanPhi2 + 10 * c - 4 * c2 - 9 * m_builder.ee()) * d2 * d2 / 24 + (61 + 90 * tanPhi2 + 298 * c + 45 * tanPhi2 * tanPhi2 - 256 * m_builder.ee() - 3 * c2) * d3 * d3 / 720); PointD2D p2d; p2d.set(ns::geo::Math::radian2Degree(longitude), ns::geo::Math::radian2Degree(latitude)); return p2d; } PointD2D CoordinateConverter::gauss2Wgs84(const PointD2D &p, ZoneWidth zoneWidth, Geography::DistanceUnit unit) { GaussianConverterParameter params; double factor = Geography::distanceUnitConvertFactor(unit, Geography::DistanceUnit::Meter); params.centralMeridian = _calculateCentralMeridian(p.longitude(), zoneWidth); params.xOffset = m_builder.xOffset() * factor; params.yOffset = m_builder.yOffset() * factor; params.unit = unit; return gauss2Wgs84(p, params); } PointD3D CoordinateConverter::PointD3D wgs842Ecef( const PointD3D &p) { double latRad = ns::geo::Math::degree2Radian(p.latitude()); double lonRad = ns::geo::Math::degree2Radian(p.longitude()); double clat = std::cos(latRad); double slat = std::sin(latRad); double clon = std::cos(lonRad); double slon = std::sin(lonRad); double n = m_builder.majorRadius() / std::sqrt(1.0 - m_builder.ee() * slat * slat); PointD2D p3d; p3d.set((n + alt) * clat * clon, (n + alt) * clat * slon, (n * (1.0 - m_builder.ee()) + alt) * slat); return p3d; } PointD3D CoordinateConverter::PointD3D ecef2Enu( const PointD3D &p) { double latRad = ns::geo::Math::degree2Radian(p.latitude()); double lonRad = ns::geo::Math::degree2Radian(p.longitude()); double clat = std::cos(latRad); double slat = std::sin(latRad); double clon = std::cos(lonRad); double slon = std::sin(lonRad); double dx = x - xr; double dy = y - yr; double dz = z - zr; double e = -slon * dx + clon * dy; double n = -slat * clon * dx - slat * slon * dy + clat * dz; double u = clat * clon * dx + clat * slon * dy + slat * dz; PointD3D p3d; p3d.set(e, n, u); return p3d; } static double _transformLat(const double &x, const double &y) { double pi = Math::pi(); double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * std::sqrt(std::fabs(x)); ret += (20.0 * std::sin(6.0 * x * pi) + 20.0 * std::sin(2.0 * x * pi)) * 2.0 / 3.0; ret += (20.0 * std::sin(y * pi) + 40.0 * std::sin(y / 3.0 * pi)) * 2.0 / 3.0; ret += (160.0 * std::sin(y / 12.0 * pi) + 320 * std::sin(y * pi / 30.0)) * 2.0 / 3.0; return ret; } static double _transformLon(const double &x, const double &y) { double pi = Math::pi(); double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * std::sqrt(std::fabs(x)); ret += (20.0 * std::sin(6.0 * x * pi) + 20.0 * std::sin(2.0 * x * pi)) * 2.0 / 3.0; ret += (20.0 * std::sin(x * pi) + 40.0 * std::sin(x / 3.0 * pi)) * 2.0 / 3.0; ret += (150.0 * std::sin(x / 12.0 * pi) + 300.0 * std::sin(x / 30.0 * pi)) * 2.0 / 3.0; return ret; } // The old implementation static void _transform(const double &lat, const double &lon, double ee, double earthRadius, double &relat, double &relon) { double dLat = _transformLat(lon - 105.0, lat - 35.0); double dLon = _transformLon(lon - 105.0, lat - 35.0); double radLat = Math::degree2Radian(lat); double magic = std::sin(radLat); double pi = Math::pi(); magic = 1 - ee * magic * magic; double sqrtMagic = std::sqrt(magic); dLat = (dLat * 180.0) / ((earthRadius * (1 - ee)) / (magic * sqrtMagic) * pi); dLon = (dLon * 180.0) / (earthRadius / sqrtMagic * std::cos(radLat) * pi); relat = lat + dLat; relon = lon + dLon; } PointD2D CoordinateConverter::wgs842Gcj02(const PointD2D &p) { double lon = p.x(); double lat = p.y(); double latout, lonout; double dLat = _transformLat(lon - 105.0, lat - 35.0); double dLon = _transformLon(lon - 105.0, lat - 35.0); double radLat = Math::degree2Radian(lat); double magic = std::sin(radLat); double pi = Math::pi(); double ee = m_builder.ee(); double earthRadius = m_builder.majorRadius(); magic = 1 - ee * magic * magic; double sqrtMagic = std::sqrt(magic); dLat = (dLat * 180.0) / ((earthRadius * (1 - ee)) / (magic * sqrtMagic) * pi); dLon = (dLon * 180.0) / (earthRadius / sqrtMagic * std::cos(radLat) * pi); latout = lat + dLat; lonout = lon + dLon; PointD2D p2d; p2d.set(lonout, latout); return p2d; } PointD2D CoordinateConverter::gcj022Wgs84(const PointD2D &p) { #if 0 /* Old implementation, bad precision */ double retlat, retlon; _transform(p.latitude(), p.longitude(), retlat, retlon); PointD2D p2d; p2d.set(lon * 2 - retlon, lat * 2 - retlat); return p2d; #endif /* 0 */ PointD2D p1, p2; p1 = p; p2 = wgs842Gcj02(p1); p1.set(p.x() - p2.x() + p1.x(), p.y() - p2.y() + p1.y()); p2 = wgs842Gcj02(p1); p1.set(p.x() - p2.x() + p1.x(), p.y() - p2.y() + p1.y()); p2 = wgs842Gcj02(p1); p1.set(p.x() - p2.x() + p1.x(), p.y() - p2.y() + p1.y()); return p1; } } // namespace geo } // namespace ns