空间直角坐标转大地坐标

将空间直角坐标XYZ转化为大地坐标BLH

#define PI acos(-1)
vector<double> convertXYZToLatLongHeight(vector<double> coor) 
{
	 double X = coor[0];
	 double Y = coor[1];
	 double Z = coor[2];
	 double _radiusEquator = 6378137;
	 double _radiusPolar = 6356752.3142;
	 double _eccentricitySquared = (_radiusEquator * _radiusEquator - _radiusPolar * _radiusPolar) / (_radiusEquator * _radiusEquator);
	 double p = sqrt(X * X + Y * Y);
	 double theta = atan2(Z * _radiusEquator, (p * _radiusPolar));
	 double eDashSquared = (_radiusEquator * _radiusEquator - _radiusPolar * _radiusPolar) /
		 (_radiusPolar * _radiusPolar);

	 double sin_theta = sin(theta);
	 double cos_theta = cos(theta);

	 double latitude = atan((Z + eDashSquared * _radiusPolar * sin_theta * sin_theta * sin_theta) /
		 (p - _eccentricitySquared * _radiusEquator * cos_theta * cos_theta * cos_theta));
	 double longitude = atan2(Y, X);

	 double sin_latitude = sin(latitude);
	 double N = _radiusEquator / sqrt(1.0 - _eccentricitySquared * sin_latitude * sin_latitude);

	 double height = p / cos(latitude) - N;
	 vector<double> BLH;
	 BLH.push_back(longitude / PI * 180);
	 BLH.push_back(latitude / PI * 180);
	 BLH.push_back(height);
	 return BLH;
}