#include "standardglobe.h"
#include <math.h>

double StandardGlobe::_radius_m = 65000000;
StandardGlobe::StandardGlobe()
{
}

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_r_lat = axis_r_g * cos(v._lat_deg);
	// ecef-x
	auto axis_x = axis_r_g * cos(v._lon_deg);
	// ecef-y
	auto axis_y = axis_r_g * sin(v._lon_deg);

	return ECEFPos{ axis_x, axis_y, axis_z };
}

void StandardGlobe::getDistanceWithin(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 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));
}

QVector3D ECEFPos::getVec3() const
{
	return QVector3D(_x_pos, _y_pos, _z_pos);
}