From 17f1b68087cdc6597a21cef10b150a27b2ff199d Mon Sep 17 00:00:00 2001 From: Yi Zhang Date: Wed, 5 Mar 2025 22:37:40 +0800 Subject: [PATCH] tmp --- example/refellipsoid_ex.cpp | 41 +++++++++++++---- lib/geometry/refellipsoid.cpp | 83 ++++++++++++++++++++++++++++------- lib/geometry/refellipsoid.h | 28 ++++++++++-- 3 files changed, 124 insertions(+), 28 deletions(-) diff --git a/example/refellipsoid_ex.cpp b/example/refellipsoid_ex.cpp index 244493e..650dd64 100644 --- a/example/refellipsoid_ex.cpp +++ b/example/refellipsoid_ex.cpp @@ -25,20 +25,45 @@ * Also add information on how to contact you by electronic and paper mail. ******************************************************/ -#include "../lib/geodesy.h" +#include "../lib/geometry.h" using namespace gctl; int main(int argc, char const *argv[]) try { - refellipsoid relli(WGS84); - refellipsoid relli2(1000, 900, false); - refellipsoid relli3(1000, 10, true); + refellipsoid ellip; + ellip.set(WGS84); +/* + point3ds ps; + ps.set_io_precision(17); + ps.lon = -110; + ellip.geodetic2spherical(-40, 0, ps.lat, ps.rad); + std::cout << "Geocentric: " << ps << std::endl; - for (size_t i = 0; i <= 90; i++) - { - std::cout << i << " " << relli.radis_at(1.0*i) << " " << relli2.radis_at(1.0*i) << " " << relli3.radis_at(1.0*i) << "\n"; - } + point3dc pc = ps.s2c(); + pc.set_io_precision(17); + std::cout << "XYZ: " << pc << std::endl; + + ellip.spherical2geodetic(ps, ps.lon, ps.lat, ps.rad); + std::cout << "Geodetic: " << ps << std::endl; + + ellip.xyz2geodetic(pc, ps.lon, ps.lat, ps.rad); + std::cout << "Geodetic: " << ps << std::endl; +*/ + + point3ds ps; + ps.set_io_precision(17); + // 注意这是一个地心纬度为40的椭球上的点 + ps.lon = 110; + ps.lat = 40; + ps.rad = ellip.radius_at(ps.lat); + + point3dc pc; + pc.set_io_precision(17); + pc = ps.s2c(); + ellip.xyz2geodetic(pc, ps.lon, ps.lat, ps.rad); + std::cout << "Geodetic: " << ps << std::endl; + return 0; } catch(std::exception &e) { diff --git a/lib/geometry/refellipsoid.cpp b/lib/geometry/refellipsoid.cpp index 785f259..97e6beb 100644 --- a/lib/geometry/refellipsoid.cpp +++ b/lib/geometry/refellipsoid.cpp @@ -51,8 +51,8 @@ void gctl::refellipsoid::set(refellipsoid_type_e refellipsoid) else if (refellipsoid == Ardalan2010) {r_ = GCTL_Mars_Ardalan2010_PoleRadius; R_ = GCTL_Mars_Ardalan2010_EquatorRadius;} else throw std::invalid_argument("Invalid reference system type for gctl::refellipsoid::set(...)"); - eps_ = sqrt(1.0 - (r_*r_)/(R_*R_)); - epssq_ = eps_*eps_; + f_ = (R_ - r_)/R_; + e_ = sqrt(R_*R_ - r_*r_)/R_; return; } @@ -62,14 +62,15 @@ void gctl::refellipsoid::set(double R, double r_or_flat, bool is_flat) if (is_flat) r_ = R_*(1.0 - 1.0/r_or_flat); else r_ = r_or_flat; - eps_ = sqrt(1.0 - (r_*r_)/(R_*R_)); - epssq_ = eps_*eps_; + f_ = (R_ - r_)/R_; + e_ = sqrt(R_*R_ - r_*r_)/R_; return; } -double gctl::refellipsoid::radius_at(double lati) +double gctl::refellipsoid::radius_at(double geocentric_lati) { - return ellipse_radius_2d(R_, r_, lati*M_PI/180.0, 0.0); + return ellipse_radius_2d(R_, r_, geocentric_lati*GCTL_Pi/180.0); + //return R_/sqrt(1.0 + e_*e_*sind(geocentric_lati)*sind(geocentric_lati)); } void gctl::refellipsoid::geodetic2spherical(double geodetic_lati, @@ -81,18 +82,66 @@ void gctl::refellipsoid::geodetic2spherical(double geodetic_lati, ** reference ellipsoid), to Earth Centered Earth Fixed Cartesian ** coordinates, and then to spherical coordinates. */ - CosLat = cosd(geodetic_lati); - SinLat = sind(geodetic_lati); + CosLat = cosd(geodetic_lati); + SinLat = sind(geodetic_lati); - // compute the local rRdius of curvature on the WGS-84 reference ellipsoid - rc = R_ / sqrt(1.0 - epssq_ * SinLat * SinLat); + double eps = sqrt(1.0 - (r_*r_)/(R_*R_)); + double epssq = eps*eps; - // compute ECEF Cartesian coordinates of specified point (for longitude=0) - xp = (rc + geodetic_hei) * CosLat; - zp = (rc*(1.0 - epssq_) + geodetic_hei) * SinLat; + // compute the local rRdius of curvature on the WGS-84 reference ellipsoid + rc = R_ / sqrt(1.0 - epssq * SinLat * SinLat); - // compute spherical rRdius and angle lambda and phi of specified point - sph_rad = sqrt(xp * xp + zp * zp); - sph_lati = deg(asin(zp/sph_rad)); - return; + // compute ECEF Cartesian coordinates of specified point (for longitude=0) + xp = (rc + geodetic_hei) * CosLat; + zp = (rc*(1.0 - epssq) + geodetic_hei) * SinLat; + + // compute spherical rRdius and angle lambda and phi of specified point + sph_rad = sqrt(xp * xp + zp * zp); + sph_lati = deg(asin(zp/sph_rad)); + return; +} + +void gctl::refellipsoid::spherical2geodetic(const point3ds& ps, double& geodetic_lon, double& geodetic_lati, + double& geodetic_hei, double eps, int cnt) +{ + point3dc pc = ps.s2c(); + xyz2geodetic(pc, geodetic_lon, geodetic_lati, geodetic_hei, eps, cnt); + return; +} + +void gctl::refellipsoid::xyz2geodetic(const point3dc& pc, double& geodetic_lon, + double& geodetic_lati, double& geodetic_hei, double eps, int cnt) +{ + double curB, N; + double H = sqrt(pc.x*pc.x + pc.y*pc.y); + double calB = atan2(pc.z, H); + + int c = 0; + do + { + curB = calB; + N = R_/sqrt(1 - e_*e_*sin(curB)*sin(curB)); + calB = atan2(pc.z + N*e_*e_*sin(curB), H); + c++; + } + while (abs(curB - calB)*180.0/GCTL_Pi > eps && c < cnt); + + geodetic_lon = atan2(pc.y, pc.x)*180.0/GCTL_Pi; + geodetic_lati = curB*180.0/GCTL_Pi; + geodetic_hei = pc.z/sin(curB) - N*(1 - e_*e_); + return; +} + +void gctl::refellipsoid::geodetic2xyz(double geodetic_lon, double geodetic_lati, + double geodetic_hei, point3dc& pc) +{ + double L = arc(geodetic_lon); + double B = arc(geodetic_lati); + double H = geodetic_hei; + + double N = R_/sqrt(1 - e_*e_*sin(B)*sin(B)); + pc.x = (N + H)*cos(B)*cos(L); + pc.y = (N + H)*cos(B)*sin(L); + pc.z = (N*(1 - e_*e_) + H)*sin(B); + return; } \ No newline at end of file diff --git a/lib/geometry/refellipsoid.h b/lib/geometry/refellipsoid.h index d1b495c..07f37b4 100644 --- a/lib/geometry/refellipsoid.h +++ b/lib/geometry/refellipsoid.h @@ -31,6 +31,8 @@ #include "../core/macro.h" #include "../core/exceptions.h" #include "../maths/mathfunc.h" +#include "point3c.h" +#include "point3s.h" namespace gctl { @@ -78,10 +80,10 @@ namespace gctl /** * @brief Get the ellipsoidal radius at the inquiring latitude * - * @param lati the inquiring latitude + * @param geocentric_lati the inquiring latitude * @return ellipsoidal radius */ - double radius_at(double lati); + double radius_at(double geocentric_lati); /** * @brief Converts Geodetic coordinates to Spherical coordinates @@ -94,8 +96,28 @@ namespace gctl void geodetic2spherical(double geodetic_lati, double geodetic_hei, double& sph_lati, double& sph_rad); + void spherical2geodetic(const point3ds& ps, double& geodetic_lon, double& geodetic_lati, + double& geodetic_hei, double eps = 1e-16, int cnt = 30); + + /** + * @brief Convert xyz coordinates to Geodetic coodinates + * + * @param pc A point + * @param geodetic_lon Geodetic longitude + * @param geodetic_lati Geodetic latitude + * @param geodetic_hei Height above the ellipsoid + * @param eps solving precision + * @param cnt maximal iteration + */ + void xyz2geodetic(const point3dc& pc, double& geodetic_lon, double& geodetic_lati, + double& geodetic_hei, double eps = 1e-16, int cnt = 30); + + void geodetic2xyz(double geodetic_lon, double geodetic_lati, + double geodetic_hei, point3dc& pc); + private: - double r_, R_, eps_, epssq_; + double r_, R_, f_; // semi-minor, semi-major and flat rate + double e_; }; };