Use better formatting options, clean up iterator

.clang-format

@@ -1,107 +1,9 @@
----
-Language:        Cpp
 BasedOnStyle: Google
-AccessModifierOffset: -1
+AlignOperands: 'true'
-AlignAfterOpenBracket: Align
+ColumnLimit: '110'
-AlignConsecutiveAssignments: false
+NamespaceIndentation: Inner
-AlignConsecutiveDeclarations: false
+TabWidth: '2'
-AlignEscapedNewlines: Left
-AlignOperands:   true
-AlignTrailingComments: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: true
-AllowShortLoopsOnASingleLine: true
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: true
-BinPackArguments: false
-BinPackParameters: false
-BraceWrapping:
-  AfterClass:      false
-  AfterControlStatement: false
-  AfterEnum:       false
-  AfterFunction:   false
-  AfterNamespace:  false
-  AfterObjCDeclaration: false
-  AfterStruct:     false
-  AfterUnion:      false
-  BeforeCatch:     false
-  BeforeElse:      false
-  IndentBraces:    false
-  SplitEmptyFunction: true
-  SplitEmptyRecord: true
-  SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeInheritanceComma: false
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit:     80
-CommentPragmas:  '^ IWYU pragma:'
-CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: true
-DerivePointerAlignment: false
-DisableFormat:   false
-ExperimentalAutoDetectBinPacking: false
-FixNamespaceComments: true
-ForEachMacros:
-  - foreach
-  - Q_FOREACH
-  - BOOST_FOREACH
-IncludeCategories:
-  - Regex:           '^<.*\.h>'
-    Priority:        1
-  - Regex:           '^<.*'
-    Priority:        2
-  - Regex:           '.*'
-    Priority:        3
-IncludeIsMainRegex: '([-_](test|unittest))?$'
-IndentCaseLabels: true
-IndentWidth:     2
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: false
-MacroBlockBegin: ''
-MacroBlockEnd:   ''
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBlockIndentWidth: 2
-ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: false
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 1
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 200
-PointerAlignment: Left
-ReflowComments:  true
-SortIncludes:    false
-SortUsingDeclarations: true
-SpaceAfterCStyleCast: false
-SpaceAfterTemplateKeyword: true
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeParens: ControlStatements
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 2
-SpacesInAngles:  false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-Standard:        Auto
-TabWidth:        8
 UseTab: Never
-...
+AllowShortFunctionsOnASingleLine: Inline
+AlignConsecutiveAssignments: 'true'
+AlignConsecutiveDeclarations: 'true'

FalconLibraryCPP.sln.DotSettings.user

@@ -1,4 +1,5 @@
 <wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=dtheta/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean>
+	<s:Boolean x:Key="/Default/UserDictionary/Words/=waypoint/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>

FalconLibraryCPP/src/FalconLibrary.h

@@ -9,8 +9,8 @@
 #include "mathematics/geometry/Translation2d.h"
 #include "mathematics/geometry/Twist2d.h"
 
-#include "mathematics/spline/ParametricSpline.h"
 #include "mathematics/spline/ParametricQuinticHermiteSpline.h"
+#include "mathematics/spline/ParametricSpline.h"
 #include "mathematics/spline/SplineGenerator.h"
 
 #include "mathematics/trajectory/TrajectoryGenerator.h"

FalconLibraryCPP/src/mathematics/geometry/Pose2d.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include "Translation2d.h"
-#include "Rotation2d.h"
-#include "Twist2d.h"
 #include "../../Utilities.h"
