大地坐标系到站心坐标系（ENU）坐标转换（提供QT和JAVA源码）

在做两个坐标系转换的时候，谷歌好长时间C++相关的代码资料，查找到两个转换的代码，但是结果都不对，故记录本次的坐标转换。

测试结果：

 QT源码：

WGS84ToENU.h

#ifndef WGS84TOENU_H
#define WGS84TOENU_H

#include <QMainWindow>
#include<QDateTime>

class WGS84ToENU
{
public:
    WGS84ToENU();
    double* wgs84ToEcef(double lat, double lon, double h);
    double* ecefToEnu(double x, double y, double z, double lat, double lng, double height);
    double DMS_RAD(double DMS);

};

#endif // WGS84TOENU_H

WGS84ToENU.cpp

#include "wgs84toenu.h"
#include<QtMath>
#include<QDebug>
#define a   6378137
#define b   6356752.3142//b为椭球的短半轴:a=6356.7523141km
#define RAD 57.295779 //一弧度约为57.295779°
WGS84ToENU::WGS84ToENU()
{

}
double WGS84ToENU::DMS_RAD(double DMS)
{
    double Rad=0.00;
    Rad=DMS/RAD;
    qDebug()<<"Rad"<<QString::number(Rad,'f',9);
    return Rad;
}
double* WGS84ToENU::wgs84ToEcef(double lat, double lon, double h)
{
            double f = (a - b) / a;
            double e_sq = f * (2 - f);
            double lamb = this->DMS_RAD(lat);

            double phi = this->DMS_RAD(lon);
            double s = sin(lamb);
            double N = a / sqrt(1 - e_sq * s * s);

            double sin_lambda = sin(lamb);
            double cos_lambda = cos(lamb);
            double sin_phi = sin(phi);
            double cos_phi = cos(phi);

            double x = (h + N) * cos_lambda * cos_phi;
            double y = (h + N) * cos_lambda * sin_phi;
            double z = (h + (1 - e_sq) * N) * sin_lambda;

            double *returnArr=new double[3];
            returnArr[0]=x;
            returnArr[1]=y;
            returnArr[2]=z;
            return returnArr;
}

 double* WGS84ToENU::ecefToEnu(double x, double y, double z, double lat, double lng, double height)
 {
     double f = (a - b) / a;
             double e_sq = f * (2 - f);
             double lamb = this->DMS_RAD(lat);
             double phi = this->DMS_RAD(lng);
             double s =sin(lamb);
             double N = a / sqrt(1 - e_sq * s * s);
             double sin_lambda = sin(lamb);
             double cos_lambda = cos(lamb);
             double sin_phi = sin(phi);
             double cos_phi = cos(phi);

             double x0 = (height + N) * cos_lambda * cos_phi;
             double y0 = (height + N) * cos_lambda * sin_phi;
             double z0 = (height + (1 - e_sq) * N) * sin_lambda;

             double xd = x - x0;
             double yd = y - y0;
             double zd = z - z0;
             double t = -cos_phi * xd - sin_phi * yd;
             double xEast = -sin_phi * xd + cos_phi * yd;
             double yNorth = t * sin_lambda + cos_lambda * zd;
             double zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
             double *returnArrAndNEU=new double[3];
                    returnArrAndNEU[0]=xEast;
                    returnArrAndNEU[1]=yNorth;
                    returnArrAndNEU[2]=zUp;
             qDebug()<<"xEast"<<QString::number(xEast,'f',9);
             qDebug()<<"yNorth"<<QString::number(yNorth,'f',9);
             qDebug()<<"zUp"<<QString::number(zUp,'f',9);
             return returnArrAndNEU;
 }

 QT测试代码（主函数代码）：

 WGS84ToENU *wgs84ToEnu=new WGS84ToENU();
    double* arr1 = wgs84ToEnu->wgs84ToEcef(31.48999455, 121.9440825, 10.800000000);//此处经纬度是需要比对的偏移经纬度
    qDebug()<<"arr1[0]"<<QString::number(arr1[0],'f',9);
    qDebug()<<"arr1[1]"<<QString::number(arr1[1],'f',9);
    qDebug()<<"arr1[2]"<<QString::number(arr1[2],'f',9);
    double* xyz=wgs84ToEnu->ecefToEnu(arr1[0],arr1[1], arr1[2],31.489996667, 121.94408167,  12.700000000);
     qDebug()<<"result1x"<<QString::number(xyz[0],'f',9);
     qDebug()<<"result1y"<<QString::number(xyz[1],'f',9);
     qDebug()<<"result1z"<<QString::number(xyz[2],'f',9);

 JAVA源码：

