【GNSS】【C++】【坐标转换】 XYZ 转换为NEU

版权声明：本文为博主原创文章，未经博主允许不得转载。保留追究法律责任，转载联系博主。交流欢迎加QQ群463339221。 https://blog.csdn.net/m0_37362454/article/details/83574768

/******************************************************                                       */
/************************************************************************/
#include<iostream>
#include<fstream>
#include<string>
#include <cmath>


using namespace  std;

#define M_PI 3.1415926



//笛卡尔坐标系结构体
typedef struct tagCRDCARTESIAN
{
	double x;
	double y;
	double z;
}CRDCARTESIAN;
typedef CRDCARTESIAN *PCRDCARTESIAN;

//大地坐标系结构体
typedef struct tagCRDGEODETIC
{
	double longitude;
	double latitude;
	double height;
}CRDGEODETIC;

typedef CRDGEODETIC *PCRDGEODETIC;

//站心地平坐标（线坐标形式）结构体
typedef struct tagCRDTOPOCENTRIC
{
	double northing;
	double easting;
	double upping;
}CRDTOPOCENTRIC;

typedef CRDTOPOCENTRIC *PCRDTOPOCENTRIC;

//站心地平坐标（极坐标形式）结构体
typedef struct tagCRDTOPOCENTRICPOLAR
{
	double range;
	double azimuth;
	double elevation;
}CRDTOPOCENTRICPOLAR;

typedef CRDTOPOCENTRICPOLAR *PCRDTOPOCENTRICPOALR;






//度分秒转换为弧度函数（1.2345表示1°23'45''）
void DMS_RAD(double DMS,double *Rad);

//弧度转换为度分秒
void RAD_DMS(double Rad,double *DMS);

//笛卡尔坐标系转换为大地坐标系
//pcc为笛卡尔坐标对象指针，pcg为大地坐标系结构体对象指针，dSemiMajorAxis为椭球长半轴，dFlattening为椭球扁率
bool CARTESIANTOGEODETIC(PCRDCARTESIAN pcc,PCRDGEODETIC pcg,double dSemiMajorAxis,double dFlattening);

//大地坐标系转换为笛卡尔坐标系
//pcg为大地坐标系结构体对象指针，pcc为笛卡尔坐标对象指针，dSemiMajorAxis为椭球长半轴，dFlattening为椭球扁率
bool GEODETICTOCARTESIAN(PCRDGEODETIC pcg,PCRDCARTESIAN pcc,double dSemiMajorAxis,double dFlattening);

//笛卡尔坐标系转换站心地平坐标系
//pcc指向待转换的笛卡尔坐标的指针，pccCenter指向站心的笛卡尔坐标的指针，pct指向所转换出的站心地平坐标的指针，
//dSemiMajorAxis为椭球长半轴，dFlattening为椭球扁率
bool CARTESIANTOTOPOCENTRIC(PCRDCARTESIAN pcc,PCRDCARTESIAN pccCenter,PCRDTOPOCENTRIC pct,double dSemiMajorAxis,double dFlattening);

//站心地平坐标系转换为站心地平极坐标系
//pct指向待转换的站心地平坐标的指针,pctp指向所转换出的站心地平极坐标的指针
bool TOPOCENTRICTOTOPOCENTRICPOLAR(PCRDTOPOCENTRIC pct,PCRDTOPOCENTRICPOALR pctp);




void DMS_RAD(double DMS,double *Rad)
{
	int Deg,Min;
	double Sec;
	Deg=(int)DMS;
	Min=(int)((DMS-Deg)*100);
	Sec=((DMS-Deg)*100-Min)*100;
	*Rad=(Deg+Min/60.0+Sec/3600.0)/180.0*M_PI;
	return;
}

void RAD_DMS(double Rad,double *DMS)
{
	int Deg,Min;
	double Sec;
	double AR,AM;
	AR=Rad;
	if (Rad<0)
		AR=-Rad;
	AR=AR+1.0e-10;
	AR=AR*180.0/M_PI;
	Deg=(int)AR;
	AM=(AR-Deg)*60.0;
	Min=(int)AM;
	Sec=(AM-Min)*60;
	*DMS=Deg+Min/100.0+Sec/10000.0;
	if(Rad<0)
		*DMS=-*DMS;
	
	return;
}

