#include "stdafx.h"
#include "CalcRadioDistance.h"
#include "Eigen/Dense"
#include "geomag.h"
using namespace Eigen;
using namespace Eigen::internal;
using namespace Eigen::Architecture;
typedef std::string String;
typedef Matrix<double,3, 3> _Matrix_Rz,_Matrix_Rx;
typedef Matrix<double,1,3> /
_Matrix_North,_DMEVOR_Pos_WGS84,_Plane_Pos_WGS84,_Plane_To_DMEVOR_WGS84,_MatrixA;
typedef Matrix<double,3, 1> _Matrix_Plane_To_DMEVOR_NAV,_Matrix_Plane_To_DMEVOR_NAV_Proj;
#define earth_radius 6378137 // m
#define f (1/298.257223563)
#define e2 (f*(2-f))
#define PI 3.1415926
RADIO_DIS_API double CalcRadioDistance(LocalPosition ap,LocalPosition radio)
{
double N1 = earth_radius/sqrt((1-e2*sin(ap.dLat*PI/180.0)));
double x_ap = (N1+ap.dAlt)*cos(ap.dLat*PI/180.0)*cos(ap.dLon*PI/180.0);
double y_ap = (N1+ap.dAlt)*cos(ap.dLat*PI/180.0)*sin(ap.dLon*PI/180.0);
double z_ap = (N1*(1-e2)+ap.dAlt)*sin(ap.dLat*PI/180.0);
double N2 = earth_radius/sqrt((1-e2*sin(radio.dLat*PI/180.0)));
double x_radio = (N2+radio.dAlt)*cos(radio.dLat*PI/180.0)*cos(radio.dLon*PI/180.0);
double y_radio = (N2+radio.dAlt)*cos(radio.dLat*PI/180.0)*sin(radio.dLon*PI/180.0);
double z_radio = (N2*(1-e2)+radio.dAlt)*sin(radio.dLat*PI/180.0);
double h_dis= pow(abs(x_ap-x_radio),2)+pow(abs(y_ap-y_radio),2) +pow(abs(z_ap-z_radio),2);
return sqrt(h_dis);
}
RADIO_DIS_API double CalcAngle_VOR(LocalPosition vor_pos,LocalPosition plane_pos)
{
// 将经纬度转换成WGS84下的坐标系
double N1 = earth_radius/sqrt((1-e2*sin(vor_pos.dLat*PI/180.0)));
double x_vor = (N1+vor_pos.dAlt)*cos(vor_pos.dLat*PI/180.0)*cos(vor_pos.dLon*PI/180.0);
double y_vor = (N1+vor_pos.dAlt)*cos(vor_pos.dLat*PI/180.0)*sin(vor_pos.dLon*PI/180.0);
double z_vor = (N1*(1-e2)+vor_pos.dAlt)*sin(vor_pos.dLat*PI/180.0);
double N2 = earth_radius/sqrt((1-e2*sin(plane_pos.dLat*PI/180.0)));
double x_plane = (N2+plane_pos.dAlt)*cos(plane_pos.dLat*PI/180.0)*cos(plane_pos.dLon*PI/180.0);
double y_plane = (N2+plane_pos.dAlt)*cos(plane_pos.dLat*PI/180.0)*sin(plane_pos.dLon*PI/180.0);
double z_plane = (N2*(1-e2)+plane_pos.dAlt)*sin(plane_pos.dLat*PI/180.0);
_Matrix_North North;
North(0,0) = 0;
North(0,1) = 1;
North(0,2) = 0;
_DMEVOR_Pos_WGS84 DMEVOR_Pos_WGS84;
DMEVOR_Pos_WGS84(0,0) = x_vor;
DMEVOR_Pos_WGS84(0,1) = y_vor;
DMEVOR_Pos_WGS84(0,2) = z_vor;
_Plane_Pos_WGS84 Plane_Pos_WGS84;
Plane_Pos_WGS84(0,0) = x_plane;
Plane_Pos_WGS84(0,1) = y_plane;
Plane_Pos_WGS84(0,2) = z_plane;
_Plane_To_DMEVOR_WGS84 Plane_To_DMEVOR_WGS84 = DMEVOR_Pos_WGS84-Plane_Pos_WGS84;
_Matrix_Rz Rz;
Rz(0,0)=cos((plane_pos.dLon+90)*PI/180.0);
Rz(0,1) = sin((plane_pos.dLon+90)*PI/180.0);
Rz(0,2) = 0;
Rz(1,0)=-sin((plane_pos.dLon+90)*PI/180.0);
Rz(1,1) = cos((plane_pos.dLon+90)*PI/180);
Rz(1,2) = 0;
Rz(2,0)= 0;
Rz(2,1) = 0;
Rz(2,2) = 1;
_Matrix_Rx Rx;
Rx(0,0) = 1;
Rx(0,1) = 0;
Rx(0,2) = 0;
Rx(1,0) = 0;
Rx(1,1) = cos((90-plane_pos.dLat)*PI/180.0);
Rx(1,2) = sin((90-plane_pos.dLat)*PI/180.0);
Rx(2,0) = 0;
Rx(2,1) = -sin((90-plane_pos.dLat)*PI/180.0);
Rx(2,2) = cos((90-plane_pos.dLat)*PI/180.0);
_Matrix_Plane_To_DMEVOR_NAV Matrix_Plane_To_DMEVOR_NAV = Rx*Rz*Plane_To_DMEVOR_WGS84.transpose();
_MatrixA MatrixA;
MatrixA(0,0) = 0;
MatrixA(0,1) = 0;
MatrixA(0,2) = 1;
_Matrix_Plane_To_DMEVOR_NAV_Proj Matrix_Plane_To_DMEVOR_NAV_Proj = Matrix_Plane_To_DMEVOR_NAV - MatrixA*Matrix_Plane_To_DMEVOR_NAV*MatrixA.transpose();
double Angle_of_Plane_DMEVOR = acos(Matrix_Plane_To_DMEVOR_NAV_Proj.dot(North)/Matrix_Plane_To_DMEVOR_NAV_Proj.norm());
if (Matrix_Plane_To_DMEVOR_NAV(0,0) > 0)
Angle_of_Plane_DMEVOR = Angle_of_Plane_DMEVOR*180/PI;
else
Angle_of_Plane_DMEVOR = 360 - Angle_of_Plane_DMEVOR*180/PI;
return Angle_of_Plane_DMEVOR ;
}