split all impls from declarations

src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp

@@ -0,0 +1,73 @@
+#include "fl/mathematics/control/PurePursuitTracker.h"
+
+namespace fl {
+
+PurePursuitTracker::PurePursuitTracker(const double lat, const units::second_t lookahead_time,
+                                       const double min_lookahead_distance)
+    : lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {}
+
+TrajectoryTrackerVelocityOutput PurePursuitTracker::CalculateState(
+    const TimedIterator<Pose2dWithCurvature>& iterator, const Pose2d& robot_pose) const {
+  const auto reference_point = iterator.CurrentState();
+
+  // Compute the lookahead state.
+  const auto lookahead_state = CalculateLookaheadPose(iterator, robot_pose);
+
+  // Find the appropriate lookahead point.
+  const auto lookahead_transform = lookahead_state.InFrameOfReferenceOf(robot_pose);
+
+  // Calculate latitude error.
+  const auto x_error =
+      reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X();
+
+  // Calculate the velocity at the reference point.
+  const double vd = units::unit_cast<double>(reference_point.state.Velocity());
+
+  // Calculate the distance from the robot to the lookahead.
+  const double l = lookahead_transform.Translation().Norm();
+
+  // Calculate the curvature of the arc that connects the robot and the lookahead point.
+  const double curvature = 2 * lookahead_transform.Translation().Y() / std::pow(l, 2);
+
+  // Adjust the linear velocity to compensate for the robot lagging behind.
+  const double adjusted_linear_velocity = vd * lookahead_transform.Rotation().Cos() + lat_ * x_error;
+
+  return {adjusted_linear_velocity, adjusted_linear_velocity * curvature};
+}
+
+Pose2d PurePursuitTracker::CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
+                                                  const Pose2d& robot_pose) const {
+  auto lookahead_pose_by_time = iterator.Preview(lookahead_time_).state.State().Pose();
+
+  // The lookahead point is farther from the robot than the minimum lookahead distance.
+  // Therefore we can use this point.
+  if (lookahead_pose_by_time.InFrameOfReferenceOf(robot_pose).Translation().Norm() >=
+      min_lookahead_distance_) {
+    return lookahead_pose_by_time;
+  }
+
+  auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose();
+  auto previewed_time             = units::second_t(0.0);
+
+  // Run the loop until a distance that is greater than the minimum lookahead distance is found or until
+  // we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory.
+  while (iterator.RemainingProgress() > previewed_time) {
+    previewed_time += 0.02_s;
+    lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
+
+    const auto lookahead_distance =
+        lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose).Translation().Norm();
+
+    if (lookahead_distance > min_lookahead_distance_) {
+      return lookahead_pose_by_distance;
+    }
+  }
+
+  const auto remaining = min_lookahead_distance_ -
+                         (lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose)).Translation().Norm();
+
+  // Extend the trajectory
+  return lookahead_pose_by_distance.TransformBy(
+      Pose2d{Translation2d{remaining * (iterator.Reversed() ? -1.0 : 1.0), 0.0}, Rotation2d{}});
+}
+}  // namespace fl

src/main/cpp/fl/mathematics/control/RamseteTracker.cpp

@@ -0,0 +1,23 @@
+#include "fl/mathematics/control/RamseteTracker.h"
+
+namespace fl {
+
+RamseteTracker::RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {}
+
+TrajectoryTrackerVelocityOutput RamseteTracker::CalculateState(
+    const TimedIterator<Pose2dWithCurvature>& iterator, const Pose2d& robot_pose) const {
+  const TimedEntry<Pose2dWithCurvature> reference_state = iterator.CurrentState().state;
+  const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
+
+  const auto vd = units::unit_cast<double>(reference_state.Velocity());
+  const auto wd = vd * reference_state.State().Curvature();
+
+  const auto k1          = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd);
+  const auto angle_error = error.Rotation().Radians();
+
+  const auto linear  = vd * error.Rotation().Cos() + k1 * error.Translation().X();
+  const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error;
+
+  return {linear, angular};
+}
+}  // namespace fl

src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp

@@ -0,0 +1,51 @@
+#include "fl/mathematics/control/TrajectoryTracker.h"
+
+namespace fl {
+
+void TrajectoryTracker::Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
+  iterator_          = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
+  previous_velocity_ = nullptr;
+  previous_time_     = units::second_t(-1);
+}
+
+TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d&         current_pose,
+                                                     const units::second_t current_time) {
+  if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
+  auto& iterator = *iterator_;
+
+  const auto dt = (units::unit_cast<double>(previous_time_) < 0.0)
+                    ? units::second_t{0.0}
+                    : current_time - previous_time_;
+  previous_time_ = current_time;
+
+  iterator.Advance(dt);
+
+  const auto velocity = CalculateState(iterator, current_pose);
+
+  const auto linear_velocity  = velocity.linear_velocity;
+  const auto angular_velocity = velocity.angular_velocity;
+
+  if (previous_velocity_ == nullptr || dt <= units::second_t()) {
+    previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity});
+    return {linear_velocity, 0.0, angular_velocity, 0.0};
+  }
+
+  const auto _dt = units::unit_cast<double>(dt);
+
+  const TrajectoryTrackerOutput output{
+    linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
+    (angular_velocity - previous_velocity_->angular_velocity) / _dt
+  };
+
+  previous_velocity_->linear_velocity  = linear_velocity;
+  previous_velocity_->angular_velocity = angular_velocity;
+
+  return output;
+}
+
+TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> TrajectoryTracker::ReferencePoint() const {
+  return iterator_->CurrentState();
+}
+
+bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); }
+}

