Run autoformat

.clang-format

@@ -18,8 +18,8 @@ AlwaysBreakAfterDefinitionReturnType: None
 AlwaysBreakAfterReturnType: None
 AlwaysBreakBeforeMultilineStrings: true
 AlwaysBreakTemplateDeclarations: true
-BinPackArguments: true
+BinPackArguments: false
-BinPackParameters: true
+BinPackParameters: false
 BraceWrapping:
   AfterClass:      false
   AfterControlStatement: false

FalconLibraryCPP/src/mathematics/geometry/Pose2d.h

@@ -82,8 +82,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
         pose.translation_ *
         Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
 
-    return Twist2d{translation_part.X(), translation_part.Y(),
+    return Twist2d{
-                   pose.rotation_.Radians()};
+        translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
   }
 
   static Pose2d FromTwist(const Twist2d& twist) {

FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h

@@ -38,7 +38,8 @@ class Rotation2d final {
 
   Rotation2d operator+(const Rotation2d& other) const {
     return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
-                      Cos() * other.Sin() + Sin() * other.Cos(), true};
+                      Cos() * other.Sin() + Sin() * other.Cos(),
+                      true};
   }
 
   // Accessors

FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h

@@ -45,9 +45,11 @@ class DistanceTrajectory : public Trajectory<double, S> {
           return TrajectorySamplePoint<S>(s, i, i);
         }
         return TrajectorySamplePoint<S>(
-            prev_s.Interpolate(s, (interpolant - distances_[i - 1]) /
+            prev_s.Interpolate(s,
-                                      (distances_[i] - distances_[i - 1])),
+                               (interpolant - distances_[i - 1]) /
-            i - 1, i);
+                                   (distances_[i] - distances_[i - 1])),
+            i - 1,
+            i);
       }
     }
     throw - 1;

FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h

@@ -7,18 +7,16 @@ namespace frc5190 {
 template <typename S>
 class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
  public:
-  TimedEntry(const S& state, const double t, const double velocity,
+  TimedEntry(const S& state,
+             const double t,
+             const double velocity,
              const double acceleration)
       : state_(state),
         t_(t),
         velocity_(velocity),
         acceleration_(acceleration) {}
 
-  TimedEntry()
+  TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
-    : t_(0),
-      velocity_(0),
-      acceleration_(0) {
-  }
 
   TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
                             double t) const override {
@@ -38,7 +36,9 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
     return TimedEntry{
         state_.Interpolate(end_value.state_,
                            new_s / state_.Distance(end_value.state_)),
-        new_t, new_v, acceleration_};
+        new_t,
+        new_v,
+        acceleration_};
   }
 
   double Distance(const TimedEntry<S>& other) const override {
@@ -100,7 +100,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
             prev_s.state.Interpolate(s.state,
                                      (interpolant - prev_s.state.T()) /
                                          (s.state.T() - prev_s.state.T())),
-            i - 1, i);
+            i - 1,
+            i);
       }
     }
     throw - 1;

FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h

@@ -13,24 +13,20 @@ struct TrajectoryPoint {
 
 template <typename S>
 struct TrajectorySamplePoint {
-
   S state;
   int index_floor;
   int index_ceil;
 
  public:
   explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
-      : state(point.state),
+      : state(point.state), index_floor(point.index), index_ceil(point.index) {}
-        index_floor(point.index),
-        index_ceil(point.index) {}
 
   TrajectorySamplePoint(S state, int index_floor, int index_ceil)
-      : state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
+      : state(std::move(state)),
+        index_floor(index_floor),
+        index_ceil(index_ceil) {}
 
-  TrajectorySamplePoint()
+  TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
-    : index_floor(0),
-      index_ceil(0) {
-  }
 };
 
 template <typename U, typename S>

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <array>
 #include "../geometry/Pose2dWithCurvature.h"
 #include "../spline/ParametricQuinticHermiteSpline.h"
 #include "../spline/SplineGenerator.h"
@@ -16,8 +15,11 @@ class TrajectoryGenerator {
   static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
       std::vector<Pose2d> waypoints,
       const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
-      const double start_velocity, const double end_velocity,
+      const double start_velocity,
-      const double max_velocity, const double max_acceleration, 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)};
 
