gpsgaussconvert.cpp

GPS 读取数据处理模块源程序

#include "GpsGaussConvert.h"
#include "stdafx.h"

#define Pi 3.1415926535897932384626433832795028841971693993751058209749445923078164 
const double awgs = 6378137.0;         // WGS84 Semi-Major Axis = Equatorial Radius in meters 
const double bwgs = 6356752.314;      // WGS84 Semi-Minor Axis = Polar Radius in meters 
const double abes = 6377397.155;       // Bessel Semi-Major Axis = Equatorial Radius in meters 
const double bbes = 6356078.962;       // Bessel Semi-Minor Axis = Polar Radius in meters 
const double cbes = 111120.6196;       // Bessel latitude to Gauss-Krueger meters 
const double dx   = -585.7;                // Translation Parameter 1 
const double dy   = -87.0;                  // Translation Parameter 2 
const double dz   = -409.2;                // Translation Parameter 3 
const double rotx = 2.540423689E-6;   // Rotation Parameter 1 
const double roty = 7.514612057E-7;   // Rotation Parameter 2 
const double rotz = -1.368144208E-5;  // Rotation Parameter 3 
// const double sc   = 1/0.99999122; // Scaling Factor wrong! 
// Maik St?ckmann reported this error on Nov 12th 2002. Thank you, Maik!
const double sc = 0.99999122;           // Scaling Factor 

double l2; 
double b2; 
double h2; 

double R; 
double H; 
double a; 
double b; 
double eq; 
double N; 
double Xq; 
double Yq; 
double Zq; 
double X; 
double Y; 
double Z; 
// For performance measurement 
static LARGE_INTEGER pm_freq; 
static LARGE_INTEGER pm_start; 
LARGE_INTEGER pm_end; 
double pm_executiontime; 

// Prototypes 
void HelmertTransformation(double,double,double,double&,double&,double&); 
void BesselBLnachGaussKrueger(double,double,double&,double&); 
void BLRauenberg (double,double,double,double&,double&,double&); 
double neuF(double,double,double,double); 
double round(double); 

void GaussConvert(double dwLongitude, double dwLatitude, double dwHeight, double &dwX, double &dwY)
{

  dwLongitude=Pi*dwLongitude/180; 
  dwLatitude=Pi*dwLatitude/180; 
  a=awgs; 
  b=bwgs; 
  eq=(a*a-b*b)/(a*a); 
  N=a/sqrt(1-eq*sin(dwLatitude)*sin(dwLatitude)); 

  Xq=(N+dwHeight)*cos(dwLatitude)*cos(dwLongitude); 
  Yq=(N+dwHeight)*cos(dwLatitude)*sin(dwLongitude); 
  Zq=((1-eq)*N+dwHeight)*sin(dwLatitude); 

  HelmertTransformation(Xq,Yq,Zq,X,Y,Z); 
  a=abes; 
  b=bbes; 
  eq=(a*a-b*b)/(a*a); 
  BLRauenberg(X,Y,Z,b2,l2,h2); 
  BesselBLnachGaussKrueger(b2,l2,R,H); 
  b2=b2*180/Pi; 
  l2=l2*180/Pi; 

  dwX = H ;
  dwY = R ;  	

  return ; 

}

void HelmertTransformation(double x,double y,double z,double& xo,double& yo,double& zo) 
{ 
  xo=dx+(sc*(1*x+rotz*y-roty*z)); 
  yo=dy+(sc*(-rotz*x+1*y+rotx*z)); 
  zo=dz+(sc*(roty*x-rotx*y+1*z)); 
} 
void BesselBLnachGaussKrueger(double b,double ll,double& Re,double& Ho) 
{ 
  double l; 
  double l0; 
  double bg; 
  double lng; 
  double Ng; 
  double k; 
  double t; 
  double eq; 
  double Vq; 
  double v; 
  double nk; 
  double X; 
  double gg; 
  double SS; 
  double Y; 
  double kk; 
  double Pii; 
  double RVV; 
  bg=180*b/Pi; 
  lng=180*ll/Pi; 
  l0=3*round((180*ll/Pi)/3); 
  l0=Pi*l0/180; 
  l=ll-l0; 
  k=cos(b); 
  t=sin(b)/k; 
  eq=(abes*abes-bbes*bbes)/(bbes*bbes); 
  Vq=1+eq*k*k; 
  v=sqrt(Vq); 
  Ng=abes*abes/(bbes*v); 
  nk=(abes-bbes)/(abes+bbes); 
  X=((Ng*t*k*k*l*l)/2)+((Ng*t*(9*Vq-t*t-4)*k*k*k*k*l*l*l*l)/24); 
  gg=b+(((-3*nk/2)+(9*nk*nk*nk/16))*sin(2*b)+15*nk*nk*sin(4*b)/16-35*nk*nk*nk*sin(6*b)/48); 
  SS=gg*180*cbes/Pi; 
  Ho=(SS+X); 
  Y=Ng*k*l+Ng*(Vq-t*t)*k*k*k*l*l*l/6+Ng*(5-18*t*t+t*t*t*t)*k*k*k*k*k*l*l*l*l*l/120; 
  kk=500000; 
  Pii=Pi; 
  RVV=round((180*ll/Pii)/3); 
  Re=RVV*1000000+kk+Y; 
} 
void BLRauenberg (double x,double y,double z,double& b,double& l,double& h) 
{ 
  double f; 
  double f1; 
  double f2; 
  double ft; 
  double p; 
  f=Pi*50/180; 
  p=Z/sqrt(x*x+y*y); 
  do 
  { 
    f1=neuF(f,x,y,p); 
    f2=f; 
    f=f1; 
    ft=180*f1/Pi; 
  } 
  while(!(fabs(f2-f1)<10E-10)); 
  b=f; 
  l=atan(y/x); 
  h=sqrt(x*x+y*y)/cos(f1)-a/sqrt(1-eq*sin(f1)*sin(f1)); 
} 
double neuF(double f,double x,double y,double p) 
{ 
  double zw; 
  double nnq; 
  zw=a/sqrt(1-eq*sin(f)*sin(f)); 
  nnq=1-eq*zw/(sqrt(x*x+y*y)/cos(f)); 
  return(atan(p/nnq)); 
} 
double round(double src) 
{ 
  double theInteger; 
  double theFraction; 
  double criterion = 0.5; 
  theFraction = modf(src,&theInteger); 
  if (!(theFraction < criterion)) 
  { 
    theInteger += 1; 
  } 
  return theInteger; 
}