src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp

@@ -25,6 +25,27 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) {
   return Rotation2d(Deg2Rad(degrees));
 }
 
+Rotation2d Rotation2d::operator-(const Rotation2d& other) const { return *this + -other; }
+
+Rotation2d Rotation2d::operator-() const { return Rotation2d(-value_); }
+
+Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+  return Rotation2d{
+    Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
+    true
+  };
+}
+
+double Rotation2d::Radians() const { return value_; }
+
+double Rotation2d::Degrees() const { return Rad2Deg(value_); }
+
+double Rotation2d::Cos() const { return cos_; }
+
+double Rotation2d::Sin() const { return sin_; }
+
+double Rotation2d::Tan() const { return sin_ / cos_; }
+
 bool Rotation2d::IsParallel(const Rotation2d& other) const {
   return EpsilonEquals((*this - other).Radians(), 0.0);
 }

src/main/cpp/fl/mathematics/geometry/Translation2d.cpp

@@ -7,6 +7,10 @@ Translation2d::Translation2d(const double x, const double y) : x_(x), y_(y) {}
 Translation2d::Translation2d(const double length, const Rotation2d& rotation)
     : x_(length * rotation.Cos()), y_(length * rotation.Sin()) {}
 
+double Translation2d::Distance(const Translation2d& other) const {
+  return std::hypot(other.X() - X(), other.Y() - Y());
+}
+
 Translation2d Translation2d::Interpolate(const Translation2d& end_value, const double t) const {
   if (t <= 0) {
     return *this;
@@ -17,4 +21,35 @@ Translation2d Translation2d::Interpolate(const Translation2d& end_value, const d
   return Translation2d{Lerp(X(), end_value.X(), t), Lerp(Y(), end_value.Y(), t)};
 }
 
+Translation2d Translation2d::operator+(const Translation2d& other) const {
+  return Translation2d{X() + other.X(), Y() + other.Y()};
+}
+
+Translation2d Translation2d::operator-(const Translation2d& other) const {
+  return Translation2d{X() - other.X(), Y() - other.Y()};
+}
+
+Translation2d Translation2d::operator*(const double scalar) const {
+  return Translation2d{X() * scalar, Y() * scalar};
+}
+
+Translation2d Translation2d::operator*(const Rotation2d& rotation) const {
+  return Translation2d{
+    x_ * rotation.Cos() - y_ * rotation.Sin(),
+    x_ * rotation.Sin() + y_ * rotation.Cos()
+  };
+}
+
+Translation2d Translation2d::operator/(const double scalar) const {
+  return Translation2d{X() / scalar, Y() / scalar};
+}
+
+Translation2d Translation2d::operator-() const { return Translation2d{-X(), -Y()}; }
+
+double Translation2d::X() const { return x_; }
+
+double Translation2d::Y() const { return y_; }
+
+double Translation2d::Norm() const { return std::hypot(x_, y_); }
+
 }  // namespace fl

src/main/cpp/fl/mathematics/geometry/Twist2d.cpp

@@ -0,0 +1,31 @@
+#include "fl/mathematics/geometry/Twist2d.h"
+
+namespace fl {
+
+Twist2d::Twist2d()
+  : dx_(0.0),
+    dy_(0.0),
+    dtheta_(0.0) {
+}
+
+Twist2d::Twist2d(const double dx, const double dy, const double dtheta)
+  : dx_(dx),
+    dy_(dy),
+    dtheta_(dtheta) {
+}
+
+Twist2d Twist2d::operator*(const double scalar) const {
+  return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
+}
+
+double Twist2d::Dx() const { return dx_; }
+
+double Twist2d::Dy() const { return dy_; }
+
+double Twist2d::Dtheta() const { return dtheta_; }
+
+double Twist2d::Norm() const {
+  if (dy_ == 0.0) return std::abs(dx_);
+  return std::hypot(dx_, dy_);
+}
+}

src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp

@@ -0,0 +1,99 @@
+#include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
+
+namespace fl {
+ParametricQuinticHermiteSpline::ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {
+  const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation());
+
+  x0_   = start.Translation().X();
+  x1_   = end.Translation().X();
+  dx0_  = scale_factor * start.Rotation().Cos();
+  dx1_  = scale_factor * end.Rotation().Cos();
+  ddx0_ = 0.;
+  ddx1_ = 0.;
+
+  y0_   = start.Translation().Y();
+  y1_   = end.Translation().Y();
+  dy0_  = scale_factor * start.Rotation().Sin();
+  dy1_  = scale_factor * end.Rotation().Sin();
+  ddy0_ = 0.;
+  ddy1_ = 0.;
+
+  ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_;
+  bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_;
+  cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_;
+  dx_ = 0.5 * ddx0_;
+  ex_ = dx0_;
+  fx_ = x0_;
+
+  ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_;
+  by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_;
+  cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
+  dy_ = 0.5 * ddy0_;
+  ey_ = dy0_;
+  fy_ = y0_;
+
+  start_ = start;
+  end_   = end;
+}
+
+const Pose2d& ParametricQuinticHermiteSpline::StartPose() const {
+  return start_;
+}
+
+const Pose2d& ParametricQuinticHermiteSpline::EndPose() const {
+  return end_;
+}
+
+Translation2d ParametricQuinticHermiteSpline::Point(const double t) const {
+  return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) +
+                           dx_ * std::pow(t, 2) + ex_ * t + fx_,
+                       ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) +
+                           dy_ * std::pow(t, 2) + ey_ * t + fy_};
+}
+
+Rotation2d ParametricQuinticHermiteSpline::Heading(const double t) const {
+  return {Dx(t), Dy(t), true};
+}
+
+double ParametricQuinticHermiteSpline::Curvature(const double t) const {
+  return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t));
+}
+
+double ParametricQuinticHermiteSpline::DCurvature(const double t) const {
+  const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t);
+  const auto num     = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 -
+                       3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
+  return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2));
+}
+
+double ParametricQuinticHermiteSpline::Velocity(const double t) const {
+  return std::hypot(Dx(t), Dy(t));
+}
+
+double ParametricQuinticHermiteSpline::Dx(const double t) const {
+  return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) +
+         2.0 * dx_ * t + ex_;
+}
+
+double ParametricQuinticHermiteSpline::Dy(const double t) const {
+  return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) +
+         2.0 * dy_ * t + ey_;
+}
+
+double ParametricQuinticHermiteSpline::Ddx(const double t) const {
+  return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_;
+}
+
+double ParametricQuinticHermiteSpline::Ddy(const double t) const {
+  return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_;
+}
+
+double ParametricQuinticHermiteSpline::Dddx(const double t) const {
+  return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_;
+}
+
+double ParametricQuinticHermiteSpline::Dddy(const double t) const {
+  return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_;
+}
+
+}  // namespace fl

