spline.hpp

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <optional>
#include <vector>
 
namespace light {
 
struct Waypoint {
  float x = 0.0f;                  
  float y = 0.0f;                  
  std::optional<float> headingRad; 
  std::optional<float> speed;      
};
 
struct SplineSample {
  float x = 0.0f, y = 0.0f;     
  float dx = 0.0f, dy = 0.0f;   
  float ddx = 0.0f, ddy = 0.0f; 
};
 
struct HermiteSegment {
  float p0x, p0y, v0x, v0y, a0x, a0y; 
  float p1x, p1y, v1x, v1y, a1x, a1y; 
 
  SplineSample eval(float u) const;
};
 
class Spline {
 public:
  Spline() = default;
 
  explicit Spline(const std::vector<Waypoint>& wps);
 
  bool valid() const { return !segs_.empty(); }
 
  float totalArcLen() const { return totalLen_; }
 
  size_t segmentCount() const { return segs_.size(); }
 
  SplineSample sampleAt(float s) const;
 
  float headingAt(float s) const;
 
  float curvatureAt(float s) const;
 
 private:
  std::vector<HermiteSegment> segs_;
  std::vector<std::array<float, 65>> segArcTables_;  // per-segment u→s LUT
  std::vector<float> segLens_;
  float totalLen_ = 0.0f;
 
  void sToSegU(float s, int& segIdx, float& u) const;
};
 
}  // namespace light