北京54坐标和经纬度坐标转换算法c++

//坐标正算
lbxy(double l, double b, double *x, double *y, int l0)
{
 double sa,sb,sep,sn,sy2,st,sm,sx,hb;
 double xx,yy,hd,sd;

 //判断值的范围
 if (l > 360 || l < 0 || b > 360 || b < 0)
 {
  *x = l;
  *y = b;
  return;
 }

 l  = l-l0;
 sa = 6378245;   
 sb = 6356863.019;
 sep= 0.006738525415;
 hd = b*PI;
 hb = hd/180.0;
 st = tan(hb);
       
 sn=pow(sa,(double)2)/sqrt(pow(sa,(double)2)*pow(cos(hb),(double)2)
  +pow(sb,(double)2)*pow(sin(hb),(double)2));

 sy2=sep*pow(cos(hb),(double)2);
 sd = cos(hb)*l*PI;
 sm = sd/180.0;
 sx = 111134.861*b-(32005.78*sin(hb)+133.924*pow(sin(hb),(double)3)+0.697*pow(sin(hb),(double)5))*cos(hb);
 xx = sx+sn*st*(0.5*pow(sm,(double)2)+1.0/24.0*(5.0-pow(st,(double)2)+9.0*sy2)*pow(sm,(double)4));
 yy = sn*(sm+1.0/6.0*(1.0-pow(st,(double)2)+sy2)*pow(sm,(double)3)+1.0/120.0*(5.0-18.0*pow(st,(double)2)+pow(st,(double)4))*pow(sm,(double)5));
 *x = xx;
 *y = yy+500000;
}

//坐标反算
xylb(double l0, double x, double y, double *l, double *b)
{
 double bf,vf,nf,ynf,tf,yf2,hbf;
 double sa,sb,se2,sep2,mf;
 double w1,w2,w,w3,w4;
 double pi = 3.1415926;

 x = x/1000000.0;
 y = y - 500000.0;
 
 bf = 9.04353692458*x-0.00001007623*pow(x,2.0)-0.00074438304*pow(x,3.0)-0.00000463064*pow(x,4.0)+0.00000505846*pow(x,5.0)-0.00000016754*pow(x,6.0);
 hbf = bf * pi/ 180.0;
 sa = 6378245.0;
 sb = 6356863.019;
 se2 = 0.006693421623;
 sep2 = 0.006738525415;

 w1 = sin(hbf);
 w2 = 1.0 - se2 * pow(w1,(double)2);
 w = sqrt(w2);
 mf = sa*(1.0-se2)/pow(w,(double)3);
 w3 = cos(hbf);

 w4 = pow(sa,(double)2)*pow(w3,(double)2) + pow(sb,(double)2)*pow(w1,(double)2);
 nf = pow(sa,(double)2) / sqrt(w4);

 ynf = y/nf;
 vf = nf/mf;
 tf = tan(hbf);
 
 yf2 = sep2 * pow(w3, (double)2);

 *b = bf - 1.0/2.0 * vf * tf * (pow(ynf,(double)2)-1.0/12.0*(5.0+3.0*pow(tf,(double)2)+yf2-9.0*yf2*pow(tf,(double)2))*pow(ynf,(double)4))*180.0/pi;
 *l = 1.0/w3*ynf*(1.0-1.0/6.0*(1.0+2.0*pow(tf,(double)2)+yf2)*pow(ynf,(double)2)+1.0/120.0*(5.0+28.0*pow(tf,(double)2)+24.0*pow(tf,(double)2)+6.0*yf2+8.0*yf2*pow(tf,(double)2))*pow(ynf,(double)4))*180.0/pi;
 *l = l0 + *l;
}