src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp

@@ -0,0 +1,13 @@
+#include "fl/mathematics/spline/ParametricSpline.h"
+
+namespace fl {
+
+Pose2dWithCurvature ParametricSpline::PoseWithCurvature(const double t) const {
+  return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)};
+}
+
+Pose2d ParametricSpline::Pose(const double t) const {
+  return Pose2d{Point(t), Heading(t)};
+}
+
+}  // namespace fl

src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp

@@ -0,0 +1,54 @@
+#include "fl/mathematics/spline/SplineGenerator.h"
+
+namespace fl {
+std::vector<Pose2dWithCurvature> SplineGenerator::ParameterizeSpline(
+    const std::shared_ptr<ParametricSpline>& spline, const double max_dx, const double max_dy,
+    const double max_dtheta, const double t0, const double t1) {
+  const auto dt = t1 - t0;
+  auto       rv = std::vector<Pose2dWithCurvature>(static_cast<int>(kMinSampleSize / dt));
+
+  rv.push_back(spline->PoseWithCurvature(0));
+
+  for (double t = 0; t < t1; t += dt / kMinSampleSize) {
+    GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta);
+  }
+
+  return rv;
+}
+
+std::vector<Pose2dWithCurvature> SplineGenerator::ParameterizeSplines(
+    std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy,
+    const double max_dtheta) {
+  auto rv = std::vector<Pose2dWithCurvature>();
+  if (splines.empty()) return rv;
+
+  rv.push_back(splines[0]->PoseWithCurvature(0.0));
+  for (const auto& spline : splines) {
+    auto samples = ParameterizeSpline(spline, max_dx, max_dy, max_dtheta);
+    samples.erase(samples.begin());
+    rv.insert(rv.end(), samples.begin(), samples.end());
+  }
+
+  return rv;
+}
+
+void SplineGenerator::GetSegmentArc(const std::shared_ptr<ParametricSpline>& spline,
+                                    std::vector<Pose2dWithCurvature>* rv, const double t0, const double t1,
+                                    const double max_dx, const double max_dy, const double max_dtheta) {
+  const auto p0 = spline->Point(t0);
+  const auto p1 = spline->Point(t1);
+  const auto r0 = spline->Heading(t0);
+  const auto r1 = spline->Heading(t1);
+
+  const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0};
+  const auto twist          = Pose2d::ToTwist(transformation);
+
+  if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) {
+    GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta);
+    GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta);
+  } else {
+    rv->push_back(spline->PoseWithCurvature(t1));
+  }
+}
+
+}  // namespace fl

src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp

@@ -0,0 +1,48 @@
+#include "fl/mathematics/trajectory/TrajectoryGenerator.h"
+
+namespace fl {
+TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std::vector<Pose2d> waypoints,
+                                                                             const Constraints&  constraints,
+                                                                             const double
+                                                                             start_velocity,
+                                                                             const double end_velocity,
+                                                                             const double max_velocity,
+                                                                             const double
+                                                                             max_acceleration,
+                                                                             const bool reversed) {
+  const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
+
+  if (reversed) {
+    for (auto& waypoint : waypoints) {
+      waypoint = waypoint.TransformBy(flipped_position);
+    }
+  }
+
+  const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 2.0, 0.05, 0.1);
+
+  auto points = indexed_trajectory.Points();
+
+  if (reversed) {
+    for (auto& point : points) {
+      point =
+          Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()};
+    }
+  }
+
+  return TimeParameterizeTrajectory(DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
+                                    start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
+                                    reversed);
+}
+
+IndexedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::TrajectoryFromSplineWaypoints(
+  const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy, const double max_dtheta) {
+  std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
+  for (auto i = 1; i < waypoints.size(); ++i) {
+    splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
+  }
+  auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
+    SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
+
+  return trajectory;
+}
+}

