坐标转换源代码，C++/C， 极为精确地大地坐标系转地心坐标系，地心坐标系转站心坐标系

摘自http://blog.sina.com.cn/s/blog_4e426d410101ahlt.html

但是我用OSG进行转换的结果和用这个文章中的方法转换进行的对比，不明白为什么他转出的结果是OSG的正好两倍。

//WGS84地理坐标系参变量

struct CRDGEODETIC

{

double latitude;

double longitude;

double altitude;

};

//空间笛卡尔坐标系坐标点

struct CRDCARTESIAN

{

double x;

double y;

double z;

};

//最精确的坐标转换办法 ,空间大地坐标系向空间直角坐标系的转换

CRDCARTESIAN Coordcovert::BLH_to_XYZ (CRDGEODETIC pos_BLH)//大地--->球心

{

double a=6378137;//a为椭球的长半轴:a=6378.137km

double b=6356752.3141;//b为椭球的短半轴:a=6356.7523141km

double H=pos_BLH.altitude+a;

double e=sqrt(1-pow(b ,2)/pow(a ,2)); //e为椭球的第一偏心率

// double e=sqrt(0.006693421622966); //克拉索夫斯基椭球

// double e=sqrt(0.006694384999588); //1975年国际椭球

// double e=sqrt(0.0066943799013); //WGS-84椭球

CRDCARTESIAN pos_XYZ;

double m=M_PI/180;//经度维度需要转换成弧度.

double B=pos_BLH.latitude*m;

double L=pos_BLH.longitude*m;

double W=sqrt(1-pow(e ,2)*pow(sin(B) ,2));

double N=a/W; //N为椭球的卯酉圈曲率半径

pos_XYZ.x=(N+H)*cos(B)*cos(L);

pos_XYZ.y=(N+H)*cos(B)*sin(L);

pos_XYZ.z=(N*(1-pow(e ,2))+H)*sin(B);

return pos_XYZ;

}

CRDGEODETIC Coordcovert::XYZ_to_BHL (CRDCARTESIAN pos_XYZ)//球心--->大地

{

double v0=pos_XYZ.z/sqrt(pow(pos_XYZ.x ,2)+pow(pos_XYZ.y ,2));

double a=6378137;//a为椭球的长半轴:a=6378.137km

double b=6356752;

double e=sqrt(1-pow(b ,2)/pow(a ,2)); //e为椭球的第一偏心率

// double e=sqrt(0.006693421622966); //克拉索夫斯基椭球

// double e=sqrt(0.006694384999588); //1975年国际椭球

// double e=sqrt(0.0066943799013); //WGS-84椭球

// double W=sqrt(1-pow(e ,2)*pow(sin(B) ,2));

double N=0 ; //N为椭球的卯酉圈曲率半径

double B1=atan(v0) ,B2=0;

double H=0;

while(qAbs(B2-B1)>1E-5)

{

N = a/sqrt(1-pow(e ,2)*pow(sin(B1) ,2));

H = pos_XYZ.z/sin(B1)-N*(1-pow(e ,2));

B2 = atan(pos_XYZ.z*(N+H)/sqrt((pow(pos_XYZ.x ,2)+pow(pos_XYZ.y ,2))*(N*(1-pow(e,2))+H)));

B1=B2;

}

double m=M_PI/180;

CRDGEODETIC pos_BLH;

pos_BLH.latitude=B1/m;

pos_BLH.longitude=atan(pos_XYZ.y/pos_XYZ.x)/m;

pos_BLH.altitude=H-a;

return pos_BLH;

}

CRDCARTESIAN Coordcovert::XYZ_to_xyz (CRDCARTESIAN pos_XYZ , CRDGEODETIC Center)//球心--->站心

{

CRDCARTESIAN pos_xyz ,tmp_XYZ;

CRDCARTESIAN Center_XYZ = BLH_to_XYZ (Center);

tmp_XYZ.x = pos_XYZ.x-Center_XYZ.x;

tmp_XYZ.y = pos_XYZ.y-Center_XYZ.y;

tmp_XYZ.z = pos_XYZ.z-Center_XYZ.z;

double m=M_PI/180;

pos_xyz.x = -sin(Center.latitude*m)*cos(Center.longitude*m)*tmp_XYZ.x-sin(Center.latitude*m)*sin(Center.longitude*m)*tmp_XYZ.y

+ cos(Center.latitude*m)*tmp_XYZ.z;

pos_xyz.y = -sin(Center.longitude*m)*tmp_XYZ.x+cos(Center.longitude*m)*tmp_XYZ.y;

pos_xyz.z =cos(Center.latitude*m)*cos(Center.longitude*m)*tmp_XYZ.x+cos(Center.latitude*m)*sin(Center.longitude*m)*tmp_XYZ.y

+ sin(Center.latitude*m)*tmp_XYZ.z - a;

return pos_xyz;

}

CRDCARTESIAN Coordcovert::xyz_to_XYZ (CRDCARTESIAN pos_xyz , CRDGEODETIC Center)//站心--->球心

{

double a=6378137;//a为椭球的长半轴:a=6378.137km

double b=6356752.3141;//b为椭球的短半轴:a=6356.7523141km

double H0=Center.altitude+a;

double e=sqrt(1-pow(b ,2)/pow(a ,2)); //e为椭球的第一偏心率

// double e=sqrt(0.006693421622966); //克拉索夫斯基椭球

// double e=sqrt(0.006694384999588); //1975年国际椭球

// double e=sqrt(0.0066943799013); //WGS-84椭球

double m=M_PI/180;//经度维度需要转换成弧度.

double B0=Center.latitude*m;

double L0=Center.longitude*m;

double W=sqrt(1-pow(e ,2)*pow(sin(B0) ,2));

double N0=a/W; //N为椭球的卯酉圈曲率半径

CRDCARTESIAN pos_XYZ;

pos_XYZ.x = (N0+H0)*cos(B0)*cos(L0)

-sin(B0)*cos(L0)*pos_xyz.x - sin(L0)*pos_xyz.y + cos(B0)*cos(L0)*pos_xyz.z;

pos_XYZ.y = (N0+H0)*cos(B0)*sin(L0)

-sin(B0)*sin(L0)*pos_xyz.x - cos(L0)*pos_xyz.y + cos(B0)*sin(L0)*pos_xyz.z;

pos_XYZ.z = (N0*(1-pow(e ,2))+H0)*sin(B0)

-cos(B0)*pos_xyz.x + sin(B0)*pos_xyz.z;

return pos_XYZ;

}