pure_pursuit.h

#pragma once
 
#include "../core/include/utils/geometry.h"
#include "../core/include/utils/vector2d.h"
#include "vex.h"
#include <vector>
 
using namespace vex;
 
namespace PurePursuit {
class Path {
public:
  Path(std::vector<point_t> points, double radius);
 
  std::vector<point_t> get_points();
 
  double get_radius();
 
  bool is_valid();
 
private:
  std::vector<point_t> points;
  double radius;
  bool valid;
};
struct spline {
  double a, b, c, d, x_start, x_end;
 
  double getY(double x) { return a * pow((x - x_start), 3) + b * pow((x - x_start), 2) + c * (x - x_start) + d; }
};
struct hermite_point {
  double x;
  double y;
  double dir;
  double mag;
 
  point_t getPoint() const { return {x, y}; }
 
  Vector2D getTangent() const { return Vector2D(dir, mag); }
};
 
extern std::vector<point_t> line_circle_intersections(point_t center, double r, point_t point1, point_t point2);
extern point_t get_lookahead(const std::vector<point_t> &path, pose_t robot_loc, double radius);
 
extern std::vector<point_t> inject_path(const std::vector<point_t> &path, double spacing);
 
extern std::vector<point_t> smooth_path(const std::vector<point_t> &path, double weight_data, double weight_smooth,
                                        double tolerance);
 
extern std::vector<point_t> smooth_path_cubic(const std::vector<point_t> &path, double res);
 
extern std::vector<point_t> smooth_path_hermite(const std::vector<hermite_point> &path, double step);
 
extern double estimate_remaining_dist(const std::vector<point_t> &path, pose_t robot_pose, double radius);
 
} // namespace PurePursuit