Lattice apoll 匹配在参考线上最近点

https://blog.csdn.net/weixin_44558122/article/details/124585202

PathPoint PathMatcher::FindProjectionPoint(const PathPoint& p0,
                                           const PathPoint& p1, const float x,
                                           const float y) {
  float v0x = x - p0.x;
  float v0y = y - p0.y;

  float v1x = p1.x - p0.x;
  float v1y = p1.y - p0.y;

  float v1_norm = std::sqrt(v1x * v1x + v1y * v1y);
  // dot product = |a||b|cos(theta) and |a|cos(theta) is the projection length
  float dot = v0x * v1x + v0y * v1y;

  float delta_s = dot / v1_norm;
  return InterpolateUsingLinearApproximation(p0, p1, p0.s + delta_s);
}

TrajectoryPoint InterpolateUsingLinearApproximation(const TrajectoryPoint &tp0,
                                                    const TrajectoryPoint &tp1,
                                                    const float t) {
  const PathPoint pp0 = tp0.path_point;
  const PathPoint pp1 = tp1.path_point;
  float t0 = tp0.relative_time;
  float t1 = tp1.relative_time;

  TrajectoryPoint tp;
  tp.path_point.x = lerp(pp0.x, t0, pp1.x, t1, t);
  tp.path_point.y = lerp(pp0.y, t0, pp1.y, t1, t);
  tp.path_point.s = lerp(pp0.s, t0, pp1.s, t1, t);
  tp.path_point.theta = slerp(pp0.theta, t0, pp1.theta, t1, t);
  tp.path_point.kappa = lerp(pp0.kappa, t0, pp1.kappa, t1, t);
  tp.path_point.dkappa = lerp(pp0.dkappa, t0, pp1.dkappa, t1, t);
  tp.path_point.ddkappa = lerp(pp0.ddkappa, t0, pp1.ddkappa, t1, t);
  
  tp.v = lerp(tp0.v, t0, tp1.v, t1, t);
  tp.a = lerp(tp0.a, t0, tp1.a, t1, t);
  tp.relative_time = t;
  tp.steer = slerp(tp0.steer, t0, tp1.steer, t1, t);

  return tp;
}

template <typename T>
T lerp(const T &x0, const float t0, const T &x1, const float t1,
       const float t) {
  if (std::abs(t1 - t0) <= 1.0e-6) {
    // std::cout << "input time difference is too small at line 29." << std::endl; // wu.kc
    return x0;
  }
  const float r = (t - t0) / (t1 - t0);
  const T x = x0 + r * (x1 - x0);
  return x;
}