public class testEcef {
	public static double[] wgs84ToEcef(double lat, double lon, double h) {
		double a = 6378137;
		double b = 6356752.3142;
		double f = (a - b) / a;
		double e_sq = f * (2 - f);
		double lamb = Math.toRadians(lat);
		double phi = Math.toRadians(lon);
		System.out.println("lamb"+lamb);
		System.out.println("phi"+phi);
		double s = Math.sin(lamb);
		double N = a / Math.sqrt(1 - e_sq * s * s);

		double sin_lambda = Math.sin(lamb);
		double cos_lambda = Math.cos(lamb);
		double sin_phi = Math.sin(phi);
		double cos_phi = Math.cos(phi);

		double x = (h + N) * cos_lambda * cos_phi;
		double y = (h + N) * cos_lambda * sin_phi;
		double z = (h + (1 - e_sq) * N) * sin_lambda;
		return new double[]{x,y,z};
	}

	public static double[] ecefToEnu(double x, double y, double z, double lat, double lng, double height) {
		double a = 6378137;
		double b = 6356752.3142;
		double f = (a - b) / a;
		double e_sq = f * (2 - f);
		double lamb = Math.toRadians(lat);
		double phi = Math.toRadians(lng);
		System.out.println("lamb2"+lamb);
		System.out.println("phi2"+phi);
		double s = Math.sin(lamb);
		double N = a / Math.sqrt(1 - e_sq * s * s);
		double sin_lambda = Math.sin(lamb);
		double cos_lambda = Math.cos(lamb);
		double sin_phi = Math.sin(phi);
		double cos_phi = Math.cos(phi);

		double x0 = (height + N) * cos_lambda * cos_phi;
		double y0 = (height + N) * cos_lambda * sin_phi;
		double z0 = (height + (1 - e_sq) * N) * sin_lambda;

		double xd = x - x0;
		double yd = y - y0;
		double zd = z - z0;

		double t = -cos_phi * xd - sin_phi * yd;

		double xEast = -sin_phi * xd + cos_phi * yd;
		double yNorth = t * sin_lambda + cos_lambda * zd;
		double zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
		return new double[] { xEast, yNorth, zUp };
	}


	public static void main(String[] args) {
		// TODO Auto-generated method stub
		//gpggaH "10.800000000"10.8
		//gpslat "31.494067233"31.48999455
		//gpslon "121.947391683"121.9440825
		//gpggaH2 "12.700000000"12.7
		//gpslat2 "31.489996667"31.489996667
		//gpslon2 "121.947391667"121.94408167
		double[] arr1 = wgs84ToEcef(31.48999455, 121.9440825, 10.800000000);//此处经纬度是需要比对的偏移经纬度
		double[] xyz=ecefToEnu(arr1[0],arr1[1], arr1[2],31.489996667, 121.94408167,  12.700000000);//此处经纬度是站点经纬度
		System.out.println("xyz[0]"+xyz[0]);
		System.out.println("xyz[1]"+xyz[1]);
		System.out.println("xyz[2]"+xyz[2]);
	}

}

经过和MATLAB相关数据核对，结果完全一样。所以代码可以放心的尝试，如有问题请留言讨论