+#include "Rotation2d.h"
+#include "Translation2d.h"
+#include "Twist2d.h"
 
 namespace frc5190 {
 class Twist2d;
@@ -19,9 +19,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
       : translation_(Translation2d(x, y)), rotation_(rotation) {}
 
   // Overriden Methods
-  double Distance(const Pose2d& other) const override {
+  double Distance(const Pose2d& other) const override { return ToTwist(-*this + other).Norm(); }
-    return ToTwist(-*this + other).Norm();
-  }
   Pose2d Interpolate(const Pose2d& end_value, const double t) const override {
     if (t <= 0) {
       return *this;
@@ -46,13 +44,11 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
   const Rotation2d&    Rotation() const { return rotation_; }
 
   Pose2d Mirror() const {
-    return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()},
+    return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, -rotation_};
-                  -rotation_};
   }
 
   Pose2d TransformBy(const Pose2d& other) const {
-    return Pose2d{translation_ + (other.translation_ * rotation_),
+    return Pose2d{translation_ + (other.translation_ * rotation_), rotation_ + other.rotation_};
-                  rotation_ + other.rotation_};
   }
 
   bool IsCollinear(const Pose2d& other) const {
@@ -74,16 +70,13 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
     if (std::abs(cos_minus_one) < kEpsilon) {
       half_theta_by_tan_of_half_dtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
     } else {
-      half_theta_by_tan_of_half_dtheta =
+      half_theta_by_tan_of_half_dtheta = -(half_dtheta * pose.rotation_.Sin()) / cos_minus_one;
-          -(half_dtheta * pose.rotation_.Sin()) / cos_minus_one;
     }
 
     const auto translation_part =
-        pose.translation_ *
+        pose.translation_ * Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
-        Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
 
-    return Twist2d{
+    return Twist2d{translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
-        translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
   }
 
   static Pose2d FromTwist(const Twist2d& twist) {
@@ -101,8 +94,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
       c = (1 - cos_theta) / dtheta;
     }
 
-    return Pose2d{Translation2d{dx * s - dy * c, dx * c + dy * s},
+    return Pose2d{Translation2d{dx * s - dy * c, dx * c + dy * s}, Rotation2d{cos_theta, sin_theta, false}};
-                  Rotation2d{cos_theta, sin_theta, false}};
   }
 
  private:

FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h

@@ -4,8 +4,7 @@
 #include "Pose2d.h"
 
 namespace frc5190 {
-class Pose2dWithCurvature final
+class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
-    : public VaryInterpolatable<Pose2dWithCurvature> {
  public:
   // Constructors
   Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
@@ -14,15 +13,11 @@ class Pose2dWithCurvature final
   Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {}
 
   // Overriden Methods
-  double Distance(const Pose2dWithCurvature& other) const override {
+  double Distance(const Pose2dWithCurvature& other) const override { return pose_.Distance(other.pose_); }
-    return pose_.Distance(other.pose_);
-  }
 
-  Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value,
+  Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, double t) const override {
-                                  double t) const override {
     return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t),
-                               Lerp(curvature_, end_value.curvature_, t),
+                               Lerp(curvature_, end_value.curvature_, t), Lerp(dkds_, end_value.dkds_, t)};
-                               Lerp(dkds_, end_value.dkds_, t)};
   }
 
   // Operator Overloads
@@ -35,9 +30,7 @@ class Pose2dWithCurvature final
   double        Curvature() const { return curvature_; }
   double        Dkds() const { return dkds_; }
 
-  Pose2dWithCurvature Mirror() const {
+  Pose2dWithCurvature Mirror() const { return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_}; }
-    return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_};
-  }
 
  private:
   Pose2d pose_;

FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h

