geopro/external/gpr3dviewer/CoordinateTransform.cpp

#include "CoordinateTransform.h"
#include <cmath>

// 静态辅助：预计算的子午线弧长系数
static inline double computeA0(double e2) {
    return 1.0 + 3.0/4.0 * e2 + 45.0/64.0 * e2*e2 + 175.0/256.0 * e2*e2*e2
           + 11025.0/16384.0 * e2*e2*e2*e2;
}
static inline double computeA2(double e2) {
    return 3.0/4.0 * e2 + 15.0/16.0 * e2*e2 + 525.0/512.0 * e2*e2*e2
           + 2205.0/2048.0 * e2*e2*e2*e2;
}
static inline double computeA4(double e2) {
    return 15.0/64.0 * e2*e2 + 105.0/256.0 * e2*e2*e2
           + 2205.0/4096.0 * e2*e2*e2*e2;
}
static inline double computeA6(double e2) {
    return 35.0/512.0 * e2*e2*e2 + 315.0/2048.0 * e2*e2*e2*e2;
}
static inline double computeA8(double e2) {
    return 315.0/16384.0 * e2*e2*e2*e2;
}

int CoordinateTransform::detectZoneFromCgcsY(double cgcsY)
{
    // 中国范围内 CGCS2000 坐标 Y 通常为正值且包含带号
    // 格式：zone * 1,000,000 + 500,000 + easting
    double absY = std::abs(cgcsY);
    if (absY > 1e6) {
        int zone = static_cast<int>(std::floor(absY / 1e6));
        if (zone >= 1 && zone <= 60)
            return zone;
    }
    // 如果无法推断，返回 0（表示需要手动指定）
    return 0;
}

double CoordinateTransform::centralMeridianFromZone(int zone)
{
    // 3度带：中央经线 = 带号 * 3°
    return zone * 3.0;
}

double CoordinateTransform::meridianArc(double latRad)
{
    const double e2 = E2;
    const double a = A;

    const double A0 = computeA0(e2);
    const double A2 = computeA2(e2);
    const double A4 = computeA4(e2);
    const double A6 = computeA6(e2);
    const double A8 = computeA8(e2);

    double s2 = std::sin(2.0 * latRad);
    double s4 = std::sin(4.0 * latRad);
    double s6 = std::sin(6.0 * latRad);
    double s8 = std::sin(8.0 * latRad);

    return a * (1.0 - e2) * (A0 * latRad - A2/2.0 * s2 + A4/4.0 * s4
                              - A6/6.0 * s6 + A8/8.0 * s8);
}

double CoordinateTransform::meridianArcInverse(double x)
{
    const double e2 = E2;
    const double a = A;

    const double A0 = computeA0(e2);
    const double A2 = computeA2(e2);
    const double A4 = computeA4(e2);
    const double A6 = computeA6(e2);
    const double A8 = computeA8(e2);

    const double c0 = a * (1.0 - e2) * A0;

    double bf = x / c0;
    for (int i = 0; i < 8; ++i) {
        double s2 = std::sin(2.0 * bf);
        double s4 = std::sin(4.0 * bf);
        double s6 = std::sin(6.0 * bf);
        double s8 = std::sin(8.0 * bf);

        double fx = a * (1.0 - e2) * (A0 * bf - A2/2.0 * s2 + A4/4.0 * s4
                                      - A6/6.0 * s6 + A8/8.0 * s8) - x;
        double fpx = a * (1.0 - e2) * (A0 - A2 * std::cos(2.0 * bf)
                                       + A4 * std::cos(4.0 * bf)
                                       - A6 * std::cos(6.0 * bf)
                                       + A8 * std::cos(8.0 * bf));

        double delta = fx / fpx;
        bf -= delta;
        if (std::abs(delta) < 1e-12)
            break;
    }
    return bf;
}