src/main/cpp/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.cpp

@@ -0,0 +1,52 @@
+#include "fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h"
+
+namespace fl {
+
+AngularAccelerationConstraint::AngularAccelerationConstraint(double max_angular_acceleration)
+    : max_angular_acceleration_(max_angular_acceleration) {}
+
+double AngularAccelerationConstraint::MaxVelocity(const Pose2dWithCurvature& state) const {
+  /**
+   * We don't want v^2 * dk/ds alone to go over the max angular acceleration.
+   * v^2 * dk/ds = maxAngularAcceleration when linear acceleration = 0.
+   * v = sqrt(maxAngularAcceleration / dk/ds)
+   */
+
+  return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds()));
+}
+
+fl::MinMaxAcceleration AngularAccelerationConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state,
+                                                                         const double velocity) const {
+  /**
+   * We want to limit the acceleration such that we never go above the
+   *
+   * Angular acceleration = dw/dt     WHERE   w = omega = angular velocity
+   * w = v * k                        WHERE   v = linear velocity, k =
+   * curvature
+   *
+   * dw/dt = d/dt (v * k)
+   *
+   * By chain rule,
+   * dw/dt = dv/dt * k + v * dk/dt   [1]
+   *
+   * We don't have dk/dt, but we do have dk/ds and ds/dt
+   * dk/ds * ds/dt = dk/dt     [2]
+   *
+   * Substituting [2] in [1], we get
+   * dw/dt = acceleration * curvature + velocity * velocity * d_curvature
+   * WHERE acceleration = dv/dt, velocity = ds/dt, d_curvature = dk/dt and
+   * curvature = k
+   *
+   * We now want to find the linear acceleration such that the angular
+   * acceleration (dw/dt) never goes above the specified amount.
+   *
+   * acceleration * curvature = dw/dt - (velocity * velocity * d_curvature)
+   * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature
+   */
+
+  const auto max_absolute_acceleration =
+      std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature());
+
+  return fl::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration};
+}
+}  // namespace fl

src/main/cpp/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.cpp

@@ -0,0 +1,17 @@
+#include "fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
+
+namespace fl {
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+    const double max_centripetal_acceleration)
+    : max_centripetal_acceleration_(max_centripetal_acceleration) {}
+
+double CentripetalAccelerationConstraint::MaxVelocity(const Pose2dWithCurvature& state) const {
+  return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature()));
+}
+
+fl::MinMaxAcceleration CentripetalAccelerationConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state,
+                                                                             double velocity) const {
+  return kNoLimits;
+}
+}  // namespace fl

src/main/cpp/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.cpp

@@ -0,0 +1,20 @@
+#include "fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h"
+
+namespace fl {
+
+VelocityLimitRadiusConstraint::VelocityLimitRadiusConstraint(const Translation2d& point, const double radius,
+                                                             const double max_velocity)
+    : point_(point), radius_(radius), max_velocity_(max_velocity) {}
+
+double VelocityLimitRadiusConstraint::MaxVelocity(const Pose2dWithCurvature& state) const {
+  if (state.Pose().Translation().Distance(point_) < radius_) {
+    return max_velocity_;
+  }
+  return std::numeric_limits<double>::max();
+}
+
+fl::MinMaxAcceleration VelocityLimitRadiusConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state,
+                                                                         double velocity) const {
+  return kNoLimits;
+}
+}  // namespace fl

src/main/include/fl/mathematics/control/PurePursuitTracker.h

@@ -7,37 +7,12 @@
 namespace fl {
 class PurePursuitTracker : public TrajectoryTracker {
  public:
-  PurePursuitTracker(const double lat, const units::second_t lookahead_time, const double min_lookahead_distance)
+  PurePursuitTracker(const double lat, const units::second_t lookahead_time,
-      : lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {}
+                     const double min_lookahead_distance);
 
   TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
-                                                 const Pose2d& robot_pose) const override {
+                                                 const Pose2d&                             robot_pose) const
-    const auto reference_point = iterator.CurrentState();
+  override;
-
-    // Compute the lookahead state.
-    const auto lookahead_state = CalculateLookaheadPose(iterator, robot_pose);
-
-    // Find the appropriate lookahead point.
-    const auto lookahead_transform = lookahead_state.InFrameOfReferenceOf(robot_pose);
-
-    // Calculate latitude error.
-    const auto x_error =
-        reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X();
-
-    // Calculate the velocity at the reference point.
-    const double vd = units::unit_cast<double>(reference_point.state.Velocity());
-
-    // Calculate the distance from the robot to the lookahead.
-    const double l = lookahead_transform.Translation().Norm();
-
-    // Calculate the curvature of the arc that connects the robot and the lookahead point.
-    const double curvature = 2 * lookahead_transform.Translation().Y() / std::pow(l, 2);
-
-    // Adjust the linear velocity to compensate for the robot lagging behind.
-    const double adjusted_linear_velocity = vd * lookahead_transform.Rotation().Cos() + lat_ * x_error;
-
-    return {adjusted_linear_velocity, adjusted_linear_velocity * curvature};
-  }
 
  private:
   double lat_;
