donkey
/
toys

 
			
			   
				 
					
						
						
							
							/**
 *  @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 <anjingyu_ws@foxmail.com>.
 *
 */

#include "geo/coordinate_converter.h"

#include <cmath> /**< std::sqrt std::fabs NAN std::isnan std::sin std::cos */
#include <map>   /**< std::map */

#include "geo/math.h"

namespace ns {
namespace geo {

static double _calculateCentralMeridian(const double longtitude,
                                        CoordinateConverter::ZoneWidth ZoneWidth)
{
    int width = static_cast<int>(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 &params)
{
    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 &params)
{
    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