void CoordinateTransform::cgcs2000ToWgs84(double cgcsX, double cgcsY, int zone,
                                          double &latRad, double &lonRad)
{
    const double a = A;
    const double e2 = E2;
    const double e12 = E12;

    // 提取东偏移（去掉带号和 500km 常数）
    double easting = cgcsY;
    if (zone > 0) {
        easting = cgcsY - zone * 1e6 - FALSE_EASTING;
    } else {
        easting = cgcsY - FALSE_EASTING;
    }
    double northing = cgcsX;
    double l0 = centralMeridianFromZone(zone) * DEG2RAD;

    // 底点纬度
    double bf = meridianArcInverse(northing);

    double sinBf = std::sin(bf);
    double cosBf = std::cos(bf);
    double tanBf = std::tan(bf);

    double Nf = a / std::sqrt(1.0 - e2 * sinBf * sinBf);
    double etaf2 = e12 * cosBf * cosBf;
    double tf = tanBf;
    double Vf2 = 1.0 + etaf2;

    double y = easting;
    double y2 = y * y;
    double y4 = y2 * y2;
    double y6 = y4 * y2;

    double Nf2 = Nf * Nf;
    double Nf4 = Nf2 * Nf2;
    double Nf6 = Nf4 * Nf2;

    // 纬度修正
    double dB = tf / Vf2 * (
        y2 / (2.0 * Nf2) * (1.0 + etaf2)
        - y4 / (24.0 * Nf4) * (5.0 + 3.0 * tf*tf + 6.0 * etaf2
                                - 6.0 * tf*tf * etaf2
                                - 3.0 * etaf2*etaf2
                                - 9.0 * tf*tf * etaf2*etaf2)
        + y6 / (720.0 * Nf6) * (61.0 + 90.0 * tf*tf + 45.0 * tf*tf*tf*tf)
    );

    latRad = bf - dB;

    // 经差
    double dl = y / (Nf * cosBf) * (
        1.0
        - y2 / (6.0 * Nf2) * (1.0 + 2.0 * tf*tf + etaf2)
        + y4 / (120.0 * Nf4) * (5.0 + 28.0 * tf*tf + 24.0 * tf*tf*tf*tf
                                + 6.0 * etaf2 + 8.0 * tf*tf * etaf2)
        - y6 / (5040.0 * Nf6) * (61.0 + 662.0 * tf*tf + 1320.0 * tf*tf*tf*tf
                                 + 720.0 * tf*tf*tf*tf*tf*tf)
    );

    lonRad = l0 + dl;
}

void CoordinateTransform::wgs84ToCgcs2000(double latRad, double lonRad, int zone,
                                          double &cgcsX, double &cgcsY)
{
    const double a = A;
    const double e2 = E2;
    const double e12 = E12;

    double l0 = centralMeridianFromZone(zone) * DEG2RAD;
    double l = lonRad - l0;

    double sinB = std::sin(latRad);
    double cosB = std::cos(latRad);
    double tanB = std::tan(latRad);

    double N = a / std::sqrt(1.0 - e2 * sinB * sinB);
    double t2 = tanB * tanB;
    double t4 = t2 * t2;
    double eta2 = e12 * cosB * cosB;
    double eta4 = eta2 * eta2;

    double l2 = l * l;
    double l4 = l2 * l2;
    double l6 = l4 * l2;

    // 子午线弧长
    double X = meridianArc(latRad);

    // 北坐标
    cgcsX = X + N / 2.0 * sinB * cosB * l2 * (
        1.0 + l2 / 12.0 * (5.0 - t2 + 9.0 * eta2 + 4.0 * eta4)
        + l4 / 360.0 * (61.0 - 58.0 * t2 + t4)
    );

    // 东坐标（自然值，不含带号和 500km）
    double yNatural = N * cosB * l * (
        1.0 + l2 / 6.0 * (1.0 - t2 + eta2)
        + l4 / 120.0 * (5.0 - 18.0 * t2 + t4 + 14.0 * eta2 - 58.0 * t2 * eta2)
    );

    // 加上带号和 500km 常数
    if (zone > 0) {
        cgcsY = zone * 1e6 + FALSE_EASTING + yNatural;
    } else {
        cgcsY = FALSE_EASTING + yNatural;
    }
}

void CoordinateTransform::wgs84ToWebMercator(double latRad, double lonRad, double &mx, double &my)
{
    mx = EARTH_RADIUS * lonRad;
    my = EARTH_RADIUS * std::log(std::tan(PI / 4.0 + latRad / 2.0));
}

void CoordinateTransform::webMercatorToWgs84(double mx, double my, double &latRad, double &lonRad)
{
    lonRad = mx / EARTH_RADIUS;
    latRad = 2.0 * std::atan(std::exp(my / EARTH_RADIUS)) - PI / 2.0;
}