@@ -45,39 +20,6 @@ class PurePursuitTracker : public TrajectoryTracker {
   double min_lookahead_distance_;
 
   Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
-                                const Pose2d&                             robot_pose) const {
+                                const Pose2d&                             robot_pose) const;
-    auto lookahead_pose_by_time = iterator.Preview(lookahead_time_).state.State().Pose();
-
-    // The lookahead point is farther from the robot than the minimum lookahead distance.
-    // Therefore we can use this point.
-    if (lookahead_pose_by_time.InFrameOfReferenceOf(robot_pose).Translation().Norm() >=
-        min_lookahead_distance_) {
-      return lookahead_pose_by_time;
-    }
-
-    auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose();
-    auto previewed_time             = units::second_t(0.0);
-
-    // Run the loop until a distance that is greater than the minimum lookahead distance is found or until
-    // we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory.
-    while (iterator.RemainingProgress() > previewed_time) {
-      previewed_time += 0.02_s;
-      lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
-
-      const auto lookahead_distance =
-          lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose).Translation().Norm();
-
-      if (lookahead_distance > min_lookahead_distance_) {
-        return lookahead_pose_by_distance;
-      }
-    }
-
-    const auto remaining = min_lookahead_distance_ -
-                           (lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose)).Translation().Norm();
-
-    // Extend the trajectory
-    return lookahead_pose_by_distance.TransformBy(
-        Pose2d{Translation2d{remaining * (iterator.Reversed() ? -1.0 : 1.0), 0.0}, Rotation2d{}});
-  }
 };
 }  // namespace fl

src/main/include/fl/mathematics/control/RamseteTracker.h

@@ -5,24 +5,10 @@
 namespace fl {
 class RamseteTracker : public TrajectoryTracker {
  public:
-  RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {}
+  RamseteTracker(const double beta, const double zeta);
 
   TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
-                                                 const Pose2d& robot_pose) const override {
+                                                 const Pose2d& robot_pose) const override;
-    const TimedEntry<Pose2dWithCurvature> reference_state = iterator.CurrentState().state;
-    const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
-
-    const auto vd = units::unit_cast<double>(reference_state.Velocity());
-    const auto wd = vd * reference_state.State().Curvature();
-
-    const auto k1          = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd);
-    const auto angle_error = error.Rotation().Radians();
-
-    const auto linear  = vd * error.Rotation().Cos() + k1 * error.Translation().X();
-    const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error;
-
-    return {linear, angular};
-  }
 
  private:
   static double Sinc(const double theta) {

src/main/include/fl/mathematics/control/TrajectoryTracker.h

@@ -4,6 +4,7 @@
 
 #include <units.h>
 #include <memory>
+#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
 
 namespace fl {
 
@@ -21,53 +22,14 @@ struct TrajectoryTrackerOutput {
 
 class TrajectoryTracker {
  public:
-  void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
+  void                    Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory);
-    iterator_          = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
+  TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time);
-    previous_velocity_ = nullptr;
+  TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const;
-    previous_time_     = units::second_t(-1);
+  bool                                                   IsFinished() const;
-  }
-
-  TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time) {
-    if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
-    auto& iterator = *iterator_;
-
-    const auto dt = (units::unit_cast<double>(previous_time_) < 0.0) ? units::second_t{0.0}
-                                                                     : current_time - previous_time_;
-    previous_time_ = current_time;
-
-    iterator.Advance(dt);
-
-    const auto velocity = CalculateState(iterator, current_pose);
-
-    const auto linear_velocity  = velocity.linear_velocity;
-    const auto angular_velocity = velocity.angular_velocity;
-
-    if (previous_velocity_ == nullptr || dt <= units::second_t()) {
-      previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity});
-      return {linear_velocity, 0.0, angular_velocity, 0.0};
-    }
-
-    const auto _dt = units::unit_cast<double>(dt);
-
-    const TrajectoryTrackerOutput output{
-        linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
-        (angular_velocity - previous_velocity_->angular_velocity) / _dt};
-
-    previous_velocity_->linear_velocity  = linear_velocity;
-    previous_velocity_->angular_velocity = angular_velocity;
-
-    return output;
-  }
 
   virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
                                                          const Pose2d& robot_pose) const = 0;
 
-  TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const {
-    return iterator_->CurrentState();
-  }
-
-  bool IsFinished() const { return iterator_->IsDone(); }
-
  private:
   TimedIterator<Pose2dWithCurvature>*              iterator_ = nullptr;
   std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;

src/main/include/fl/mathematics/geometry/Rotation2d.h

