From 895991ba98907b2716d78c9bd34d18571506ba47 Mon Sep 17 00:00:00 2001 From: Yi Zhang Date: Tue, 15 Jul 2025 22:22:11 +0800 Subject: [PATCH] tmp --- .vscode/settings.json | 16 +++++ example/ZJGFP01_OUT.kmz | Bin 0 -> 1594 bytes example/config.toml | 4 +- src/dji_kmz.cpp | 98 ++++++++---------------------- src/geometry_sph.hpp | 104 ++++++++++++++++++++++++++++++++ src/simple_kml.hpp | 129 ++++++++++++++++++++++++++++++++++++++++ 6 files changed, 277 insertions(+), 74 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 example/ZJGFP01_OUT.kmz create mode 100644 src/geometry_sph.hpp create mode 100644 src/simple_kml.hpp diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..a58981e --- /dev/null +++ b/.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" + } +} \ No newline at end of file diff --git a/example/ZJGFP01_OUT.kmz b/example/ZJGFP01_OUT.kmz new file mode 100644 index 0000000000000000000000000000000000000000..e3052c6a1b61f78b8fb33ab0118a50e752a2f8ff GIT binary patch literal 1594 zcmWIWW@h1H0D+K&@1wvBD8b4g!%$w3TcsZw!pXpF?^>1t!lf1542&#a85tN@M1Tqd zfZ9Pc2Uz>+JinwLEDQ{B77Pr6FzqF&xdl0iC8>JZxjFcZdOJHh|CWbH!~Ngd0_S6A z{8m*83Ej5SBi59w5%f!-yUj_Ov6eKjYM&C-Pwf$qa|F1Kzy|2AboFlhM^NXq-osaKct*Eh(am!0@ z+S=>Z*m-<`p_;Oh&FWa$vgrMbcKa0uen=GPM7x1ik4PAysz-!sdeX@e71b*FaG{UwpMz^{yUd^l=D;n zB)@t!cS}|_vvjVVe$ofq*xl9JO=reN25mIVH~S}MbGN26kjKnm$qlyOXVxjLy>ay3 z>Yv-9&Ynq@XomY4T$zOqbjCELI4<+O?BBf)emuh zn&y=^Wto?8^U3G#xiLp+^Vg=m&(rr_ZQ*6E@oU^^9yiHUdj9K$tBQ-Z7VUe_mno)o z=%?TN>+9^;UtgMQ#-(q;VBnYIbJ6|BYReO?``^iy$Tcmk+-=ej6Swr)_r`3l4>j^^ z%R5AZ6*IHG%zSP3YN!7(2Lrl;}A)#fK|i~AA9<*Dm(A&5s;Ydb&N zj0J*|W+bf@4f5#d7wOedisD#e>D?Wcn@x6e zUo{=u6SN{n!^E%{%R7SLpUtPg>gcdEGGiQc-F4%!^Z1{0(=Y?Tw3* zd*k|DcU*ow=R^5|Kjtq4oK`q=6gr-J@!(02mfMm8x)%@HTFrH1FV-}f?Da@Q)@h4S zlb2JdcZo;8-}H+IBeY$&xJ^pq7JDpIcv2-%_u@jZYDQ%lJFoN?0?Jjqyl6{X=yAAluCY#ZILq&>qUARon}W|8=tTeQa+N&p zlD+PBURgoB-#asF8SSMS$1Luv8>>oQjrw@)&6lrF{5Cg>3b7>WZ1B3S!^vKv;JIPj zeU}IO=H1CHQJET5!1z<$)m`BvYwI58+hVMR@d58ltUY-H1dmuAy*cUV&S^mh%a*@o zl>ILHG;gyr%bUraQ+`TY3zeO9;LG}bjPD)yskv(G>ZvknOAg#*oO^l6jG9$O+f$e8 zYnIQJyN;tUq`v`T%TfnmhX^5XIh77ygNsyri$#feFitgr&^!u zyVMe(8xnSB;+rp*#1l8Y^_%&{^(oNpGcSFZFZNPV+p;wCkV*O~Q>$9vpY@&RQeyrr z$THHaJ8kY+$8YghJiwcgNsbv;87u)UjRhFqI)a$c5}6fJB4d=sAfs@V#}K1{B{##8 bMthi1sHHP5V_Dfib~6FtA|Tzw3gQ6(*gc>} literal 0 HcmV?d00001 diff --git a/example/config.toml b/example/config.toml index 85e5a90..f7c3e00 100644 --- a/example/config.toml +++ b/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" diff --git a/src/dji_kmz.cpp b/src/dji_kmz.cpp index 5c977c6..5969477 100644 --- a/src/dji_kmz.cpp +++ b/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 &plon, vector &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 == "") - { - 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 &lons, vector &lats, vector &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 diff --git a/src/geometry_sph.hpp b/src/geometry_sph.hpp new file mode 100644 index 0000000..e7b53f6 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/src/simple_kml.hpp b/src/simple_kml.hpp new file mode 100644 index 0000000..31b425d --- /dev/null +++ b/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 &lons, vector &lats, vector &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 &plon, vector &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) == "") // 寻找路径标记段 + { + while (getline(ifile, line)) + { + if (trimstr(line) == "") // 找到坐标点开始行 + { + 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 &plon, vector &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) == "") // 寻找路径标记段 + { + while (getline(ifile, line)) + { + if (trimstr(line) == "") // 找到坐标点开始行 + { + 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; +} \ No newline at end of file