tmp

.vscode/settings.json

@@ -0,0 +1,16 @@
+{
+    "files.associations": {
+        "fstream": "cpp",
+        "__hash_table": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "__tree": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_set": "cpp"
+    }
+}

example/config.toml

@@ -1,8 +1,8 @@
 author = "zhangyi"
-kmzFile = "ZJGFP01_OUT.kmz"
+kmzFile = "example/ZJGFP01_OUT.kmz"
 
 [placemark]
-PointFile = "ZJGFP01.kml"
+PointFile = "example/ZJGFP01.kml"
 useGlobalHeight = "yes"
 useGlobalSpeed = "yes"
 useGlobalHeadingParam = "yes"

src/dji_kmz.cpp

@@ -1,10 +1,31 @@
+/*
+ * dji_kmz.cpp - DJI Waypoint File Generator
+ * 
+ * Converts Google Earth KML files or plain coordinate files to DJI-compatible KMZ wayline formats.
+ * 
+ * Features:
+ *  - Supports multiple drone models (Matrice series, Mavic series)
+ *  - Configurable flight parameters (altitude, speed, heading modes)
+ *  - Flexible waypoint import options (KML/CSV)
+ * 
+ * Disclaimer: 
+ *  This open-source software is provided "as is" without warranty of any kind,
+ *  and the author/contributors shall not be liable for any damages arising from its use.
+ * 
+ * Author: Dr. Yi Zhang
+ * Contact: School of Earth Science, Zhejiang University
+ * Email: yizhang-geo@zju.edu.cn
+ */
+
 #include "iostream"
 #include "fstream"
 #include "sstream"
 #include "iomanip"
 #include "vector"
-#include "filesystem"
+//#include "filesystem"
 #include "toml.hpp"
+#include "simple_kml.hpp"
+#include "geometry_sph.hpp"
 
 using namespace std;
 
@@ -166,78 +187,11 @@ namespace dji_kmz
         throw std::runtime_error("dji_kmz: Invalid placemark use global turn-param mode.");
     }
 
