110 lines
2.8 KiB
C++
110 lines
2.8 KiB
C++
#include "io/gpr/GpsTrack.hpp"
|
||
|
||
#include <cmath>
|
||
#include <fstream>
|
||
#include <sstream>
|
||
#include <stdexcept>
|
||
|
||
namespace geopro::io::gpr {
|
||
|
||
namespace {
|
||
constexpr double kMetersPerDegLat = 111320.0;
|
||
constexpr double kPi = 3.14159265358979323846;
|
||
|
||
// 严格把整列解析为 double；失败返回 false（不抛，供逐行容错跳过）。
|
||
bool parseDouble(const std::string& s, double& out) {
|
||
try {
|
||
std::size_t used = 0;
|
||
out = std::stod(s, &used);
|
||
return used == s.size();
|
||
} catch (...) {
|
||
return false;
|
||
}
|
||
}
|
||
} // namespace
|
||
|
||
GpsTrack parseGps(const std::string& path) {
|
||
std::ifstream f(path);
|
||
if (!f) throw std::runtime_error("parseGps: 打不开 " + path);
|
||
|
||
GpsTrack track;
|
||
std::string line;
|
||
while (std::getline(f, line)) {
|
||
// 去掉可能的 \r（CRLF）。
|
||
if (!line.empty() && line.back() == '\r') line.pop_back();
|
||
|
||
std::istringstream tok(line);
|
||
std::vector<std::string> cols;
|
||
std::string c;
|
||
while (tok >> c) cols.push_back(c);
|
||
if (cols.size() < 7) continue; // 列数不足
|
||
|
||
GpsPt p;
|
||
if (!parseDouble(cols[2], p.lat)) continue; // 纬
|
||
if (!parseDouble(cols[4], p.lon)) continue; // 经
|
||
if (!parseDouble(cols[6], p.elev)) continue; // 高
|
||
track.pts.push_back(p);
|
||
}
|
||
return track;
|
||
}
|
||
|
||
XY lonLatToLocalM(double lat, double lon, double lat0, double lon0) {
|
||
XY xy;
|
||
xy.x = (lon - lon0) * kMetersPerDegLat * std::cos(lat0 * kPi / 180.0);
|
||
xy.y = (lat - lat0) * kMetersPerDegLat;
|
||
return xy;
|
||
}
|
||
|
||
PosHeading interpAlongTrack(const std::vector<XY>& trackM, double frac) {
|
||
PosHeading r;
|
||
if (trackM.empty()) return r;
|
||
if (trackM.size() == 1) {
|
||
r.pos = trackM.front();
|
||
return r;
|
||
}
|
||
|
||
// 各段长度 + 累积里程。
|
||
const std::size_t n = trackM.size();
|
||
std::vector<double> cum(n, 0.0);
|
||
for (std::size_t i = 1; i < n; ++i) {
|
||
const double dx = trackM[i].x - trackM[i - 1].x;
|
||
const double dy = trackM[i].y - trackM[i - 1].y;
|
||
cum[i] = cum[i - 1] + std::hypot(dx, dy);
|
||
}
|
||
const double total = cum.back();
|
||
|
||
// 退化（零长轨迹）：返回起点，航向取首段方向或 (1,0)。
|
||
if (total <= 0.0) {
|
||
r.pos = trackM.front();
|
||
return r;
|
||
}
|
||
|
||
if (frac <= 0.0) frac = 0.0;
|
||
if (frac >= 1.0) frac = 1.0;
|
||
const double target = frac * total;
|
||
|
||
// 找包含 target 里程的段 [i-1, i]。
|
||
std::size_t seg = 1;
|
||
while (seg < n && cum[seg] < target) ++seg;
|
||
if (seg >= n) seg = n - 1;
|
||
|
||
const double segLen = cum[seg] - cum[seg - 1];
|
||
const double localFrac = segLen > 0.0 ? (target - cum[seg - 1]) / segLen : 0.0;
|
||
|
||
const XY& a = trackM[seg - 1];
|
||
const XY& b = trackM[seg];
|
||
r.pos.x = a.x + (b.x - a.x) * localFrac;
|
||
r.pos.y = a.y + (b.y - a.y) * localFrac;
|
||
|
||
double dx = b.x - a.x;
|
||
double dy = b.y - a.y;
|
||
const double len = std::hypot(dx, dy);
|
||
if (len > 0.0) {
|
||
r.hx = dx / len;
|
||
r.hy = dy / len;
|
||
}
|
||
return r;
|
||
}
|
||
|
||
} // namespace geopro::io::gpr
|