@@ -8,8 +8,7 @@ class Rotation2d final {
  public:
   // Constructors
   Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {}
-  explicit Rotation2d(const double value)
+  explicit Rotation2d(const double value) : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {}
-      : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {}
 
   Rotation2d(const double x, const double y, const bool normalize) {
     if (normalize) {
@@ -28,17 +27,14 @@ class Rotation2d final {
     value_ = std::atan2(sin_, cos_);
   }
 
-  static Rotation2d FromDegrees(const double degrees) {
+  static Rotation2d FromDegrees(const double degrees) { return Rotation2d(Deg2Rad(degrees)); }
-    return Rotation2d(Deg2Rad(degrees));
-  }
 
   // Operator Overloads
   Rotation2d operator-(const Rotation2d& other) const { return *this + -other; }
   Rotation2d operator-() const { return Rotation2d(-value_); }
 
   Rotation2d operator+(const Rotation2d& other) const {
-    return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
+    return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
-                      Cos() * other.Sin() + Sin() * other.Cos(),
                       true};
   }
 
@@ -49,9 +45,7 @@ class Rotation2d final {
   double Sin() const { return sin_; }
   double Tan() const { return sin_ / cos_; }
 
-  bool IsParallel(const Rotation2d& other) const {
+  bool IsParallel(const Rotation2d& other) const { return EpsilonEquals((*this - other).Radians(), 0.0); }
-    return EpsilonEquals((*this - other).Radians(), 0.0);
-  }
 
  private:
   double value_;

FalconLibraryCPP/src/mathematics/geometry/Translation2d.h

@@ -2,8 +2,8 @@
 
 #include <cmath>
 
-#include "Rotation2d.h"
 #include "../../types/VaryInterpolatable.h"
+#include "Rotation2d.h"
 
 namespace frc5190 {
 
@@ -20,16 +20,14 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
     return std::hypot(other.X() - X(), other.Y() - Y());
   }
 
-  Translation2d Interpolate(const Translation2d& end_value,
+  Translation2d Interpolate(const Translation2d& end_value, const double t) const override {
-                            const double t) const override {
     if (t <= 0) {
       return *this;
     }
     if (t >= 1) {
       return end_value;
     }
-    return Translation2d{Lerp(X(), end_value.X(), t),
+    return Translation2d{Lerp(X(), end_value.X(), t), Lerp(Y(), end_value.Y(), t)};
-                         Lerp(Y(), end_value.Y(), t)};
   }
 
   // Operator Overloads
@@ -41,18 +39,14 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
     return Translation2d{X() - other.X(), Y() - other.Y()};
   }
 
-  Translation2d operator*(const double scalar) const {
+  Translation2d operator*(const double scalar) const { return Translation2d{X() * scalar, Y() * scalar}; }
-    return Translation2d{X() * scalar, Y() * scalar};
-  }
 
   Translation2d operator*(const Rotation2d& rotation) const {
     return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(),
                          x_ * rotation.Sin() + y_ * rotation.Cos()};
   }
 
-  Translation2d operator/(const double scalar) const {
+  Translation2d operator/(const double scalar) const { return Translation2d{X() / scalar, Y() / scalar}; }
-    return Translation2d{X() / scalar, Y() / scalar};
-  }
 
   Translation2d operator-() const { return Translation2d{-X(), -Y()}; }
 

FalconLibraryCPP/src/mathematics/geometry/Twist2d.h

@@ -7,13 +7,10 @@ class Twist2d {
  public:
   // Constructors
   Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
-  Twist2d(const double dx, const double dy, const double dtheta)
+  Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {}
-      : dx_(dx), dy_(dy), dtheta_(dtheta) {}
 
   // Operator Overloads
-  Twist2d operator*(const double scalar) const {
+  Twist2d operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; }
-    return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
-  }
 
   // Accessors
   double Dx() const { return dx_; }

FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h

@@ -6,8 +6,7 @@ namespace frc5190 {
 class ParametricQuinticHermiteSpline final : public ParametricSpline {
  public:
   ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {
-    const auto scale_factor =
+    const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation());
-        1.2 * start.Translation().Distance(end.Translation());
 
     x0_   = start.Translation().X();
     x1_   = end.Translation().X();
@@ -25,16 +24,14 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
 
     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_ =
+    cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_;
-        -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_ =
+    cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
-        -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
     dy_ = 0.5 * ddy0_;
     ey_ = dy0_;
     fy_ = y0_;
@@ -47,33 +44,26 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
   const Pose2d& EndPose() const { return end_; }
 
   Translation2d Point(const double t) const override {
-    return Translation2d{
+    return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) +
-        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 {
+  Rotation2d Heading(const double t) const override { return {Dx(t), Dy(t), true}; }
-    return {Dx(t), Dy(t), true};
-  }
 
   double Curvature(const double t) const override {
-    return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) /
+    return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(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)) *
+                     3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(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 {
+  double Velocity(const double t) const override { return std::hypot(Dx(t), Dy(t)); }
-    return std::hypot(Dx(t), Dy(t));
-  }
 
  private:
   double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_;
@@ -86,32 +76,26 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
   Pose2d end_;
 
   double Dx(const double t) const {
-    return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) +
+    return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) +
-           3.0 * cx_ * std::pow(t, 2) + 2.0 * dx_ * t + ex_;
+           2.0 * dx_ * t + ex_;
   }
 
   double Dy(const double t) const {
-    return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) +
+    return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) +
-           3.0 * cy_ * std::pow(t, 2) + 2.0 * dy_ * t + ey_;
+           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) +
+    return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_;
-           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) +
+    return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_;
-           6.0 * cy_ * t + 2 * dy_;
   }
 
-  double Dddx(const double t) const {
+  double Dddx(const double t) const { return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; }
-    return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_;
-  }
 
-  double Dddy(const double t) const {
+  double Dddy(const double t) const { return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; }
-    return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_;
-  }
 
   template <typename T>
   static constexpr double BoundRadians(const T radians) {

FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h

@@ -11,8 +11,7 @@ class ParametricSpline {
   virtual double        Velocity(double t) const   = 0;
 
   Pose2dWithCurvature PoseWithCurvature(const double t) const {
-    return Pose2dWithCurvature{Pose(t), Curvature(t),
+    return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)};
-                               DCurvature(t) / Velocity(t)};
   }
 
  private:

FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h

@@ -9,26 +9,25 @@ constexpr static double kMinSampleSize = 1.;
 
 class SplineGenerator {
  public:
-  static std::vector<Pose2dWithCurvature> ParameterizeSpline(
+  static std::vector<Pose2dWithCurvature> ParameterizeSpline(const std::shared_ptr<ParametricSpline>& spline,
-      ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
+                                                             const double max_dx, const double max_dy,
-      const double t0 = 0.0, const double t1 = 1.0) {
+                                                             const double max_dtheta, const double t0 = 0.0,
+                                                             const double t1 = 1.0) {
     const auto dt = t1 - t0;
-    auto rv =
+    auto       rv = std::vector<Pose2dWithCurvature>(static_cast<int>(kMinSampleSize / dt));
-        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,
+      GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta);
-                    max_dtheta);
     }
 
     return rv;
   }
 
   static std::vector<Pose2dWithCurvature> ParameterizeSplines(
-      std::vector<ParametricSpline*> splines, double max_dx, double max_dy,
+      std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy,
-      double max_dtheta) {
+      const double max_dtheta) {
     auto rv = std::vector<Pose2dWithCurvature>();
     if (splines.empty()) return rv;
 
@@ -42,11 +41,9 @@ public:
     return rv;
   }
 
-  static void GetSegmentArc(ParametricSpline* spline,
+  static void GetSegmentArc(const std::shared_ptr<ParametricSpline>& spline,
-                            std::vector<Pose2dWithCurvature>* rv,
+                            std::vector<Pose2dWithCurvature>* rv, const double t0, const double t1,
-                            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,
-                            double max_dtheta) {
     const auto p0 = spline->Point(t0);
     const auto p1 = spline->Point(t1);
     const auto r0 = spline->Heading(t0);
@@ -55,8 +52,7 @@ public:
     const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0};
     const auto twist          = Pose2d::ToTwist(transformation);
 
-    if (twist.Dy() > max_dy || twist.Dx() > max_dx ||
+    if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) {
-        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 {

FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h

@@ -7,6 +7,8 @@ template <typename S>
 class DistanceIterator : public TrajectoryIterator<double, S> {
  public:
   DistanceIterator(){};
+
+ protected:
   double Addition(double a, double b) const override { return a + b; }
 };
 
@@ -18,8 +20,7 @@ class DistanceTrajectory : public Trajectory<double, S> {
 
     distances_.push_back(0.0);
     for (int i = 1; i < points_.size(); ++i) {
-      distances_.push_back(distances_[i - 1] +
+      distances_.push_back(distances_[i - 1] + points_[i - 1].Distance(points_[i]));
-                           points_[i - 1].Distance(points_[i]));
     }
 
     iterator_->SetTrajectory(this);
@@ -45,24 +46,17 @@ class DistanceTrajectory : public Trajectory<double, S> {
           return TrajectorySamplePoint<S>(s, i, i);
         }
         return TrajectorySamplePoint<S>(
-            prev_s.Interpolate(s,
+            prev_s.Interpolate(s, (interpolant - distances_[i - 1]) / (distances_[i] - distances_[i - 1])),
-                               (interpolant - distances_[i - 1]) /
+            i - 1, i);
-                                   (distances_[i] - distances_[i - 1])),
-            i - 1,
-            i);
       }
     }
     throw - 1;
   }
 
-  std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override {
+  std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override { return iterator_; }
-    return iterator_;
-  }
 
   double FirstInterpolant() const override { return 0; }
-  double LastInterpolant() const override {
+  double LastInterpolant() const override { return distances_[distances_.size() - 1]; }
-    return distances_[distances_.size() - 1];
-  }
 
   S FirstState() const override { return points_[0]; }
   S LastState() const override { return points_[points_.size() - 1]; }

FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h

@@ -14,9 +14,9 @@ template <typename S>
 class IndexedIterator : public TrajectoryIterator<double, S> {
  public:
   IndexedIterator() {}
-  double Addition(const double a, const double b) const override {
+
-    return a + b;
+ protected:
-  }
+  double Addition(const double a, const double b) const override { return a + b; }
 };
 
 template <typename S>
@@ -49,21 +49,16 @@ class IndexedTrajectory : public Trajectory<double, S> {
     if (percent >= 1 - kLowestDouble) {
       return TrajectorySamplePoint<S>(this->Point(index + 1));
     }
-    return TrajectorySamplePoint<S>(
+    return TrajectorySamplePoint<S>(points_[index].Interpolate(points_[index], percent), index, index + 1);
-        points_[index].Interpolate(points_[index], percent), index, index + 1);
   }
 
   double FirstInterpolant() const override { return 0.0; }
-  double LastInterpolant() const override {
+  double LastInterpolant() const override { return std::max(0.0, points_.size() - 1.0); }
-    return std::max(0.0, points_.size() - 1.0);
-  }
 
   S FirstState() const override { return points_[0]; }
   S LastState() const override { return points_[points_.size() - 1]; }
 
-  std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override {
+  std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override { return iterator_; }
-    return iterator_;
-  }
 
  private:
   std::vector<S>                                 points_;

FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h

@@ -1,58 +1,40 @@
 #pragma once
 
