计算VOR方位角的算法

#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 ;

}