166 lines
4.6 KiB
C++
166 lines
4.6 KiB
C++
#include "standardglobe.h"
|
|
#include <cmath>
|
|
#include <QVector3D>
|
|
|
|
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()
|
|
{
|
|
}
|
|
|
|
ECEFPos StandardGlobe::llaToEcef(const LonLatAltPos& v)
|
|
{
|
|
// 计算距离球心距离
|
|
auto axis_r_g = v._alt_m + _radius_m;
|
|
// ecef-z坐标
|
|
auto axis_z = axis_r_g * sin(deg2a(v._lat_deg));
|
|
// 纬度圈半径
|
|
auto axis_r_lat = axis_r_g * cos(deg2a(v._lat_deg));
|
|
// ecef-x
|
|
auto axis_x = axis_r_lat * cos(deg2a(v._lon_deg));
|
|
// ecef-y
|
|
auto axis_y = axis_r_lat * sin(deg2a(v._lon_deg));
|
|
|
|
return ECEFPos{ axis_x, axis_y, axis_z };
|
|
}
|
|
|
|
LonLatAltPos 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);
|
|
|
|
LonLatAltPos 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 LonLatAltPos& base, const LonLatAltPos& 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();
|
|
// 目标平面切线矢量
|
|
auto vec_hn = QVector3D::crossProduct(vec_vn, vec_bn).normalized();
|
|
// 初始夹角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 LonLatAltPos& base, double dist, double azi, LonLatAltPos& 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);
|
|
}
|
|
|
|
PolarPos StandardGlobe::getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target)
|
|
{
|
|
// 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 vec_vn = QVector3D::crossProduct(vec_bn, vec_tn).normalized();
|
|
// 目标平面切线矢量
|
|
auto vec_hn = QVector3D::crossProduct(vec_vn, vec_bn).normalized();
|
|
|
|
// 初始方位角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;
|
|
|
|
// 返回方位角
|
|
auto azi_deg = rad2d(angle);
|
|
|
|
// 视线矢量
|
|
auto vec_bt = vec_target - vec_base;
|
|
auto vec_btn = vec_bt.normalized();
|
|
auto cos_pitch = QVector3D::dotProduct(vec_hn, vec_btn);
|
|
auto pit_deg = rad2d(acos(cos_pitch));
|
|
|
|
PolarPos p;
|
|
p._azimuth_deg = azi_deg;
|
|
p._pitch_deg = pit_deg;
|
|
p._dist_m = vec_bt.length();
|
|
return p;
|
|
}
|
|
|
|
LonLatAltPos StandardGlobe::getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target)
|
|
{
|
|
auto ecef_b = llaToEcef(base).getVec3();
|
|
auto u_azi = QQuaternion(-deg2a(target._azimuth_deg), ecef_b.normalized());
|
|
|
|
auto vec_ts = u_azi.rotatedVector(QVector3D(0,0,1)).normalized();
|
|
auto axis_pitch = QVector3D::crossProduct(vec_ts, ecef_b.normalized());
|
|
auto u_pit = QQuaternion(deg2a(target._pitch_deg), axis_pitch);
|
|
|
|
auto r_shadow = target._dist_m * cos(deg2a(target._pitch_deg));
|
|
auto ts_appv = u_pit.rotatedVector(vec_ts).normalized() * r_shadow;
|
|
|
|
auto ecef_target = ecef_b + ts_appv;
|
|
ECEFPos tt = ECEFPos::fromVec3(ecef_target);
|
|
|
|
return ecefToLLA(tt);
|
|
}
|
|
|
|
QVector3D ECEFPos::getVec3() const
|
|
{
|
|
return QVector3D(_x_pos, _y_pos, _z_pos);
|
|
}
|
|
ECEFPos ECEFPos::fromVec3(const QVector3D& p) {
|
|
ECEFPos o;
|
|
o._x_pos = p.x();
|
|
o._y_pos = p.y();
|
|
o._z_pos = p.z();
|
|
return o;
|
|
}
|