-#include "TrajectoryIterator.h"
 #include "../../types/VaryInterpolatable.h"
+#include "TrajectoryIterator.h"
 
 namespace frc5190 {
 template <typename S>
 class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
  public:
-  TimedEntry(const S& state,
+  TimedEntry(const S& state, const double t, const double velocity, const double acceleration)
-             const double t,
+      : state_(state), t_(t), velocity_(velocity), acceleration_(acceleration) {}
-             const double velocity,
-             const double acceleration)
-      : state_(state),
-        t_(t),
-        velocity_(velocity),
-        acceleration_(acceleration) {}
 
   TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
 
-  TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
+  TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) const override {
-                            double t) const override {
     auto new_t   = this->Lerp(t_, end_value.t_, t);
     auto delta_t = new_t - this->t_;
 
     if (delta_t < 0.0) return end_value.Interpolate(*this, 1.0 - t);
 
-    auto reversing =
+    auto reversing = velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
-        velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
 
     auto new_v = velocity_ + acceleration_ * delta_t;
-    auto new_s = reversing ? -1.0
+    auto new_s = reversing ? -1.0 : 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ * delta_t * delta_t);
-                           : 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ *
-                                    delta_t * delta_t);
 
-    return TimedEntry{
+    return TimedEntry{state_.Interpolate(end_value.state_, new_s / state_.Distance(end_value.state_)), new_t,
-        state_.Interpolate(end_value.state_,
+                      new_v, acceleration_};
-                           new_s / state_.Distance(end_value.state_)),
-        new_t,
-        new_v,
-        acceleration_};
   }
 
-  double Distance(const TimedEntry<S>& other) const override {
+  double Distance(const TimedEntry<S>& other) const override { return state_.Distance(other.state_); }
-    return state_.Distance(other.state_);
-  }
 
   S      State() const { return state_; }
   double T() const { return t_; }
   double Velocity() const { return velocity_; }
   double Acceleration() const { return acceleration_; }
 
-  void SetAcceleration(const double acceleration) {
+  void SetAcceleration(const double acceleration) { acceleration_ = acceleration; }
-    acceleration_ = acceleration;
-  }
 
  private:
   S      state_;
@@ -63,9 +45,8 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
 
 template <typename S>
 class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
-  double Addition(const double a, const double b) const override {
+ protected:
-    return a + b;
+  double Addition(const double a, const double b) const override { return a + b; }
-  }
 };
 
 template <typename S>
@@ -80,11 +61,9 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
   std::vector<TimedEntry<S>> Points() const override { return points_; }
   bool                       Reversed() const override { return reversed_; }
 
-  TrajectorySamplePoint<TimedEntry<S>> Sample(
+  TrajectorySamplePoint<TimedEntry<S>> Sample(const double interpolant) override {
-      const double interpolant) override {
     if (interpolant >= LastInterpolant()) {
-      return TrajectorySamplePoint<TimedEntry<S>>(
+      return TrajectorySamplePoint<TimedEntry<S>>(this->Point(points_.size() - 1));
-          this->Point(points_.size() - 1));
     }
     if (interpolant <= FirstInterpolant()) {
       return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0));
@@ -98,26 +77,19 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
         }
         return TrajectorySamplePoint<TimedEntry<S>>(
             prev_s.state.Interpolate(s.state,
-                                     (interpolant - prev_s.state.T()) /
+                                     (interpolant - prev_s.state.T()) / (s.state.T() - prev_s.state.T())),
-                                         (s.state.T() - prev_s.state.T())),
+            i - 1, i);
-            i - 1,
-            i);
       }
     }
     throw - 1;
   }
 
-  std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator()
+  std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator() const override { return iterator_; }
-      const override {
-    return iterator_;
-  }
 
   double        FirstInterpolant() const override { return FirstState().T(); }
   double        LastInterpolant() const override { return LastState().T(); }
   TimedEntry<S> FirstState() const override { return points_[0]; }
-  TimedEntry<S> LastState() const override {
+  TimedEntry<S> LastState() const override { return points_[points_.size() - 1]; }
-    return points_[points_.size() - 1];
-  }
 
  private:
   std::vector<TimedEntry<S>>                                 points_;

FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h

@@ -22,9 +22,7 @@ struct TrajectorySamplePoint {
       : state(point.state), index_floor(point.index), index_ceil(point.index) {}
 
   TrajectorySamplePoint(S state, int index_floor, int index_ceil)
-      : state(std::move(state)),
+      : state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
-        index_floor(index_floor),
-        index_ceil(index_ceil) {}
 
   TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
 };
