lla和大地线互转
This commit is contained in:
parent
3177b23825
commit
f2148707b7
|
@ -1,7 +1,11 @@
|
|||
#include "standardglobe.h"
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
|
||||
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,18 +15,35 @@ 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();
|
||||
|
@ -30,6 +51,11 @@ void StandardGlobe::getDistanceWithin(const LLAPos& base, const LLAPos& target,
|
|||
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;
|
||||
|
||||
// 朕炎峠中隈<E4B8AD>文楚
|
||||
auto vec_vn = QVector3D::crossProduct(vec_bn, vec_tn).normalized();
|
||||
// 朕炎峠中俳<E4B8AD>文楚
|
||||
|
@ -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 <qquaternion.h>
|
||||
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
|
||||
|
|
|
@ -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);
|
||||
/// <summary>
|
||||
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角,warning:本计算忽视LLA的高度值
|
||||
/// </summary>
|
||||
/// <param name="base"></param>
|
||||
/// <param name="target"></param>
|
||||
/// <param name="dist"></param>
|
||||
/// <param name="azi"></param>
|
||||
static void getDistanceWithTargetLLA(const LLAPos &base, const LLAPos &target, double &dist, double &azi);
|
||||
/// <summary>
|
||||
/// 根据两点之间的大地线长度和b->t初始方位角,计算目标LLA定位坐标
|
||||
/// </summary>
|
||||
/// <param name="base"></param>
|
||||
/// <param name="dist"></param>
|
||||
/// <param name="azi"></param>
|
||||
/// <param name="target"></param>
|
||||
static void getTargetLLAWithDistance(const LLAPos& base, double dist, double azi, LLAPos& target);
|
||||
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue