toys/gps/docs/source/coord/wgs84_gcj02.cpp

#include "geo/private/base/constant.h"
#include "geo/private/util/angle_util.h"
#include "geo/private/coord/wgs84_gcj02.h"

namespace ns
{
namespace geo
{

namespace
{
constexpr auto ee = 0.00669342162296594323;
constexpr auto earth_radius = EARTH_EQUATOR_RADIUS;

double transformLat(const double& x, const double& y)
{
    double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(fabs(x));
    ret += (20.0 * sin(6.0 * x * c_pi()) + 20.0 * sin(2.0 * x * c_pi())) * 2.0 / 3.0;
    ret += (20.0 * sin(y * c_pi()) + 40.0 * sin(y / 3.0 * c_pi())) * 2.0 / 3.0;
    ret += (160.0 * sin(y / 12.0 * c_pi()) + 320 * sin(y * c_pi() / 30.0)) * 2.0 / 3.0;
    return ret;
}

double transformLon(const double& x, const double& y)
{
    double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(fabs(x));
    ret += (20.0 * sin(6.0 * x * c_pi()) + 20.0 * sin(2.0 * x * c_pi())) * 2.0 / 3.0;
    ret += (20.0 * sin(x * c_pi()) + 40.0 * sin(x / 3.0 * c_pi())) * 2.0 / 3.0;
    ret += (150.0 * sin(x / 12.0 * c_pi()) + 300.0 * sin(x / 30.0 * c_pi())) * 2.0 / 3.0;
    return ret;
}

void transform(const double& lat, const double& lon, double& relat, double& relon)
{
    double dLat = transformLat(lon - 105.0, lat - 35.0);
    double dLon = transformLon(lon - 105.0, lat - 35.0);
    double radLat = lat / 180.0 * c_pi();
    double magic = sin(radLat);
    magic = 1 - ee * magic * magic;
    double sqrtMagic = sqrt(magic);
    dLat = (dLat * 180.0) / ((earth_radius * (1 - ee)) / (magic * sqrtMagic) * c_pi());
    dLon = (dLon * 180.0) / (earth_radius / sqrtMagic * cos(radLat) * c_pi());
    relat = lat + dLat;
    relon = lon + dLon;
}

} // namespace

void wgs84_to_gcj02(const PointD& pt_wgs84, PointD& pt_gcj02)
{
    double lon = pt_wgs84.x;
    double lat = pt_wgs84.y;
    double latout, lonout;
    double dLat = transformLat(lon - 105.0, lat - 35.0);
    double dLon = transformLon(lon - 105.0, lat - 35.0);
    double radLat = lat / 180.0 * c_pi();
    double magic = sin(radLat);
    magic = 1 - ee * magic * magic;
    double sqrtMagic = sqrt(magic);
    dLat = (dLat * 180.0) / ((earth_radius * (1 - ee)) / (magic * sqrtMagic) * c_pi());
    dLon = (dLon * 180.0) / (earth_radius / sqrtMagic * cos(radLat) * c_pi());
    latout = lat + dLat;
    lonout = lon + dLon;

    pt_gcj02.x = lonout;
    pt_gcj02.y = latout;
    pt_gcj02.z = pt_wgs84.z;
}

void gcj02_to_wgs84(const PointD& pt_gcj02, PointD& pt_wgs84)
{
    // double lat = pt_gcj02.y;
    // double lon = pt_gcj02.x;
    // double retlat, retlon;
    // transform(lat, lon, retlat, retlon);
    // double longitude = lon * 2 - retlon;
    // double latitude = lat * 2 - retlat;

    // pt_wgs84.x = longitude;
    // pt_wgs84.y = latitude;
    // pt_wgs84.z = pt_gcj02.z;

    PointD p1, p2;
    p1 = pt_gcj02;

    wgs84_to_gcj02(p1, p2);
    p1.x = pt_gcj02.x - p2.x + p1.x;
    p1.y = pt_gcj02.y - p2.y + p1.y;
    wgs84_to_gcj02(p1, p2);
    p1.x = pt_gcj02.x - p2.x + p1.x;
    p1.y = pt_gcj02.y - p2.y + p1.y;
    wgs84_to_gcj02(p1, p2);
    p1.x = pt_gcj02.x - p2.x + p1.x;
    p1.y = pt_gcj02.y - p2.y + p1.y;

    pt_wgs84.x = p1.x;
    pt_wgs84.y = p1.y;
    pt_wgs84.z = p1.z;
}

} // namespace geo
} // namespace ns