run clang format

src/bench/bench_vector_creation.cpp

@@ -24,8 +24,8 @@ struct ConstrainedPose {
   }
 
   ConstrainedPose(ConstrainedPose& other) = default;
-  ConstrainedPose& operator=(ConstrainedPose& other) {
+  ConstrainedPose& operator               =(ConstrainedPose& other) {
-    //std::cout << "Copy operator called";
+    // std::cout << "Copy operator called";
     this->state            = other.state;
     this->distance         = other.distance;
     this->max_velocity     = other.max_velocity;

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

@@ -13,9 +13,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d&         curre
   if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
   auto& iterator = *iterator_;
 
-  const auto dt = (units::unit_cast<double>(previous_time_) < 0.0)
+  const auto dt =
-                    ? units::second_t{0.0}
+      (units::unit_cast<double>(previous_time_) < 0.0) ? units::second_t{0.0} : current_time - previous_time_;
-                    : current_time - previous_time_;
   previous_time_ = current_time;
 
   iterator.Advance(dt);
@@ -33,9 +32,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d&         curre
   const auto _dt = units::unit_cast<double>(dt);
 
   const TrajectoryTrackerOutput output{
-    linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
+      linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
-    (angular_velocity - previous_velocity_->angular_velocity) / _dt
+      (angular_velocity - previous_velocity_->angular_velocity) / _dt};
-  };
 
   previous_velocity_->linear_velocity  = linear_velocity;
   previous_velocity_->angular_velocity = angular_velocity;
@@ -47,5 +45,7 @@ TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> TrajectoryTracker::Refere
   return iterator_->CurrentState();
 }
 
-bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); }
+bool TrajectoryTracker::IsFinished() const {
+  return iterator_->IsDone();
 }
+}  // namespace fl

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

@@ -25,26 +25,38 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) {
   return Rotation2d(Deg2Rad(degrees));
 }
 
-Rotation2d Rotation2d::operator-(const Rotation2d& other) const { return *this + -other; }
+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_; }
+Rotation2d Rotation2d::operator-() const {
+  return Rotation2d(-value_);
+}
 
-double Rotation2d::Degrees() const { return Rad2Deg(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::Cos() const { return cos_; }
+double Rotation2d::Radians() const {
+  return value_;
+}
 
-double Rotation2d::Sin() const { return sin_; }
+double Rotation2d::Degrees() const {
+  return Rad2Deg(value_);
+}
 
-double Rotation2d::Tan() const { return sin_ / cos_; }
+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

@@ -34,22 +34,27 @@ Translation2d Translation2d::operator*(const double scalar) const {
 }
 
 Translation2d Translation2d::operator*(const Rotation2d& rotation) const {
-  return Translation2d{
+  return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), x_ * rotation.Sin() + y_ * rotation.Cos()};
-    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()}; }
+Translation2d Translation2d::operator-() const {
+  return Translation2d{-X(), -Y()};
+}
 
-double Translation2d::X() const { return x_; }
+double Translation2d::X() const {
+  return x_;
+}
 
-double Translation2d::Y() const { return y_; }
+double Translation2d::Y() const {
+  return y_;
+}
 
-double Translation2d::Norm() const { return std::hypot(x_, y_); }
+double Translation2d::Norm() const {
+  return std::hypot(x_, y_);
+}
 
 }  // namespace fl

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

@@ -2,30 +2,28 @@
 
 namespace fl {
 
-Twist2d::Twist2d()
+Twist2d::Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
-  : dx_(0.0),
-    dy_(0.0),
-    dtheta_(0.0) {
-}
 
-Twist2d::Twist2d(const double dx, const double dy, const double dtheta)
+Twist2d::Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(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::Dx() const {
+  return dx_;
+}
 
-double Twist2d::Dy() const { return dy_; }
+double Twist2d::Dy() const {
+  return dy_;
+}
 
-double Twist2d::Dtheta() const { return dtheta_; }
+double Twist2d::Dtheta() const {
+  return dtheta_;
+}
 
 double Twist2d::Norm() const {
   if (dy_ == 0.0) return std::abs(dx_);
   return std::hypot(dx_, dy_);
 }
-}
+}  // namespace fl

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

@@ -62,7 +62,7 @@ double ParametricQuinticHermiteSpline::Curvature(const double t) const {
 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));
+                   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));
 }
 

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