@@ -38,9 +36,7 @@ class Trajectory {
   virtual std::vector<S> Points() const   = 0;
   virtual bool           Reversed() const = 0;
 
-  TrajectoryPoint<S> Point(int index) const {
+  TrajectoryPoint<S> Point(int index) const { return TrajectoryPoint<S>{index, Points()[index]}; }
-    return TrajectoryPoint<S>{index, Points()[index]};
-  }
 
   virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
 

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <array>
+
 #include "../geometry/Pose2dWithCurvature.h"
 #include "../spline/ParametricQuinticHermiteSpline.h"
 #include "../spline/SplineGenerator.h"
@@ -11,17 +13,14 @@
 namespace frc5190 {
 
 class TrajectoryGenerator {
+  using Constraints = std::vector<TimingConstraint<Pose2dWithCurvature>*>;
+
  public:
   static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
-      std::vector<Pose2d> waypoints,
+      std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
-      const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
+      const double end_velocity, const double max_velocity, const double max_acceleration,
-      const double start_velocity,
-      const double end_velocity,
-      const double max_velocity,
-      const double max_acceleration,
       const bool reversed) {
-    const auto flipped_position =
+    const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
-        Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
 
     if (reversed) {
       for (auto& waypoint : waypoints) {
@@ -29,48 +28,31 @@ class TrajectoryGenerator {
       }
     }
 
-    const auto indexed_trajectory =
+    const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
-        TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
 
     auto points = indexed_trajectory.Points();
 
     if (reversed) {
       for (auto& point : points) {
-        point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
+        point =
-                                    -point.Curvature(),
+            Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()};
-                                    point.Dkds()};
       }
     }
 
-    return TimeParameterizeTrajectory(
+    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 std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
-      const double max_dx,
-      const double max_dy,
       const double max_dtheta) {
-    auto size = static_cast<int>(waypoints.size());
+    std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
-    std::vector<ParametricSpline*> splines(size - 1);
+    for (auto i = 1; i < waypoints.size(); ++i) {
-    for (int i = 1; i < waypoints.size(); ++i) {
+      splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
-      splines[i - 1] =
-          new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
     }
     auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
-        SplineGenerator::ParameterizeSplines(
+        SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
-            splines, max_dx, max_dy, max_dtheta));
-
-    for (auto ptr : splines) {
-      delete ptr;
-    }
 
     return trajectory;
   }
@@ -78,56 +60,46 @@ class TrajectoryGenerator {
   template <typename S>
   struct ConstrainedPose {
     S      state;
-    double distance;
+    double distance         = 0.0;
-    double max_velocity;
+    double max_velocity     = 0.0;
-    double min_acceleration;
+    double min_acceleration = 0.0;
-    double max_acceleration;
+    double max_acceleration = 0.0;
   };
 
   template <typename S>
-  static void EnforceAccelerationLimits(
+  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(
-          constrained_pose->state,
+          constrained_pose->state, reverse ? -1.0 : 1.0 * constrained_pose->max_velocity);
-          reverse ? -1.0 : 1.0 * constrained_pose->max_velocity);
 
       if (!min_max_accel.IsValid()) throw - 1;
 
       constrained_pose->min_acceleration =
           std::max(constrained_pose->min_acceleration,
-                   reverse ? -min_max_accel.max_acceleration
+                   reverse ? -min_max_accel.max_acceleration : min_max_accel.min_acceleration);
-                           : min_max_accel.min_acceleration);
 
       constrained_pose->max_acceleration =
           std::min(constrained_pose->max_acceleration,
-                   reverse ? -min_max_accel.min_acceleration
+                   reverse ? -min_max_accel.min_acceleration : min_max_accel.max_acceleration);
-                           : min_max_accel.max_acceleration);
     }
   }
 
   template <typename S>