@@ -35,19 +37,27 @@ class TrajectoryGenerator {
     if (reversed) {
       for (auto& point : points) {
         point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
-                                    -point.Curvature(), point.Dkds()};
+                                    -point.Curvature(),
+                                    point.Dkds()};
       }
     }
 
     return TimeParameterizeTrajectory(
-        DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
+        DistanceTrajectory<Pose2dWithCurvature>(points),
-        start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
+        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 std::vector<Pose2d>& waypoints,
-      const double max_dy, const double max_dtheta) {
+      const double max_dx,
+      const double max_dy,
+      const double max_dtheta) {
     auto size = static_cast<int>(waypoints.size());
     std::vector<ParametricSpline*> splines(size - 1);
     for (int i = 1; i < waypoints.size(); ++i) {
@@ -55,8 +65,8 @@ class TrajectoryGenerator {
           new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
     }
     auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
-        SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
+        SplineGenerator::ParameterizeSplines(
-                                             max_dtheta));
+            splines, max_dx, max_dy, max_dtheta));
 
     for (auto ptr : splines) {
       delete ptr;
@@ -76,7 +86,8 @@ class TrajectoryGenerator {
 
   template <typename S>
   static void EnforceAccelerationLimits(
-      bool reverse, std::vector<TimingConstraint<S>*> constraints,
+      bool reverse,
+      std::vector<TimingConstraint<S>*> constraints,
       ConstrainedPose<S>* constrained_pose) {
     for (const auto& constraint : constraints) {
       auto min_max_accel = constraint->MinMaxAcceleration(
@@ -101,8 +112,12 @@ class TrajectoryGenerator {
   static TimedTrajectory<S> TimeParameterizeTrajectory(
       DistanceTrajectory<S> distance_trajectory,
       std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
-      double start_velocity, double end_velocity, double max_velocity,
+      double start_velocity,
-      double max_acceleration, double step_size, bool reversed) {
+      double end_velocity,
+      double max_velocity,
+      double max_acceleration,
+      double step_size,
+      bool reversed) {
     const auto num_states = static_cast<int>(
         std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
 
@@ -126,8 +141,8 @@ class TrajectoryGenerator {
 
     std::vector<ConstrainedPose<S>> constrained_poses(num_states);
 
-    auto _predecessor = ConstrainedPose<S>{states[0], 0.0, start_velocity,
+    auto _predecessor = ConstrainedPose<S>{
-                                           -max_acceleration, max_acceleration};
+        states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
     ConstrainedPose<S>* predecessor = &_predecessor;
 
     for (int i = 0; i < states.size(); ++i) {
@@ -196,7 +211,9 @@ class TrajectoryGenerator {
     auto _successor =
         ConstrainedPose<S>{states[states.size() - 1],
                            constrained_poses[states.size() - 1].distance,
-                           end_velocity, -max_acceleration, max_acceleration};
+                           end_velocity,
+                           -max_acceleration,
+                           max_acceleration};
     ConstrainedPose<S>* successor = &_successor;
 
     for (int i = states.size() - 1; i >= 0; --i) {
@@ -268,9 +285,10 @@ class TrajectoryGenerator {
 
       v = constrained_pose.max_velocity;
       s = constrained_pose.distance;
-      timed_states[i] =
+      timed_states[i] = TimedEntry<S>{constrained_pose.state,
-          TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v,
+                                      t,
-                        reversed ? -accel : accel};
+                                      reversed ? -v : v,
+                                      reversed ? -accel : accel};
 
       t += dt;
     }

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h

@@ -19,17 +19,17 @@ class TrajectoryIterator {
   }
 
   TrajectorySamplePoint<S> Advance(U amount) {
-    progress_ =
+    progress_ = Clamp(Addition(progress_, amount),
-        Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
+                      trajectory_->FirstInterpolant(),
-              trajectory_->LastInterpolant());
+                      trajectory_->LastInterpolant());
     sample_ = trajectory_->Sample(progress_);
     return sample_;
   }
 
   TrajectorySamplePoint<S> Preview(U amount) {
-    auto progress =
+    auto progress = Clamp(Addition(progress_, amount),
-        Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
+                          trajectory_->FirstInterpolant(),
-              trajectory_->LastInterpolant());
+                          trajectory_->LastInterpolant());
     return trajectory_->Sample(progress);
   }
 

FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h

@@ -13,7 +13,6 @@ class AngularAccelerationConstraint final
 
   ~AngularAccelerationConstraint() = default;
 
-
   double MaxVelocity(const Pose2dWithCurvature& state) const override {
     /**
      * We don't want v^2 * dk/ds alone to go over the max angular acceleration.

FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h

@@ -7,7 +7,6 @@ namespace frc5190 {
 
 class CentripetalAccelerationConstraint final
     : public TimingConstraint<Pose2dWithCurvature> {
-
  public:
   explicit CentripetalAccelerationConstraint(
       const double max_centripetal_acceleration)

FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h

@@ -8,7 +8,8 @@ namespace frc5190 {
 class VelocityLimitRadiusConstraint
     : public TimingConstraint<Pose2dWithCurvature> {
  public:
-  VelocityLimitRadiusConstraint(const Translation2d& point, const double radius,
+  VelocityLimitRadiusConstraint(const Translation2d& point,
+                                const double radius,
                                 const double max_velocity)
       : point_(point), radius_(radius), max_velocity_(max_velocity) {}
 

FalconLibraryCPP/src/types/Interpolatable.h

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

Tests/trajectory-tests.cpp

@@ -1,6 +1,5 @@
 #include "pch.h"
 #include "../FalconLibraryCPP/src/FalconLibrary.h"
-#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
 
 constexpr double kTestEpsilon = 1E-6;
 
@@ -8,25 +7,30 @@ class TrajectoryTest : public ::testing::Test {
  public:
   TrajectoryTest() {}
 
-  void Run(frc5190::Pose2d initial, frc5190::Pose2d final,
+  void Run(frc5190::Pose2d initial,
-           double max_velocity = 3, double max_acceleration = 2,
+           frc5190::Pose2d final,
+           double max_velocity = 3,
+           double max_acceleration = 2,
            bool backwards = false) {
     auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
         std::vector<frc5190::Pose2d>{initial, final},
-        std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{
+        std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{},
-            new frc5190::CentripetalAccelerationConstraint{100.0}},
+        0.0,
-        0.0, 0.0, max_velocity, max_acceleration, backwards);
+        0.0,
+        max_velocity,
+        max_acceleration,
+        backwards);
 
     auto pose = trajectory.Sample(0.0).state.State().Pose();
 
     EXPECT_FALSE(false);
 
-    EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(),
+    EXPECT_NEAR(
-                kTestEpsilon);
+        pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
-    EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(),
+    EXPECT_NEAR(
-                kTestEpsilon);
+        pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
-    EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(),
+    EXPECT_NEAR(
-                kTestEpsilon);
+        pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
 
     const auto iterator = trajectory.Iterator();
 
@@ -51,8 +55,8 @@ 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(pose1.Rotation().Radians(), final.Rotation().Radians(),
+    EXPECT_NEAR(
-                kTestEpsilon);
+        pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
   }
 };