gpschange.cpp

很好的GPS源码

//#include "stdafx.h"
#include "math.h"
//#include "gps.h"

/////////////////////////////////////////////////////////////
//弧度转化为度
//input:
//弧度值
//return:
//度、分、秒（xx.xxxxxx)
//            -- ------
//            度 分 秒
/////////////////////////////////////////////////////////////
double hdhd(double x)
{
	double xx,x1,y;
	double z,pi;
	double out;
	pi=3.1415926535897932;
	xx=x;
	xx=xx*180.0/pi;
	x1=(xx);
	y= ((xx-x1)*60.0);
	z=(xx-x1-y/60.0)*3600.0;
	xx=x1+y/100.0+z/10000.0;
	out=xx;
	return out;
}

/////////////////////////////////////////////////////////////
//度化为弧度
//input:
//度、分、秒（xx.xxxxxx)
//            -- ------
//            度 分 秒
//return:
//弧度
/////////////////////////////////////////////////////////////
double dhhd(double x)
{
	double xx,x1,y;
	double z,pi;
	double out;
	pi=3.1415926535897932;
	xx=x;
	x1=(xx);
	y=((xx-x1)*100.0);
	z=(xx-x1);
	z=z*100.0-y;
	z=z*100.0;
	xx=(x1+y/60.0+z/3600.0)*pi/180.0;
	out=xx;
	return out;
}

/////////////////////////////////////////////////////////////
//GPS度化为弧度
//input:
//度、分（xx xx.xxxx)
//        -- ------
//        度 分 
//return:
//弧度
/////////////////////////////////////////////////////////////
double GPSdhhd(double x)
{
	double xx1,xx2;
	double out;
	double pi;
	pi=3.1415926535897932;
	xx1 = int(x)/100;
	xx2 = x - xx1*100;

	out = (xx1 +xx2/60.) * pi /180.;
	return out;
}