@@ -8,26 +8,23 @@ class Rotation2d final {
  public:
   // Constructors
   Rotation2d();
-  Rotation2d(const double x, const double y, const bool normalize);
+  Rotation2d(double x, double y, bool normalize);
   explicit Rotation2d(const double value);
-  
+
-  static Rotation2d FromDegrees(const double degrees);
+  static Rotation2d FromDegrees(double degrees);
 
   // Operator Overloads
-  inline Rotation2d operator-(const Rotation2d& other) const { return *this + -other; }
+  Rotation2d operator-(const Rotation2d& other) const;
-  inline Rotation2d operator-() const { return Rotation2d(-value_); }
+  Rotation2d operator-() const;
 
-  inline Rotation2d operator+(const Rotation2d& other) const {
+  Rotation2d operator+(const Rotation2d& other) const;
-    return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
-                      true};
-  }
 
   // Accessors
-  inline double Radians() const { return value_; }
+  double Radians() const;
-  inline double Degrees() const { return Rad2Deg(value_); }
+  double Degrees() const;
-  inline double Cos() const { return cos_; }
+  double Cos() const;
-  inline double Sin() const { return sin_; }
+  double Sin() const;
-  inline double Tan() const { return sin_ / cos_; }
+  double Tan() const;
 
   bool IsParallel(const Rotation2d& other) const;
 

src/main/include/fl/mathematics/geometry/Translation2d.h

@@ -11,45 +11,32 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
  public:
   // Constructors
   Translation2d();
-  Translation2d(const double x, const double y);
+  Translation2d(double x, double y);
-  Translation2d(const double length, const Rotation2d& rotation);
+  Translation2d(double length, const Rotation2d& rotation);
 
   // Overriden Methods
-  inline double Distance(const Translation2d& other) const override {
+  double Distance(const Translation2d& other) const override;
-    return std::hypot(other.X() - X(), other.Y() - Y());
-  }
 
   Translation2d Interpolate(const Translation2d& end_value, const double t) const override;
 
   // Operator Overloads
-  inline Translation2d operator+(const Translation2d& other) const {
+  Translation2d operator+(const Translation2d& other) const;
-    return Translation2d{X() + other.X(), Y() + other.Y()};
-  }
 
-  inline Translation2d operator-(const Translation2d& other) const {
+  Translation2d operator-(const Translation2d& other) const;
-    return Translation2d{X() - other.X(), Y() - other.Y()};
-  }
 
-  inline Translation2d operator*(const double scalar) const {
+  Translation2d operator*(double scalar) const;
-    return Translation2d{X() * scalar, Y() * scalar};
-  }
 
-  inline Translation2d operator*(const Rotation2d& rotation) const {
+  Translation2d operator*(const Rotation2d& rotation) const;
-    return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(),
-                         x_ * rotation.Sin() + y_ * rotation.Cos()};
-  }
 
-  inline Translation2d operator/(const double scalar) const {
+  Translation2d operator/(double scalar) const;
-    return Translation2d{X() / scalar, Y() / scalar};
-  }
 
-  inline Translation2d operator-() const { return Translation2d{-X(), -Y()}; }
+  Translation2d operator-() const;
 
   // Accessors
-  inline double X() const { return x_; }
+  double X() const;
-  inline double Y() const { return y_; }
+  double Y() const;
 
-  inline double Norm() const { return std::hypot(x_, y_); }
+  double Norm() const;
 
  private:
   double x_;

src/main/include/fl/mathematics/geometry/Twist2d.h

@@ -6,21 +6,18 @@ namespace fl {
 class Twist2d {
  public:
   // Constructors
-  Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
+  Twist2d();
-  Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {}
+  Twist2d(const double dx, const double dy, const double dtheta);
 
   // Operator Overloads
-  Twist2d operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; }
+  Twist2d operator*(const double scalar) const;
 
   // Accessors
-  double Dx() const { return dx_; }
+  double Dx() const;
-  double Dy() const { return dy_; }
+  double Dy() const;
-  double Dtheta() const { return dtheta_; }
+  double Dtheta() const;
 
-  double Norm() const {
+  double Norm() const;
-    if (dy_ == 0.0) return std::abs(dx_);
-    return std::hypot(dx_, dy_);
-  }
 
  private:
   double dx_;

src/main/include/fl/mathematics/spline/ParametricQuinticHermiteSpline.h

@@ -5,65 +5,16 @@
 namespace fl {
 class ParametricQuinticHermiteSpline final : public ParametricSpline {
  public:
-  ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {
+  ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end);
-    const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation());
 
-    x0_   = start.Translation().X();
+  const Pose2d& StartPose() const;
-    x1_   = end.Translation().X();
+  const Pose2d& EndPose() const;
-    dx0_  = scale_factor * start.Rotation().Cos();
-    dx1_  = scale_factor * end.Rotation().Cos();
-    ddx0_ = 0.;
-    ddx1_ = 0.;
 
-    y0_   = start.Translation().Y();
+  Translation2d Point(const double t) const override;
-    y1_   = end.Translation().Y();
+  Rotation2d    Heading(const double t) const override;
-    dy0_  = scale_factor * start.Rotation().Sin();
+  double        Curvature(const double t) const override;
-    dy1_  = scale_factor * end.Rotation().Sin();
+  double        DCurvature(const double t) const override;
-    ddy0_ = 0.;
+  double        Velocity(const double t) const override;
-    ddy1_ = 0.;
-
-    ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_;
-    bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_;
-    cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_;
-    dx_ = 0.5 * ddx0_;
-    ex_ = dx0_;
-    fx_ = x0_;
-
-    ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_;
-    by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_;
-    cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
-    dy_ = 0.5 * ddy0_;
-    ey_ = dy0_;
-    fy_ = y0_;
-
-    start_ = start;
-    end_   = end;
-  }
-
-  const Pose2d& StartPose() const { return start_; }
-  const Pose2d& EndPose() const { return end_; }
-
-  Translation2d Point(const double t) const override {
-    return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) +
-                             dx_ * std::pow(t, 2) + ex_ * t + fx_,
-                         ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) +
-                             dy_ * std::pow(t, 2) + ey_ * t + fy_};
-  }
-
-  Rotation2d Heading(const double t) const override { return {Dx(t), Dy(t), true}; }
-
-  double Curvature(const double t) const override {
-    return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t));
-  }
-
-  double DCurvature(const double t) const override {
-    const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t);
-    const auto num     = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 -
-                     3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
-    return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2));
-  }
-
-  double Velocity(const double t) const override { return std::hypot(Dx(t), Dy(t)); }
 
  private:
   double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_;
