SimsWorld/StandardGlobe/standardglobe.cpp

#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()
{
}

ECEFv3d StandardGlobe::llaToEcef(const LonLatAltPos& v)
{
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
	auto axis_r_g = v._alt_m + _radius_m;
	// ecef-z<><7A><EFBFBD><EFBFBD>
	auto axis_z = axis_r_g * sin(deg2a(v._lat_deg));
	// γ<><CEB3>Ȧ<EFBFBD>뾶
	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 ECEFv3d{ axis_x, axis_y, axis_z };
}

LonLatAltPos StandardGlobe::ecefToLLA(const ECEFv3d& 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ʸ<66><CAB8>
	auto vec_base = llaToEcef(base).getVec3();
	auto vec_target = llaToEcef(target).getVec3();
	auto vec_bn = vec_base.normalized();
	auto vec_tn = vec_target.normalized();

	// <20><><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8><EFBFBD>н<EFBFBD>
	auto cos_between = QVector3D::dotProduct(vec_bn, vec_tn);
	// <20><><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>满<EFBFBD>߳<EFBFBD><DFB3><EFBFBD>
	dist = acos(cos_between) * _radius_m;

	// Ŀ<><C4BF>ƽ<EFBFBD>淨<EFBFBD><E6B7A8>ʸ<EFBFBD><CAB8>
	auto vec_vn = QVector3D::crossProduct(vec_bn, vec_tn).normalized();
	// Ŀ<><C4BF>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
	auto vec_hn = QVector3D::crossProduct(vec_vn, vec_bn).normalized();
	// <20><>ʼ<EFBFBD>н<EFBFBD>cosֵ
	auto cos_angle = QVector3D::dotProduct(vec_hn, QVector3D(0, 0, 1));
	auto angle = acos(cos_angle);

	// 180<38><30>У<EFBFBD><D0A3>
	auto flag_vec = QVector3D::crossProduct(vec_hn, QVector3D(0, 0, 1)).normalized();
	if (vec_bn.z() * flag_vec.z() < 0)
		angle = 2 * PI - angle;

	// <20><><EFBFBD>ط<EFBFBD>λ<EFBFBD><CEBB>
	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);

	// <20><>ȡ<EFBFBD><C8A1><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ʸ<EFBFBD><CAB8>
	auto vec_hv = u0.rotatedVector(QVector3D(0, 0, 1)).normalized();
	// <20><>ȡ<EFBFBD><C8A1><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ʸ<EFBFBD><CAB8>
	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());

	ECEFv3d 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ʸ<66><CAB8>
	auto vec_base = llaToEcef(base).getVec3();
	auto vec_target = llaToEcef(target).getVec3();
	auto vec_bn = vec_base.normalized();
	auto vec_tn = vec_target.normalized();

	// Ŀ<><C4BF>ƽ<EFBFBD>淨<EFBFBD><E6B7A8>ʸ<EFBFBD><CAB8>
	auto vec_vn = QVector3D::crossProduct(vec_bn, vec_tn).normalized();
	// Ŀ<><C4BF>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
	auto vec_hn = QVector3D::crossProduct(vec_vn, vec_bn).normalized();

	// <20><>ʼ<EFBFBD><CABC>λ<EFBFBD><CEBB>cosֵ
	auto cos_angle = QVector3D::dotProduct(vec_hn, QVector3D(0, 0, 1));
	auto angle = acos(cos_angle);

	// 180<38><30>У<EFBFBD><D0A3>
	auto flag_vec = QVector3D::crossProduct(vec_hn, QVector3D(0, 0, 1)).normalized();
	if (vec_bn.z() * flag_vec.z() < 0)
		angle = 2 * PI - angle;

	// <20><><EFBFBD>ط<EFBFBD>λ<EFBFBD><CEBB>
	auto azi_deg = rad2d(angle);

	// <20><><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
	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;
	ECEFv3d tt = ECEFv3d::fromVec3(ecef_target);

	return ecefToLLA(tt);
}

QVector3D ECEFv3d::getVec3() const
{
	return QVector3D(_x_pos, _y_pos, _z_pos);
}
ECEFv3d ECEFv3d::fromVec3(const QVector3D& p) {
	ECEFv3d o;
	o._x_pos = p.x();
	o._y_pos = p.y();
	o._z_pos = p.z();
	return o;
}

LonLatAltPos::LonLatAltPos(const LonLatPos& plain_pos)
	: _lon_deg(plain_pos._lon_deg),
	_lat_deg(plain_pos._lat_deg),
	_alt_m(0) {
}

LonLatAltPos::LonLatAltPos(double lon, double lat, double alt)
	:_lon_deg(lon), _lat_deg(lat), _alt_m(alt) {
}

LonLatPos LonLatAltPos::toLonLatPos() const
{
	return LonLatPos{ _lon_deg, _lat_deg };
}