@@ -1,15 +1,10 @@
 #include "fl/mathematics/trajectory/TrajectoryGenerator.h"
 
 namespace fl {
-TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std::vector<Pose2d> waypoints,
+TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(
-                                                                             const Constraints&  constraints,
+    std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
-                                                                             const double
+    const double end_velocity, const double max_velocity, const double max_acceleration,
-                                                                             start_velocity,
+    const bool reversed) {
-                                                                             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) {
@@ -35,14 +30,14 @@ TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std
 }
 
 IndexedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::TrajectoryFromSplineWaypoints(
-  const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy, const double max_dtheta) {
+    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));
+      SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
 
   return trajectory;
 }
-}
+}  // namespace fl

src/main/include/FalconLibrary.h

@@ -8,9 +8,9 @@
 #include "fl/mathematics/geometry/Translation2d.h"
 #include "fl/mathematics/geometry/Twist2d.h"
 
+#include "fl/mathematics/control/PurePursuitTracker.h"
 #include "fl/mathematics/control/RamseteTracker.h"
 #include "fl/mathematics/control/TrajectoryTracker.h"
-#include "fl/mathematics/control/PurePursuitTracker.h"
 
 #include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
 #include "fl/mathematics/spline/ParametricSpline.h"

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

@@ -11,13 +11,12 @@ class PurePursuitTracker : public TrajectoryTracker {
                      const double min_lookahead_distance);
 
   TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
-                                                 const Pose2d&                             robot_pose) const
+                                                 const Pose2d& robot_pose) const override;
-  override;
 
  private:
-  double lat_;
+  double          lat_;
   units::second_t lookahead_time_;
-  double min_lookahead_distance_;
+  double          min_lookahead_distance_;
 
   Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
                                 const Pose2d&                             robot_pose) const;

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

@@ -28,12 +28,12 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
   // Member Functions
   Pose2d Mirror() const;
   Pose2d TransformBy(const Pose2d& other) const;
-  bool IsCollinear(const Pose2d& other) const;
+  bool   IsCollinear(const Pose2d& other) const;
   Pose2d InFrameOfReferenceOf(const Pose2d& other) const;
 
   // Static Methods
   static Twist2d ToTwist(const Pose2d& pose);
-  static Pose2d FromTwist(const Twist2d& twist);
+  static Pose2d  FromTwist(const Twist2d& twist);
 
  private:
   Translation2d translation_;

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

@@ -28,8 +28,7 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
 
     if (delta_t < 0_s) return end_value.Interpolate(*this, 1.0 - t);
 
-    auto reversing = velocity_ < 0_mps ||
+    auto reversing = velocity_ < 0_mps || EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
-                     EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
 
     units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t;
     units::meter_t             new_s =

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

@@ -17,13 +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);
 
   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);
 
   template <typename S>
   struct ConstrainedPose {

src/main/include/fl/types/Interpolatable.h

@@ -9,7 +9,7 @@ class Interpolatable {
   virtual ~Interpolatable()                                 = default;
   virtual T Interpolate(const T& end_value, double t) const = 0;
 
-  template<typename T>
+  template <typename T>
   static constexpr T Lerp(const T& start_value, const T& end_value, const double t) {
     return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
   }

src/test/cpp/trajectory-tests.cpp

@@ -7,30 +7,20 @@ class TrajectoryTest : public ::testing::Test {
  public:
   TrajectoryTest() {}
 
-  void Run(fl::Pose2d initial,
+  void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2,
-           fl::Pose2d final,
-           double max_velocity = 3,
-           double max_acceleration = 2,
            bool backwards = false) {
     auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
         std::vector<fl::Pose2d>{initial, final},
-        std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{},
+        std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
-        0.0,
+        max_acceleration, backwards);
-        0.0,
-        max_velocity,
-        max_acceleration,
-        backwards);
 
     auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose();
 
     EXPECT_FALSE(false);
 
-    EXPECT_NEAR(
+    EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
-        pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
+    EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
-    EXPECT_NEAR(
+    EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
-        pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
-    EXPECT_NEAR(
-        pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
 
     const auto iterator = trajectory.Iterator();
 
@@ -38,7 +28,7 @@ class TrajectoryTest : public ::testing::Test {
 
     while (!iterator->IsDone()) {
       auto prev_sample = sample;
-      sample = iterator->Advance(0.02_s);
+      sample           = iterator->Advance(0.02_s);
 
       EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Velocity())), max_velocity + kTestEpsilon);
       EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Acceleration())),
@@ -55,8 +45,7 @@ class TrajectoryTest : public ::testing::Test {
 
     EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
     EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
-    EXPECT_NEAR(
+    EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
-        pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
   }
 };