@@ -75,33 +26,11 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
   Pose2d start_;
   Pose2d end_;
 
-  double Dx(const double t) const {
+  double Dx(const double t) const;
-    return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) +
+  double Dy(const double t) const;
-           2.0 * dx_ * t + ex_;
+  double Ddx(const double t) const;
-  }
+  double Ddy(const double t) const;
-
+  double Dddx(const double t) const;
-  double Dy(const double t) const {
+  double Dddy(const double t) const;
-    return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) +
-           2.0 * dy_ * t + ey_;
-  }
-
-  double Ddx(const double t) const {
-    return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_;
-  }
-
-  double Ddy(const double t) const {
-    return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_;
-  }
-
-  double Dddx(const double t) const { return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; }
-
-  double Dddy(const double t) const { return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; }
-
-  template <typename T>
-  static constexpr double BoundRadians(const T radians) {
-    while (radians >= kPi) radians -= 2 * kPi;
-    while (radians < kPi) radians += 2 * kPi;
-    return radians;
-  }
 };
 }  // namespace fl

src/main/include/fl/mathematics/spline/ParametricSpline.h

@@ -4,17 +4,17 @@
 namespace fl {
 class ParametricSpline {
  public:
+  virtual ~ParametricSpline() = default;
+
   virtual Translation2d Point(double t) const      = 0;
   virtual Rotation2d    Heading(double t) const    = 0;
   virtual double        Curvature(double t) const  = 0;
   virtual double        DCurvature(double t) const = 0;
   virtual double        Velocity(double t) const   = 0;
 
-  Pose2dWithCurvature PoseWithCurvature(const double t) const {
+  Pose2dWithCurvature PoseWithCurvature(const double t) const;
-    return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)};
-  }
 
  private:
-  Pose2d Pose(const double t) const { return Pose2d{Point(t), Heading(t)}; }
+  Pose2d Pose(const double t) const;
 };
 }  // namespace fl

src/main/include/fl/mathematics/spline/SplineGenerator.h

@@ -14,52 +14,14 @@ class SplineGenerator {
   static std::vector<Pose2dWithCurvature> ParameterizeSpline(const std::shared_ptr<ParametricSpline>& spline,
                                                              const double max_dx, const double max_dy,
                                                              const double max_dtheta, const double t0 = 0.0,
-                                                             const double t1 = 1.0) {
+                                                             const double t1 = 1.0);
-    const auto dt = t1 - t0;
-    auto       rv = std::vector<Pose2dWithCurvature>(static_cast<int>(kMinSampleSize / dt));
-
-    rv.push_back(spline->PoseWithCurvature(0));
-
-    for (double t = 0; t < t1; t += dt / kMinSampleSize) {
-      GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta);
-    }
-
-    return rv;
-  }
 
   static std::vector<Pose2dWithCurvature> ParameterizeSplines(
       std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy,
-      const double max_dtheta) {
+      const double max_dtheta);
-    auto rv = std::vector<Pose2dWithCurvature>();
-    if (splines.empty()) return rv;
-
-    rv.push_back(splines[0]->PoseWithCurvature(0.0));
-    for (const auto& spline : splines) {
-      auto samples = ParameterizeSpline(spline, max_dx, max_dy, max_dtheta);
-      samples.erase(samples.begin());
-      rv.insert(rv.end(), samples.begin(), samples.end());
-    }
-
-    return rv;
-  }
 
   static void GetSegmentArc(const std::shared_ptr<ParametricSpline>& spline,
                             std::vector<Pose2dWithCurvature>* rv, const double t0, const double t1,
-                            const double max_dx, const double max_dy, const double max_dtheta) {
+                            const double max_dx, const double max_dy, const double max_dtheta);
-    const auto p0 = spline->Point(t0);
-    const auto p1 = spline->Point(t1);
-    const auto r0 = spline->Heading(t0);
-    const auto r1 = spline->Heading(t1);
-
-    const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0};
-    const auto twist          = Pose2d::ToTwist(transformation);
-
-    if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) {
-      GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta);
-      GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta);
-    } else {
-      rv->push_back(spline->PoseWithCurvature(t1));
-    }
-  }
 };
 }  // namespace fl

src/main/include/fl/mathematics/trajectory/Trajectory.h