bool CARTESIANTOGEODETIC(PCRDCARTESIAN pcc,PCRDGEODETIC pcg,double dSemiMajorAxis,double dFlattening)
{
	double B0,R,N;
	double B_,L_;
	double X=pcc->x;
	double Y=pcc->y;
	double Z=pcc->z;

	R=sqrt(X*X+Y*Y);
	B0=atan2(Z,R);
	
	while (1)
	{
		N=dSemiMajorAxis/sqrt(1.0-dFlattening*(2-dFlattening)*sin(B0)*sin(B0));
		B_=atan2(Z+N*dFlattening*(2-dFlattening)*sin(B0),R);
		if(fabs(B_-B0)<1.0e-10)
			break;
		B0=B_;
	}
	L_=atan2(Y,X);
	pcg->height=R/cos(B_)-N;
	
	RAD_DMS(B_,&pcg->latitude);
	RAD_DMS(L_,&pcg->longitude);
	return true;
}

bool GEODETICTOCARTESIAN(PCRDGEODETIC pcg,PCRDCARTESIAN pcc,double dSemiMajorAxis,double dFlattening)
{
	double N;
	double B_,L_;
	
	double B=pcg->latitude;
	double L=pcg->longitude;
	double H=pcg->height;

	DMS_RAD(B,&B_);
	DMS_RAD(L,&L_);
	N=dSemiMajorAxis/sqrt(1.0-dFlattening*(2-dFlattening)*sin(B_)*sin(B_));
	pcc->x=(N+H)*cos(B_)*cos(L_);
	pcc->y=(N+H)*cos(B_)*sin(L_);
	pcc->z=(N*(1.0-dFlattening*(2-dFlattening))+H)*sin(B_);
	return true;
}

bool CARTESIANTOTOPOCENTRIC(PCRDCARTESIAN pcc,PCRDCARTESIAN pccCenter,PCRDTOPOCENTRIC pct,double dSemiMajorAxis,double dFlattening)
{
	double dX,dY,dZ;
	dX=pcc->x-pccCenter->x;
	dY=pcc->y-pccCenter->y;
	dZ=pcc->z-pccCenter->z;

	CRDGEODETIC Geodetic;
	CARTESIANTOGEODETIC(pccCenter,&Geodetic,dSemiMajorAxis,dFlattening);

	double B,L,H;
	DMS_RAD(Geodetic.latitude,&B);
	DMS_RAD(Geodetic.longitude,&L);
	DMS_RAD(Geodetic.height,&H);

	pct->northing=-sin(B)*cos(L)*dX-sin(B)*sin(L)*dY+cos(B)*dZ;
	pct->easting=-sin(L)*dX+cos(L)*dY;
	pct->upping=cos(B)*cos(L)*dX+cos(B)*sin(L)*dY+sin(B)*dZ;

	return true;
}

bool TOPOCENTRICTOTOPOCENTRICPOLAR(PCRDTOPOCENTRIC pct,PCRDTOPOCENTRICPOALR pctp)
{
	double S,A,E;
	S=sqrt(pct->northing*pct->northing+pct->easting*pct->easting+pct->upping*pct->upping);
	A=atan2(pct->easting,pct->northing);
	E=asin(pct->upping/S);

	pctp->range=S;
	RAD_DMS(A,&pctp->azimuth);
	RAD_DMS(E,&pctp->elevation);

	return true;
}



void main()
{
	
	double dSemiMajorAxis=6378136.6;
	double dFlattening=1/298.25642;

PCRDCARTESIAN pcc=new CRDCARTESIAN;
PCRDCARTESIAN pccCenter=new CRDCARTESIAN;
PCRDTOPOCENTRIC pct=new CRDTOPOCENTRIC;

pccCenter->x=4027893.6684 ;
pccCenter->y= 307045.9239;
pccCenter->z= 4919475.1809;


char *p;
char filename[20]="shuru.txt";
	p=filename;

string str,getstr;
fstream infile(p,ios::in);
if(!infile)
{ 
	cerr<<"N file open error!"<<endl;
	exit(1);
}
fstream outfile("result.txt",ios::out);
getline(infile,str);
//cout<<str<<endl;
while(!infile.eof())
{	
	
	
	getstr=str.substr(0,12);
	pcc->x=atof(getstr.c_str());
	getstr=str.substr(13,24);
	pcc->y=atof(getstr.c_str());
	getstr=str.substr(25,39);
	pcc->z=atof(getstr.c_str());

//	cout<<std::fixed<<pcc->x<<" "<<pcc->y<<"    "<<pcc->z<<endl;

	
 CARTESIANTOTOPOCENTRIC( pcc, pccCenter,pct, dSemiMajorAxis,dFlattening);


	
	outfile<<pct->northing<<"   "<<pct->easting<<"     "<<pct->upping<<endl;
	
	getline(infile,str);


}
	infile.close();

//cout<<pct->northing<<"   "<<pct->easting<<"     "<<pct->upping<<endl;


}