geopro/src/io/gpr/GpsTrack.cpp

#include "io/gpr/GpsTrack.hpp"

#include <algorithm>
#include <cmath>
#include <fstream>
#include <limits>
#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;
}

CenterlineProj projectToCenterline(const std::vector<XY>& centerline,
                                   const XY& p) {
  CenterlineProj r;
  const std::size_t n = centerline.size();
  if (n == 0) return r;
  if (n == 1) {
    r.s = 0.0;
    r.d = std::hypot(p.x - centerline[0].x, p.y - centerline[0].y);
    return r;
  }

  // 逐段求脚点（夹到段内），取距 p 最近者；累计里程 + 带符号横偏。
  double bestDist2 = std::numeric_limits<double>::infinity();
  double cum = 0.0;  // 段起点累计里程
  for (std::size_t i = 1; i < n; ++i) {
    const XY& a = centerline[i - 1];
    const XY& b = centerline[i];
    const double ex = b.x - a.x, ey = b.y - a.y;
    const double segLen2 = ex * ex + ey * ey;
    const double segLen = std::sqrt(segLen2);

    // p 在段上的投影参数 u（夹到 [0,1]）。
    double u = 0.0;
    if (segLen2 > 0.0)
      u = ((p.x - a.x) * ex + (p.y - a.y) * ey) / segLen2;
    if (u < 0.0) u = 0.0;
    if (u > 1.0) u = 1.0;

    const double footX = a.x + u * ex;
    const double footY = a.y + u * ey;
    const double dx = p.x - footX, dy = p.y - footY;
    const double dist2 = dx * dx + dy * dy;

    if (dist2 < bestDist2) {
      bestDist2 = dist2;
      r.s = cum + u * segLen;
      // 带符号横偏：左法向 n=(-ty,tx)（单位段向量）。退化段 → d=到脚点距离。
      if (segLen > 0.0) {
        const double tx = ex / segLen, ty = ey / segLen;
        const double nx = -ty, ny = tx;  // 左法向
        r.d = dx * nx + dy * ny;
      } else {
        r.d = std::sqrt(dist2);
      }
    }
    cum += segLen;
  }
  return r;
}

std::vector<XY> resampleAndSmooth(const std::vector<XY>& polyline, double step,
                                  int smoothWindow) {
  const std::size_t n = polyline.size();
  if (n < 2 || step <= 0.0) return polyline;

  // 累计里程。
  std::vector<double> cum(n, 0.0);
  for (std::size_t i = 1; i < n; ++i)
    cum[i] = cum[i - 1] +
             std::hypot(polyline[i].x - polyline[i - 1].x,
                        polyline[i].y - polyline[i - 1].y);
  const double total = cum.back();
  if (total <= 0.0) return polyline;

  // 均匀里程取样（含终点）。
  std::vector<XY> out;
  const int m = static_cast<int>(std::floor(total / step));
  out.reserve(static_cast<std::size_t>(m) + 2);
  std::size_t seg = 1;
  for (int k = 0; k <= m; ++k) {
    const double target = k * step;
    while (seg < n && cum[seg] < target) ++seg;
    if (seg >= n) seg = n - 1;
    const double segLen = cum[seg] - cum[seg - 1];
    const double lf = segLen > 0.0 ? (target - cum[seg - 1]) / segLen : 0.0;
    XY q;
    q.x = polyline[seg - 1].x + (polyline[seg].x - polyline[seg - 1].x) * lf;
    q.y = polyline[seg - 1].y + (polyline[seg].y - polyline[seg - 1].y) * lf;
    out.push_back(q);
  }
  if (out.empty() || std::hypot(out.back().x - polyline.back().x,
                                out.back().y - polyline.back().y) > 1e-9)
    out.push_back(polyline.back());

  // 轻度滑动平均（端点收缩窗口；不平滑首尾以保里程范围）。
  if (smoothWindow <= 0 || out.size() < 3) return out;
  std::vector<XY> sm = out;
  const int N = static_cast<int>(out.size());
  for (int i = 1; i < N - 1; ++i) {
    int w = std::min({smoothWindow, i, N - 1 - i});
    double sx = 0, sy = 0;
    int cnt = 0;
    for (int j = i - w; j <= i + w; ++j) {
      sx += out[j].x;
      sy += out[j].y;
      ++cnt;
    }
    sm[i].x = sx / cnt;
    sm[i].y = sy / cnt;
  }
  return sm;
}

}  // namespace geopro::io::gpr