From f2148707b722f47f7f5f8dec68a30a0965401d69 Mon Sep 17 00:00:00 2001 From: codeboss <2422523675@qq.com> Date: Sat, 7 Jun 2025 15:11:48 +0800 Subject: [PATCH] =?UTF-8?q?lla=E5=92=8C=E5=A4=A7=E5=9C=B0=E7=BA=BF?= =?UTF-8?q?=E4=BA=92=E8=BD=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- StandardGlobe/standardglobe.cpp | 70 +++++++++++++++++++++++++++++---- StandardGlobe/standardglobe.h | 18 ++++++++- 2 files changed, 80 insertions(+), 8 deletions(-) diff --git a/StandardGlobe/standardglobe.cpp b/StandardGlobe/standardglobe.cpp index ebba849..6ff891b 100644 --- a/StandardGlobe/standardglobe.cpp +++ b/StandardGlobe/standardglobe.cpp @@ -1,7 +1,11 @@ #include "standardglobe.h" -#include +#include double StandardGlobe::_radius_m = 65000000; +#define PI 3.14159265354 +auto deg2a = [](double v) { return v / 180.0 * PI; }; +auto rad2d = [](double v) { return v / PI * 180.0; }; + StandardGlobe::StandardGlobe() { } @@ -11,25 +15,47 @@ ECEFPos StandardGlobe::llaToEcef(const LLAPos& v) // 计算距离球心距离 auto axis_r_g = v._alt_m + _radius_m; // ecef-z坐标 - auto axis_z = axis_r_g * sin(v._lat_deg); + auto axis_z = axis_r_g * sin(deg2a(v._lat_deg)); // 纬度圈半径 - auto axis_r_lat = axis_r_g * cos(v._lat_deg); + auto axis_r_lat = axis_r_g * cos(deg2a(v._lat_deg)); // ecef-x - auto axis_x = axis_r_g * cos(v._lon_deg); + auto axis_x = axis_r_lat * cos(deg2a(v._lon_deg)); // ecef-y - auto axis_y = axis_r_g * sin(v._lon_deg); + auto axis_y = axis_r_lat * sin(deg2a(v._lon_deg)); return ECEFPos{ axis_x, axis_y, axis_z }; } -void StandardGlobe::getDistanceWithin(const LLAPos& base, const LLAPos& target, double dist, double azi) +LLAPos StandardGlobe::ecefToLLA(const ECEFPos& v) +{ + auto r2 = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos); + auto rad_lon = acos(v._x_pos / r2); + if(v._y_pos < 0) + rad_lon *= -1; + auto rx = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos + v._z_pos * v._z_pos); + auto rad_lat = asin(v._z_pos / rx); + + LLAPos ret; + ret._lon_deg = rad2d(rad_lon); + ret._lat_deg = rad2d(rad_lat); + ret._alt_m = rx - _radius_m; + + return ret; +} + +void StandardGlobe::getDistanceWithTargetLLA(const LLAPos& base, const LLAPos& target, double& dist, double& azi) { // ecef矢量 auto vec_base = llaToEcef(base).getVec3(); auto vec_target = llaToEcef(target).getVec3(); auto vec_bn = vec_base.normalized(); auto vec_tn = vec_target.normalized(); - + + // 两点之间的矢量夹角 + auto cos_between = QVector3D::dotProduct(vec_bn, vec_tn); + // 两点之间最短球面弧线长度 + dist = acos(cos_between) * _radius_m; + // 目标平面法线矢量 auto vec_vn = QVector3D::crossProduct(vec_bn, vec_tn).normalized(); // 目标平面切线矢量 @@ -37,6 +63,36 @@ void StandardGlobe::getDistanceWithin(const LLAPos& base, const LLAPos& target, // 初始夹角cos值 auto cos_angle = QVector3D::dotProduct(vec_hn, QVector3D(0, 0, 1)); auto angle = acos(cos_angle); + + // 180°校验 + auto flag_vec = QVector3D::crossProduct(vec_hn, QVector3D(0, 0, 1)).normalized(); + if (vec_bn.z() * flag_vec.z() < 0) + angle = 2 * PI - angle; + + // 返回方位角 + azi = rad2d(angle); +} + +#include +void StandardGlobe::getTargetLLAWithDistance(const LLAPos& base, double dist, double azi, LLAPos& target) +{ + auto ecef_bvec = llaToEcef(base).getVec3().normalized(); + QQuaternion u0(-deg2a(azi), ecef_bvec); + + // 获取切线方向矢量 + auto vec_hv = u0.rotatedVector(QVector3D(0, 0, 1)).normalized(); + // 获取法线方向矢量 + auto vec_bv = QVector3D::crossProduct(ecef_bvec, vec_hv).normalized(); + auto rad_between = dist / _radius_m; + + QQuaternion u1(rad_between, vec_bv); + auto ecef_t = u1.rotatedVector(llaToEcef(base).getVec3()); + + ECEFPos sv; + sv._x_pos = ecef_t.x(); + sv._y_pos = ecef_t.y(); + sv._z_pos = ecef_t.z(); + target = ecefToLLA(sv); } QVector3D ECEFPos::getVec3() const diff --git a/StandardGlobe/standardglobe.h b/StandardGlobe/standardglobe.h index abeb083..a08ede2 100644 --- a/StandardGlobe/standardglobe.h +++ b/StandardGlobe/standardglobe.h @@ -55,6 +55,22 @@ public: StandardGlobe(); static ECEFPos llaToEcef(const LLAPos &v); - static void getDistanceWithin(const LLAPos &base, const LLAPos &target, double dist, double azi); + static LLAPos ecefToLLA(const ECEFPos &v); + /// + /// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角，warning：本计算忽视LLA的高度值 + /// + /// + /// + /// + /// + static void getDistanceWithTargetLLA(const LLAPos &base, const LLAPos &target, double &dist, double &azi); + /// + /// 根据两点之间的大地线长度和b->t初始方位角，计算目标LLA定位坐标 + /// + /// + /// + /// + /// + static void getTargetLLAWithDistance(const LLAPos& base, double dist, double azi, LLAPos& target); };