Add UTM.

src/driver/driver_gps_hcp2/main.cpp

@@ -12,10 +12,10 @@ iv::Ivfault *gfault = nullptr;
 int main(int argc, char *argv[])
 {
 
-//    double lat = 29.0;
-//    double lon = 139.0;
-//    double xv,yv;
-//    GaussProjCal(lon,lat,&xv,&yv);
+    double lat = 28.32;
+    double lon = 117.81;
+    double xv,yv;
+    GaussProjCal(lon,lat,&xv,&yv);
     showversion("driver_gps_hcp2");
     QCoreApplication a(argc, argv);
 

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -79,4 +79,10 @@ DEFINES += INPILOT
 
 LIBS += -livprotoif
 
+#DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}
+
 

src/driver/driver_map_xodrload/gnss_coordinate_convert.cpp

@@ -1,5 +1,12 @@
 #include <gnss_coordinate_convert.h>
 
+
+
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
+
 void gps2xy(double J4, double K4, double *x, double *y)
 {
     int L4 = (int)((K4 - 1.5) / 3) + 1;
@@ -25,6 +32,19 @@ void gps2xy(double J4, double K4, double *x, double *y)
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
 void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
     int ProjNo = 0; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double a, f, e2, ee, NN, T, C, A, M, iPI;
@@ -55,6 +75,7 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
     xval = xval + X0; yval = yval + Y0;
     *X = xval;
     *Y = yval;
+#endif
 }
 
 
@@ -115,6 +136,19 @@ void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
 void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
 {
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
     int ProjNo; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
@@ -150,4 +184,6 @@ void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
     //转换为度 DD
     *longitude = longitude1 / iPI;
     *latitude = latitude1 / iPI;
+
+#endif
 }

src/tool/tool_xodrobj/gnss_coordinate_convert.cpp

@@ -1,5 +1,10 @@
 #include <gnss_coordinate_convert.h>
 
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
+
 void gps2xy(double J4, double K4, double *x, double *y)
 {
     int L4 = (int)((K4 - 1.5) / 3) + 1;
@@ -25,6 +30,19 @@ void gps2xy(double J4, double K4, double *x, double *y)
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
 void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
     int ProjNo = 0; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double a, f, e2, ee, NN, T, C, A, M, iPI;
@@ -55,57 +73,58 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
     xval = xval + X0; yval = yval + Y0;
     *X = xval;
     *Y = yval;
+#endif
 }
 
 
 
 //=======================zhaobo0904
 #define PI  3.14159265358979
-//void GaussProjCal(double lon, double lat, double *X, double *Y)
-//{
-//    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
-//    double a = 6378140.0;
-//    double e2 = 0.006694384999588;
-//    double ep2 = e2/(1-e2);
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
 
-//    // 原点所在经度
-//    double lon_origin = 6.0*int(lon/6) + 3.0;
-//    lon_origin *= PI / 180.0;
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
 
-//    double k0 = 0.9996;
+    double k0 = 0.9996;
 
-//    // 角度转弧度
-//    double lat1 = lat * PI / 180.0;
-//    double lon1 = lon * PI / 180.0;
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
 
 
-//    // 经线在该点处的曲率半径,
-//    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
 
 
-//    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
-//    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
-//    // 首先计算前四项的系数 a1~a4.
-//    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
-//    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
-//    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
-//    double a4 = (35*e2*e2*e2)/3072;
-//    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
 
-//    // 辅助量
-//    double T = tan(lat1)*tan(lat1);
-//    double C = ep2*cos(lat1)*cos(lat1);
-//    double A = (lon1 - lon_origin)*cos(lat1);
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
 
-//    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
-//    *Y = M + N * tan(lat1) * ((A*A)/2 +
-//                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
-//                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
 
 
-//    *Y *= k0;
-//    return;
-//}
+    *Y *= k0;
+    return;
+}
 //==========================================================
 
 
@@ -115,6 +134,19 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
 void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
 {
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
     int ProjNo; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
@@ -150,4 +182,6 @@ void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
     //转换为度 DD
     *longitude = longitude1 / iPI;
     *latitude = latitude1 / iPI;
+
+#endif
 }

src/tool/tool_xodrobj/tool_xodrobj.pro

@@ -74,3 +74,9 @@ DISTFILES += \
     geodata.proto
 
 LIBS += -livprotoif
+
+#DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}