bang_bang_trajectory_2d.h

#pragma once
 
#include <memory>
 
#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d.h"
#include "software/ai/navigator/trajectory/kinematic_constraints.h"
#include "software/ai/navigator/trajectory/trajectory_2d.h"
#include "software/geom/rectangle.h"
 
class BangBangTrajectory2D : public Trajectory2D
{
   public:
    BangBangTrajectory2D() = default;
 
    BangBangTrajectory2D(const Point& initial_pos, const Point& final_pos,
                         const Vector& initial_vel,
                         const KinematicConstraints& constraints);
 
    void generate(const Point& initial_pos, const Point& final_pos,
                  const Vector& initial_vel, double max_vel, double max_accel,
                  double max_decel);
 
    Point getPosition(double t_sec) const override;
 
    Vector getVelocity(double t_sec) const override;
 
    Vector getAcceleration(double t_sec) const override;
 
    double getTotalTime() const override;
 
    std::vector<Rectangle> getBoundingBoxes() const override;
 
    static std::shared_ptr<Trajectory2D> generator(
        const Point& initial_pos, const Point& final_pos, const Vector& initial_vel,
        const KinematicConstraints& constraints);
 
   private:
    BangBangTrajectory1D x_trajectory;
    BangBangTrajectory1D y_trajectory;
 
    // The maximum difference the x and y trajectory runtimes could have from each other
    // in seconds
    static constexpr double TRAJ_ACCURACY_TOLERANCE_SEC = 0.01;
};