func.cpp

根据GPS卫星观测文件（.00O）与星历文件(.00N)

#include "func.h"


void strMid(char* destination,char* source,int pos,int num)//用于拷贝字符
{                                                          //pos为第几列(从0开始)
	int len = strlen(source);
	if(num >(len-pos +1) ) num = len-pos+1;
 	if(len>pos)
	{
 		memcpy(destination,source + pos,num);	
	}
	destination[num] = 0;

}

double Distance(double x1,double y1,double z1,double x2,double y2,double z2)//计算两点之间的距离
{
	return sqrt(pow((x1-x2),2)+pow((y1-y2),2)+pow((z1-z2),2));
}

void XYZ_to_BLH(const double& X,const double& Y,const double& Z,double& B,double& L,double& H)
{//84坐标到 BLH
	////见大地测量学
	double N;
	double a=6378137;
	double e2=0.0066943799013;
	double e12=0.00673949674227;
    double B_rad0=0;
    double B_rad=0;
	B_rad0=atan(Z*(1+e12)/(sqrt(X*X+Y*Y)));
	do{
		 B_rad=B_rad0;
         N=a/sqrt(1-e2*sin(B_rad)*sin(B_rad));
         B_rad0=atan((Z+N*e2*sin(B_rad))/(sqrt(X*X+Y*Y)));
    }while(fabs(B_rad-B_rad0)>0.0000000001);
	double    B_deg=B_rad*180/PI;
	B=B_deg;

	double L_rad=atan(Y/X);
	double L_deg=180+L_rad*180/PI;
    L_rad=L_rad+PI;
    L=L_deg;
	double h=sqrt(X*X+Y*Y)/cos(B_rad)-N;
	H=h;

}


void  BLH_XYH(double& B,double& L,double& X,double& Y)
{//本公式采用自周忠谟《GPS..》P31

	//以下三个为投影时需要的椭球参数
	//////////////
	//bj54_克托球参数
	//double a=6378245.0;
	//double e2=0.00669342162297;      //椭球第一偏心率
	//double e1=sqrt(0.00673852541468);//椭球第二偏心率
	//bj54_克托球参数

	//84椭球 参数
	double a=6378137.0;
	double e2=0.0066943799013;      //椭球第一偏心率
	double e1=sqrt(0.00673949674227);//椭球第二偏心率
	//84椭球 参数
	double L0=114;   //轴子午线的大地经度，为114度/////////////////////////////////////////////可根据实际情况予以修改
    double l=L-L0;
	l=l*PI/180.0;
	B=B*PI/180.0;
	double  A0=1+3.0*e2/4.0+45.0*pow(e2,2)/64.0 +350.0*pow(e2,3)/512.0+11025.0*pow(e2,4)/16384.0;
    double  A2=-0.5*(3.0*e2/4.0+60.0*pow(e2,2)/64.0 +525.0*pow(e2,3)/512.0+17640.0*pow(e2,4)/16384.0);
    double  A4=0.25*(15.0*pow(e2,2)/64.0 +210.0*pow(e2,3)/512.0+8820.0*pow(e2,4)/16384.0);
    double  A6=(-1.0/6.0)*(35.0*pow(e2,3)/512.0+2520.0*pow(e2,4)/16384.0);
    double  A8=0.125*(315.0*pow(e2,4)/16384.0);
	double  X0=a*(1-e2)*(A0*B+A2*sin(2*B)+A4*sin(4*B)+A6*sin(6*B)+A8*sin(8*B));
	double  t=tan(B);
	double  n=e1*cos(B);
	double  N=a/sqrt(1-e2*sin(B)*sin(B));

	double x=X0+0.5*N*t*pow(l*cos(B),2)+(1.0/24.0)*N*t*(5-t*t+9*n*n+4*n*n*n*n)*pow(l*cos(B),4)
             +(1.0/720.0)*N*t*(61-58*t*t+t*t*t*t+270*n*n-330*n*n*t*t)*pow(l*cos(B),6);
	double y=N*cos(B)*l+1.0/6.0*N*(1-t*t+n*n)*pow(l*cos(B),3)+
		1.0/120.0*N*(5-18*t*t+t*t*t*t+14*n*n-58*n*n*t*t)*pow(l*cos(B),5);
    X=x;
	Y=y+500000;
}

/*
double     Elevation(double x1,double x2,double y1,double y2,double z1,double z2)
{   //获取卫星的高度角
	double B1,L1,H1;
	XYZ_to_BLH(x1,y1,z1,B1,L1,H1);
   	B1=B1*PI/180.00;
	L1=L1*PI/180.00;
	double deltaN,deltaE,deltaH;
	XYZ_to_NEU(x2-x1,y2-y1,z2-z1,B1, L1,&deltaN,&deltaE,&deltaH);
	double elevation=atan(deltaH/sqrt(deltaN*deltaN+deltaE*deltaE));
    elevation=elevation*180/PI;
	return(elevation);
}


  */