@@ -1,8 +1,8 @@
 #pragma once
 
+#include <memory>
 #include <utility>
 #include <vector>
-#include <memory>
 
 namespace fl {
 
@@ -18,7 +18,6 @@ struct TrajectorySamplePoint {
   int index_floor;
   int index_ceil;
 
- public:
   explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
       : state(point.state), index_floor(point.index), index_ceil(point.index) {}
 

src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h

@@ -17,45 +17,13 @@ class TrajectoryGenerator {
 
  public:
   static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
-      std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
+    std::vector<Pose2d> waypoints, const Constraints& constraints, const double  start_velocity,
-      const double end_velocity, const double max_velocity, const double max_acceleration,
+    const double        end_velocity, const double    max_velocity, const double max_acceleration,
-      const bool reversed) {
+    const bool          reversed);
-    const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
-
-    if (reversed) {
-      for (auto& waypoint : waypoints) {
-        waypoint = waypoint.TransformBy(flipped_position);
-      }
-    }
-
-    const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 2.0, 0.05, 0.1);
-
-    auto points = indexed_trajectory.Points();
-
-    if (reversed) {
-      for (auto& point : points) {
-        point =
-            Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()};
-      }
-    }
-
-    return TimeParameterizeTrajectory(DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
-                                      start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
-                                      reversed);
-  }
 
   static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
-      const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
+    const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
-      const double max_dtheta) {
+    const double               max_dtheta);
-    std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
-    for (auto i = 1; i < waypoints.size(); ++i) {
-      splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
-    }
-    auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
-        SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
-
-    return trajectory;
-  }
 
   template <typename S>
   struct ConstrainedPose {

src/main/include/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h

@@ -7,56 +7,10 @@ namespace fl {
 
 class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
  public:
-  explicit AngularAccelerationConstraint(double max_angular_acceleration)
+  explicit AngularAccelerationConstraint(double max_angular_acceleration);
-      : max_angular_acceleration_(max_angular_acceleration) {}
 
-  ~AngularAccelerationConstraint() = default;
+  double                 MaxVelocity(const Pose2dWithCurvature& state) const override;
-
+  fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override;
-  double MaxVelocity(const Pose2dWithCurvature& state) const override {
-    /**
-     * We don't want v^2 * dk/ds alone to go over the max angular acceleration.
-     * v^2 * dk/ds = maxAngularAcceleration when linear acceleration = 0.
-     * v = sqrt(maxAngularAcceleration / dk/ds)
-     */
-
-    return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds()));
-  }
-
-  fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
-                                            double                     velocity) const override {
-    /**
-     * We want to limit the acceleration such that we never go above the
-     * specified angular acceleration.
-     *
-     * Angular acceleration = dw/dt     WHERE   w = omega = angular velocity
-     * w = v * k                        WHERE   v = linear velocity, k =
-     * curvature
-     *
-     * dw/dt = d/dt (v * k)
-     *
-     * By chain rule,
-     * dw/dt = dv/dt * k + v * dk/dt   [1]
-     *
-     * We don't have dk/dt, but we do have dk/ds and ds/dt
-     * dk/ds * ds/dt = dk/dt     [2]
-     *
-     * Substituting [2] in [1], we get
-     * dw/dt = acceleration * curvature + velocity * velocity * d_curvature
-     * WHERE acceleration = dv/dt, velocity = ds/dt, d_curvature = dk/dt and
-     * curvature = k
-     *
-     * We now want to find the linear acceleration such that the angular
-     * acceleration (dw/dt) never goes above the specified amount.
-     *
-     * acceleration * curvature = dw/dt - (velocity * velocity * d_curvature)
-     * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature
-     */
-
-    const auto max_absolute_acceleration =
-        std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature());
-
-    return fl::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration};
-  }
 
  private:
   double max_angular_acceleration_;

src/main/include/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h

@@ -7,19 +7,10 @@ namespace fl {
 
 class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
  public:
-  explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration)
+  explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration);
-      : max_centripetal_acceleration_(max_centripetal_acceleration) {}
 
-  ~CentripetalAccelerationConstraint() = default;
+  double                 MaxVelocity(const Pose2dWithCurvature& state) const override;
-
+  fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override;
-  double MaxVelocity(const Pose2dWithCurvature& state) const override {
-    return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature()));
-  }
-
-  fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
-                                            double                     velocity) const override {
-    return kNoLimits;
-  }
 
  private:
   double max_centripetal_acceleration_;

src/main/include/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h

@@ -7,22 +7,10 @@ namespace fl {
 
 class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> {
  public:
-  VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity)
+  VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity);
-      : point_(point), radius_(radius), max_velocity_(max_velocity) {}
 
-  ~VelocityLimitRadiusConstraint() = default;
+  double                 MaxVelocity(const Pose2dWithCurvature& state) const override;
-
+  fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override;
-  double MaxVelocity(const Pose2dWithCurvature& state) const override {
-    if (state.Pose().Translation().Distance(point_) < radius_) {
-      return max_velocity_;
-    }
-    return std::numeric_limits<double>::max();
-  }
-
-  fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
-                                            double                     velocity) const override {
-    return kNoLimits;
-  }
 
  private:
   Translation2d point_;