-  static TimedTrajectory<S> TimeParameterizeTrajectory(
+  static TimedTrajectory<S> TimeParameterizeTrajectory(DistanceTrajectory<S> distance_trajectory,
-      DistanceTrajectory<S> distance_trajectory,
+                                                       Constraints constraints, double start_velocity,
-      std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
+                                                       double end_velocity, double max_velocity,
-      double start_velocity,
+                                                       double max_acceleration, double step_size,
-      double end_velocity,
-      double max_velocity,
-      double max_acceleration,
-      double step_size,
                                                        bool reversed) {
-    const auto num_states = static_cast<int>(
+    const auto num_states =
-        std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
+        static_cast<int>(std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
 
     constexpr static auto epsilon = 1E-6;
     static auto           last    = distance_trajectory.LastInterpolant();
 
     std::vector<S> states(num_states);
     for (auto i = 0; i < num_states; ++i) {
-      states[i] =
+      states[i] = distance_trajectory.Sample(std::min(i * step_size, last)).state;
-          distance_trajectory.Sample(std::min(i * step_size, last)).state;
     }
 
     // Forward pass. We look at pairs of consecutive states, where the start
@@ -141,8 +113,8 @@ class TrajectoryGenerator {
 
     std::vector<ConstrainedPose<S>> constrained_poses(num_states);
 
-    auto _predecessor = ConstrainedPose<S>{
+    auto _predecessor =
-        states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
+        ConstrainedPose<S>{states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
     ConstrainedPose<S>* predecessor = &_predecessor;
 
     for (int i = 0; i < states.size(); ++i) {
@@ -158,9 +130,8 @@ class TrajectoryGenerator {
       while (true) {
         // Enforce global max velocity and max reachable velocity by global
         // acceleration limit. vf = sqrt(vi^2 + 2*a*d)
-        constrained_pose.max_velocity = std::min(
+        constrained_pose.max_velocity =
-            max_velocity,
+            std::min(max_velocity, std::sqrt(predecessor->max_velocity * predecessor->max_velocity +
-            std::sqrt(predecessor->max_velocity * predecessor->max_velocity +
                                              2.0 * predecessor->max_acceleration * ds));
 
         if (std::isnan(constrained_pose.max_velocity)) {
@@ -177,8 +148,7 @@ class TrajectoryGenerator {
 
         for (const auto& constraint : constraints) {
           constrained_pose.max_velocity =
-              std::min(constraint->MaxVelocity(constrained_pose.state),
+              std::min(constraint->MaxVelocity(constrained_pose.state), constrained_pose.max_velocity);
-                       constrained_pose.max_velocity);
         }
 
         if (constrained_pose.max_velocity < 0.0) throw - 1;
@@ -191,8 +161,8 @@ class TrajectoryGenerator {
         // If the max acceleration for this constraint state is more
         // conservative than what we had applied, we need to reduce the max
         // accel at the predecessor state and try again.
-        auto actual_acceleration = (std::pow(constrained_pose.max_velocity, 2) -
+        auto actual_acceleration =
-                                    std::pow(predecessor->max_velocity, 2)) /
+            (std::pow(constrained_pose.max_velocity, 2) - std::pow(predecessor->max_velocity, 2)) /
             (2.0 * ds);
 
         if (constrained_pose.max_acceleration < actual_acceleration - epsilon) {
@@ -209,11 +179,8 @@ class TrajectoryGenerator {
 
     // Backward pass
     auto _successor =
-        ConstrainedPose<S>{states[states.size() - 1],
+        ConstrainedPose<S>{states[states.size() - 1], constrained_poses[states.size() - 1].distance,
-                           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) {
@@ -224,8 +191,7 @@ class TrajectoryGenerator {
         // Enforce reverse max reachable velocity limit.
         // vf = sqrt(vi^2 + 2*a*d), where vi = successor.
 
-        const auto new_max_velocity =
+        const auto new_max_velocity = std::sqrt(successor->max_velocity * successor->max_velocity +
-            std::sqrt(successor->max_velocity * successor->max_velocity +
                                                 2.0 * successor->min_acceleration * ds);
 
         if (new_max_velocity >= state.max_velocity) {
@@ -246,9 +212,8 @@ class TrajectoryGenerator {
         // conservative than what we have applied, we need to reduce the min
         // accel and try again.
 
-        auto actual_acceleration = (std::pow(state.max_velocity, 2) -
+        auto actual_acceleration =
-                                    std::pow(successor->max_velocity, 2)) /
+            (std::pow(state.max_velocity, 2) - std::pow(successor->max_velocity, 2)) / (2 * ds);
-                                   (2 * ds);
 
         if (state.min_acceleration > actual_acceleration + epsilon) {
           successor->min_acceleration = state.min_acceleration;
@@ -269,10 +234,7 @@ class TrajectoryGenerator {
     for (int i = 0; i < states.size(); i++) {
       const ConstrainedPose<S> constrained_pose = constrained_poses.at(i);
       const double             ds               = constrained_pose.distance - s;
-      double accel =
+      double accel = (constrained_pose.max_velocity * constrained_pose.max_velocity - v * v) / (2. * ds);
-          (constrained_pose.max_velocity * constrained_pose.max_velocity -
-           v * v) /
-          (2. * ds);
       double dt    = 0.;
       if (i > 0) {
         timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel);
@@ -285,10 +247,8 @@ class TrajectoryGenerator {
 
       v = constrained_pose.max_velocity;
       s = constrained_pose.distance;
-      timed_states[i] = TimedEntry<S>{constrained_pose.state,
+      timed_states[i] =
-                                      t,
+          TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v, reversed ? -accel : accel};
-                                      reversed ? -v : v,
-                                      reversed ? -accel : accel};
 
       t += dt;
     }

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include "Trajectory.h"
 #include "../../Utilities.h"
+#include "Trajectory.h"
 
 namespace frc5190 {
 template <typename U, typename S>
@@ -10,8 +10,6 @@ class TrajectoryIterator {
   TrajectoryIterator() {}
   ~TrajectoryIterator() = default;
 
-  virtual U Addition(U a, U b) const = 0;
-
   void SetTrajectory(Trajectory<U, S>* trajectory) {
     trajectory_ = trajectory;
     progress_   = trajectory_->FirstInterpolant();
@@ -19,17 +17,15 @@ class TrajectoryIterator {
   }
 
   TrajectorySamplePoint<S> Advance(U amount) {
-    progress_ = Clamp(Addition(progress_, amount),
+    progress_ =
-                      trajectory_->FirstInterpolant(),
+        Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant());
-                      trajectory_->LastInterpolant());
     sample_ = trajectory_->Sample(progress_);
     return sample_;
   }
 
   TrajectorySamplePoint<S> Preview(U amount) {
-    auto progress = Clamp(Addition(progress_, amount),
+    auto progress =
-                          trajectory_->FirstInterpolant(),
+        Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant());
-                          trajectory_->LastInterpolant());
     return trajectory_->Sample(progress);
   }
 
@@ -37,6 +33,9 @@ class TrajectoryIterator {
   TrajectoryPoint<S> CurrentState() const { return sample_; }
 
  protected:
+  virtual U Addition(U a, U b) const = 0;
+
+ private:
   Trajectory<U, S>*        trajectory_;
   U                        progress_;
   TrajectorySamplePoint<S> sample_;

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

@@ -5,8 +5,7 @@
 
 namespace frc5190 {
 
-class AngularAccelerationConstraint final
+class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
-    : public TimingConstraint<Pose2dWithCurvature> {
  public:
   explicit AngularAccelerationConstraint(double max_angular_acceleration)
       : max_angular_acceleration_(max_angular_acceleration) {}
@@ -23,8 +22,8 @@ class AngularAccelerationConstraint final
     return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds()));
   }
 
-  frc5190::MinMaxAcceleration MinMaxAcceleration(
+  frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
-      const Pose2dWithCurvature& state, double velocity) const override {
+                                                 double                     velocity) const override {
     /**
      * We want to limit the acceleration such that we never go above the
      * specified angular acceleration.
@@ -53,12 +52,10 @@ class AngularAccelerationConstraint final
      * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature
      */
 
-    const auto max_absolute_acceleration = std::abs(
+    const auto max_absolute_acceleration =
-        (max_angular_acceleration_ - (velocity * velocity * state.Dkds())) /
+        std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature());
-        state.Curvature());
 
-    return frc5190::MinMaxAcceleration{-max_absolute_acceleration,
+    return frc5190::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration};
-                                       max_absolute_acceleration};
   }
 
  private:

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

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

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

@@ -16,11 +16,9 @@ class TimingConstraint {
  public:
   virtual ~TimingConstraint() = default;
   static constexpr MinMaxAcceleration kNoLimits =
-      MinMaxAcceleration{std::numeric_limits<double>::lowest(),
+      MinMaxAcceleration{std::numeric_limits<double>::lowest(), std::numeric_limits<double>::max()};
-                         std::numeric_limits<double>::max()};
 
   virtual double             MaxVelocity(const S& state) const                         = 0;
-  virtual MinMaxAcceleration MinMaxAcceleration(const S& state,
+  virtual MinMaxAcceleration MinMaxAcceleration(const S& state, double velocity) const = 0;
-                                                double velocity) const = 0;
 };
 }  // namespace frc5190

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

@@ -5,12 +5,9 @@
 
 namespace frc5190 {
 
-class VelocityLimitRadiusConstraint
+class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> {
-    : public TimingConstraint<Pose2dWithCurvature> {
  public:
-  VelocityLimitRadiusConstraint(const Translation2d& point,
+  VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity)
-                                const double radius,
-                                const double max_velocity)
       : point_(point), radius_(radius), max_velocity_(max_velocity) {}
 
   ~VelocityLimitRadiusConstraint() = default;
@@ -22,8 +19,8 @@ class VelocityLimitRadiusConstraint
     return std::numeric_limits<double>::max();
   }
 
-  frc5190::MinMaxAcceleration MinMaxAcceleration(
+  frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
-      const Pose2dWithCurvature& state, double velocity) const override {
+                                                 double                     velocity) const override {
     return kNoLimits;
   }
 

FalconLibraryCPP/src/types/Interpolatable.h

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