///////////////////////////////////////////////////////////
//WGS84向新北京54坐标系转换
//input:
//WGS84中的纬度、经度(度、分、秒 如23.12569含义是23为度、12为分、569为秒)、大地高
//output:
//新北京54坐标系中的纬度、经度(度、分、秒)、大地高
///////////////////////////////////////////////////////////
void WGS84toNewBJ54(double WGS84B, double WGS84L, double WGSH, double& NewBJ54B, double& NewBJ54L, double& NewBJ54H)
{
	double wd,l,h,rou,n,dl,dh,db,da,df,b,e,mm;
	double dx,dy,dz,ex,ey,ez,m;

	//54坐标系中的a,f
	double a1=6378245.0;
	double f1=(1.0/298.3);
	//84坐标系中的a,f
	double a=6378137.0;
	double f=(1.0/298.257223563);

	dx=-15.415; dy=157.025; dz=94.740;
	ex=0.312; ey=0.080; ez=0.102; m=-1.465e-6;
	rou=3600.0*180.0/3.1415926535;

//	wd=dhhd(WGS84B);
	wd=GPSdhhd(WGS84B);
//	l=dhhd(WGS84L);
	l=GPSdhhd(WGS84L);

	h=WGSH;
	b=a-f*a;
	e=(a*a-b*b)/(a*a);
	da=a1-a;
	df=f1-f;
	n=a/(sqrt(1.0-e*sin(wd)*sin(wd)));
	mm=a*(1-e)/(sqrt(1.0-e*sin(wd)*sin(wd))*sqrt(1.0-e*sin(wd)*sin(wd))*
		sqrt(1.0-e*sin(wd)*sin(wd)));
	dl=rou*(dy*cos(l)-dx*sin(l))/((n+h)*cos(wd))+(1.0-e)*tan(wd)*(cos(l)*
		ex+sin(l)*ey)-ez;
	db=rou*(-dx*sin(wd)*cos(l)-dy*sin(wd)*sin(l)+dz*cos(wd)+f*sin(2.0*wd)
		*(da-a*m)+(1.0-f*cos(2.0*wd))*sin(2.0*wd)*a*df)/(mm+h)-(1.0+e*
		cos(2.0*wd))*((sin(l)*ex-cos(l)*ey));
	dh=dx*cos(wd)*cos(l)+dy*cos(wd)*sin(l)+dz*sin(wd)-(1.0-f*sin(wd)*
	sin(wd))*(da-a*m)+a*(1.0-f*cos(wd)*cos(wd))*sin(wd)*sin(wd)*
		df-a*f*sin(2.0*wd)*(sin(l)*ex-cos(l)*ey)/rou+h*m;
	NewBJ54B=wd+db/rou;
	NewBJ54L=l+dl/rou;
	NewBJ54H=h+dh;
	NewBJ54B=hdhd(wd+db/rou);
	NewBJ54L=hdhd(l+dl/rou);
	NewBJ54H=h+dh;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//功能:正解高斯克吕格投影投影
//参数说明: l0----中央经线；单位：度；
//      lx,bx-----经纬坐标；单位：度、分、秒；
//      x,y-------高斯坐标，x(--->方向)为横假定值、y为纵坐标；单位为：米；
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//void gsdy(double l0,double lx, double bx, double a,double e,double ee, double * x, double * y)

void gsdy(double l0,double lx, double bx, double &x, double &y)
{
	double p,fi,dm,w,an,a1,a2,b1,b2,s,a3,b3;
	double b0,b4,rou;
	double e,ee,a;

/* bx=26.45326664;
 lx=127.44528306;
 l0=129;
*/
	e=0.006693421622966;
	ee=0.006738525414684;
	a=6378245.0;
	p=3.1415926535/180.0;
	fi=dhhd(bx);
	dm=dhhd(lx)-dhhd(l0);
	w=ee*cos(fi)*cos(fi);
	an=a/sqrt(1.0-e*sin(fi)*sin(fi));

	rou=206264.806247096355;
	b0=a*(1.0-e)*(1+(3.0/4.0)*e+(45.0/64.0)*e*e+(175.0/256.0)
		*e*e*e+(11025.0/16384.0)*e*e*e*e+(43659.0/65536.0)
		*e*e*e*e*e+(693693.0/1048576.0)*e*e*e*e*e*e
		+(2760615.0/4194304.0)*e*e*e*e*e*e*e);
	b1=-a*(1.0-e)*((3.0/4.0)*e+(15.0/16.0)*e*e+(525.0/512.0)*
		e*e*e+(2205.0/2048)*e*e*e*e);
	b2=a*(1.0-e)*((15.0/64.0)*e*e+(105.0/256.0)*e*e*e+
		(2205.0/4096.0)*e*e*e*e);
	b3=-a*(1.0-e)*((35.0/512.0)*e*e*e+(315.0/2048.0)*e*e*e*e);
	b4=a*(1.0-e)*((315.0/16384.0)*e*e*e*e);
	s=b0*fi+b1/2*sin(2*fi)+
	b2/4*sin(4*fi)+b3/6*sin(6*fi)+
	b4/8*sin(8*fi);
	a1=an*sin(fi)*cos(fi)/2;
	a2=an*sin(fi)*pow(cos(fi),3)*(5.0-pow(tan(fi),2)+9*w+4*pow(w,2))/24;
	a3=an*sin(fi)*pow(cos(fi),5)*(61-58*pow(tan(fi),2)+pow(tan(fi),4))/720;
	b1=an*cos(fi);
	b2=an*pow(cos(fi),3)*(1.0-tan(fi)*tan(fi)+w)/6.0;
	b3=an*pow(cos(fi),5)*(5.0-18.0*pow(tan(fi),2)+pow(tan(fi),4)+14*w-
		58*w*pow(tan(fi),2))/120.0;
	y=s+a1*pow(dm,2)+a2*pow(dm,4)+a3*pow(dm,6);
	x=b1*dm+b2*pow(dm,3)+b3*pow(dm,5)+500000;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//参数说明: h ---- 中央经线；单位：度；
//WGS84中的纬度、经度(3954.6453)、大地高
//                    XX度、xx.xxxx分
//      x,y-------高斯坐标，x(--->方向)为横假定值、y为纵坐标；单位为：米；
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//	调用示例  GpsToNodeXY(3954.6346, 11625.7019, 0, x, y);
void GpsToNodeXY(double x0, double y0, double h, double &x,double &y)
{
	double xx, yy, zz;

	WGS84toNewBJ54(x0, y0, h,xx, yy, zz);
	gsdy(117, yy, xx, x, y);
	x +=20000000+40;
}