-    void get_googleEarth_coorpoints(string google_kml, vector<double> &plon, vector<double> &plat)
-    {
-        ifstream ifile(google_kml);
-        if (!ifile) throw std::runtime_error("dji_kmz: Fail to open the google earth's .kml file.");
-
-        plon.clear();
-        plat.clear();
-
-        double lon, lat;
-        string line, cor_str;
-        stringstream line_ss, cor_ss;
-        while (getline(ifile, line))
-        {
-            line.erase(0, line.find_first_not_of(" \t"));
-            line.erase(line.find_last_not_of(" \t") + 1);
-
-            if (line == "<coordinates>")
-            {
-                getline(ifile, line);
-                line.erase(0, line.find_first_not_of(" \t"));
-                line.erase(line.find_last_not_of(" \t") + 1);
-
-                line_ss.clear();
-                line_ss.str(line);
-                while (line_ss >> cor_str)
-                {
-                    replace(cor_str.begin(), cor_str.end(), ',', ' ');
-
-                    cor_ss.clear();
-                    cor_ss.str(cor_str);
-                    cor_ss >> lon >> lat;
-                    plon.push_back(lon);
-                    plat.push_back(lat);
-                }
-                break;
-            }
-        }
-        ifile.close();
-    }
-
-    void read_PointFile(string filename, vector<double> &lons, vector<double> &lats, vector<double> &hgts)
-    {
-        std::ifstream infile(filename);
-        if (!infile.is_open()) throw std::runtime_error("dji_kmz: Fail to open point file.");
-
-        lons.clear();
-        lats.clear();
-        hgts.clear();
-
-        string line;
-        stringstream ss_line;
-        double lon, lat, hgt;
-        while (getline(infile, line))
-        {
-            replace(line.begin(), line.end(), ',', ' ');
-            ss_line.clear();
-            ss_line.str(line);
-            ss_line >> lon >> lat >> hgt;
-
-            lons.push_back(lon);
-            lats.push_back(lat);
-            hgts.push_back(hgt);
-        }
-        infile.close();
-        return;
-    }
-
     /**
      * @brief 在多边形区域内按一定间隔生成航线点
      * 
-     * @param plon 多边形顶点经度数组
-     * @param plat 多边形顶点纬度数组
+     * @param plon 多边形顶点经度数组（首尾点重合）
+     * @param plat 多边形顶点纬度数组（首尾点重合）
      * @param lon1 航线参考方向起点经度
      * @param lat1 航线参考方向起点纬度
      * @param lon2 航线参考方向终点经度
@@ -403,10 +357,10 @@ This program can read coordinate marker files saved by Google Earth (in KML form
     exten_name = point_file.substr(point_file.find_last_of('.'));
     if (exten_name == ".kml")
     {
-        dji_kmz::get_googleEarth_coorpoints(point_file, lons, lats);
+        read_kml_lineString(point_file, lons, lats);
         hgts.resize(lons.size(), -1.0);
     }
-    else dji_kmz::read_PointFile(point_file, lons, lats, hgts);
+    else read_plainPoints(point_file, lons, lats, hgts);
 
     // Create a template folder
     // The following may diff between different OS-systems, improve later

src/geometry_sph.hpp

@@ -0,0 +1,104 @@
+#include "cmath"
+#include "algorithm"
+
+#define Earth_R 6371008.8
+#define WGS84_r 6356752.3142
+#define WGS84_R 6378136.460
+#define ZERO 1e-30
+
+struct pointC
+{
+    double x, y, z;
+};
+
+struct pointS
+{
+    double lon, lat, rad;
+};
+
+double module(const pointC& a)
+{
+    return sqrt(a.x*a.x + a.y*a.y + a.z*a.z);
+}
+
+double dot(const pointC& a, const pointC& b)
+{
+    return a.x*b.x+a.y*b.y+a.z*b.z;
+}
+
+pointC cross(const pointC& a, const pointC& b, double cut_off = ZERO)
+{
+    pointC v;
+    v.x = a.y*b.z-a.z*b.y;
+    v.y = a.z*b.x-a.x*b.z;
+    v.z = a.x*b.y-a.y*b.x;
+    if (fabs(v.x) <= cut_off) v.x = 0;
+    if (fabs(v.y) <= cut_off) v.y = 0;
+    if (fabs(v.z) <= cut_off) v.z = 0;
+    return v;
+}
+
+pointC s2c(const pointS& ps)
+{
+    pointC pc;
+    pc.x = ps.rad*cos(M_PI * ps.lat/180.0)*cos(M_PI * ps.lon/180.0);
+	pc.y = ps.rad*cos(M_PI * ps.lat/180.0)*sin(M_PI * ps.lon/180.0);
+	pc.z = ps.rad*sin(M_PI * ps.lat/180.0);
+    return pc;
+}
+
+pointC e2c(const pointS& ps, double r, double R)
+{
+    pointC v;
+	v.x = R*cos(M_PI*ps.lat/180.0)*cos(M_PI*ps.lon/180.0);
+	v.y = R*cos(M_PI*ps.lat/180.0)*sin(M_PI*ps.lon/180.0);
+    v.z = r*sin(M_PI*ps.lat/180.0);
+    return v;
+}
+
+pointS c2s(const pointC& pc)
+{
+    pointS ps;
+    ps.rad = sqrt(pow(pc.x,2)+pow(pc.y,2)+pow(pc.z,2));
+	if (fabs(ps.rad) < ZERO) //点距离原点极近 将点置于原点
+	{
+		ps.lat = ps.lon = ps.rad = 0.0;
+	}
+	else
+	{
+		ps.lat = 90.0 - acos(pc.z/ps.rad)*180.0/M_PI;
+		ps.lon = atan2(pc.y, pc.x)*180.0/M_PI;
+	}
+    return ps;
+}
+
+double bigArc_angle(const pointS& ps1, const pointS& ps2)
+{
+    pointC a = s2c(ps1);
+    pointC b = s2c(ps2);
+    double d = std::min(1.0, std::max(-1.0, dot(a, b)/(module(a)*module(b))));
+	return acos(d);
+}
+
+double bigArc_distance(const pointS& ps1, const pointS& ps2)
+{
+    double arc = bigArc_angle(ps1, ps2);
+    return Earth_R*arc/(2*M_PI);
+}
+
+pointS bigArc_intersection(const pointS& ps11, const pointS& ps12, 
+    const pointS& ps21, const pointS& ps22)
+{
+    pointC p1 = cross(s2c(ps12), s2c(ps11));
+    pointC p2 = cross(s2c(ps22), s2c(ps21));
+    pointC c = cross(p1, p2);
+
+    // 这里假设大圆弧从1点到2点并延伸，因此交点与1点夹角应更大，否则反向
+    if (bigArc_angle(ps11, c2s(c)) <  bigArc_angle(ps12, c2s(c)) ||
+        bigArc_angle(ps21, c2s(c)) <  bigArc_angle(ps22, c2s(c)))
+    {
+        c.x *= -1.0; c.y *= -1.0; c.z *= -1.0;
+    }
+
+    return c2s(c);
+}

src/simple_kml.hpp

@@ -0,0 +1,129 @@
+#include "iostream"
+#include "fstream"
+#include "sstream"
+#include "iomanip"
+#include "vector"
+
+using namespace std;
+
+string trimstr(string s)
+{
+    s.erase(0, s.find_first_not_of(" \t"));
+    s.erase(s.find_last_not_of(" \t") + 1);
+    return s;
+}
+
+void read_plainPoints(string filename, vector<double> &lons, vector<double> &lats, vector<double> &hgts)
+    {
+        std::ifstream infile(filename);
+        if (!infile.is_open()) throw std::runtime_error("dji_kmz: Fail to open point file.");
+
+        lons.clear();
+        lats.clear();
+        hgts.clear();
+
+        string line;
+        stringstream ss_line;
+        double lon, lat, hgt;
+        while (getline(infile, line))
+        {
+            replace(line.begin(), line.end(), ',', ' ');
+            ss_line.clear();
+            ss_line.str(line);
+            ss_line >> lon >> lat >> hgt;
+
+            lons.push_back(lon);
+            lats.push_back(lat);
+            hgts.push_back(hgt);
+        }
+        infile.close();
+        return;
+    }
+
+void read_kml_lineString(string google_kml, vector<double> &plon, vector<double> &plat)
+{
+    ifstream ifile(google_kml);
+    if (!ifile) throw std::runtime_error("dji_kmz: Fail to open the google earth's .kml file.");
+
+    plon.clear();
+    plat.clear();
+
+    double lon, lat;
+    string line, cor_str, l_name;
+    stringstream line_ss, cor_ss;
+    while (getline(ifile, line))
+    {
+        if (trimstr(line) == "<LineString>") // 寻找路径标记段
+        {
+            while (getline(ifile, line))
+            {
+                if (trimstr(line) == "<coordinates>") // 找到坐标点开始行
+                {
+                    getline(ifile, line);
+
+                    line_ss.clear();
+                    line_ss.str(trimstr(line));
+                    while (line_ss >> cor_str)
+                    {
+                        replace(cor_str.begin(), cor_str.end(), ',', ' ');
+
+                        cor_ss.clear();
+                        cor_ss.str(cor_str);
+                        cor_ss >> lon >> lat;
+                        plon.push_back(lon);
+                        plat.push_back(lat);
+                    }
+                    break;
+                }
+            }
+            break;
+        }
+    }
+
+    ifile.close();
+    return;
+}
+
+void read_kml_polygon(string google_kml, vector<double> &plon, vector<double> &plat)
+{
+    ifstream ifile(google_kml);
+    if (!ifile) throw std::runtime_error("dji_kmz: Fail to open the google earth's .kml file.");
+
+    plon.clear();
+    plat.clear();
+
+    double lon, lat;
+    string line, cor_str, l_name;
+    stringstream line_ss, cor_ss;
+    while (getline(ifile, line))
+    {
+        if (trimstr(line) == "<Polygon>") // 寻找路径标记段
+        {
+            while (getline(ifile, line))
+            {
+                if (trimstr(line) == "<coordinates>") // 找到坐标点开始行
+                {
+                    getline(ifile, line);
+
+                    line_ss.clear();
+                    line_ss.str(trimstr(line));
+                    while (line_ss >> cor_str)
+                    {
+                        replace(cor_str.begin(), cor_str.end(), ',', ' ');
+
+                        cor_ss.clear();
+                        cor_ss.str(cor_str);
+                        cor_ss >> lon >> lat;
+                        plon.push_back(lon);
+                        plat.push_back(lat);
+                    }
+                    break;
+                }
+            }
+            break;
+        }
+    }
+
+    ifile.close();
+    return;
+}