pure_pursuit.h

#pragma once
 
#include "core/utils/geometry.h"
#include "core/utils/math/geometry/pose2d.h"
#include "core/utils/math/geometry/translation2d.h"
#include "vex.h"
#include <vector>
 
namespace PurePursuit {
class Path {
  public:
    Path(std::vector<Translation2d> points, double radius);

    const std::vector<Translation2d> get_points();

    double get_radius();

    bool is_valid();
 
  private:
    std::vector<Translation2d> 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;
 
    Translation2d getPoint() const { return Translation2d(x, y); }
 
    Translation2d getTangent() const { return Translation2d(mag, Rotation2d(dir)); }
};

extern std::vector<Translation2d>
line_circle_intersections(Translation2d center, double r, Translation2d point1, Translation2d point2);
extern Translation2d get_lookahead(const std::vector<Translation2d> &path, Pose2d robot_loc, double radius);

extern std::vector<Translation2d> inject_path(const std::vector<Translation2d> &path, double spacing);

 
extern std::vector<Translation2d>
smooth_path(const std::vector<Translation2d> &path, double weight_data, double weight_smooth, double tolerance);
 
extern std::vector<Translation2d> smooth_path_cubic(const std::vector<Translation2d> &path, double res);

extern std::vector<Translation2d> smooth_path_hermite(const std::vector<hermite_point> &path, double step);

extern double estimate_remaining_dist(const std::vector<Translation2d> &path, Pose2d robot_pose, double radius);
 
} // namespace PurePursuit