it works

FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h

@@ -55,7 +55,7 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
   }
 
   Rotation2d Heading(const double t) const override {
-    return {Dx(t), Dx(t), true};
+    return {Dx(t), Dy(t), true};
   }
 
   double Curvature(const double t) const override {

FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h

@@ -14,10 +14,10 @@ template <typename S>
 class DistanceTrajectory : public Trajectory<double, S> {
  public:
   explicit DistanceTrajectory(std::vector<S> points) : points_(points) {
-    iterator_ = new DistanceIterator<S>;
+    iterator_ = std::make_shared<DistanceIterator<S>>();
 
     distances_.push_back(0.0);
-    for (auto i = 1; i < points_.size(); ++i) {
+    for (int i = 1; i < points_.size(); ++i) {
       distances_.push_back(distances_[i - 1] +
                            points_[i - 1].Distance(points_[i]));
     }
@@ -25,8 +25,6 @@ class DistanceTrajectory : public Trajectory<double, S> {
     iterator_->SetTrajectory(this);
   }
 
-  ~DistanceTrajectory() { delete iterator_; }
-
   std::vector<S> Points() const override { return points_; }
 
   bool Reversed() const override { return false; }
@@ -39,7 +37,7 @@ class DistanceTrajectory : public Trajectory<double, S> {
       return TrajectorySamplePoint<S>(this->Point(0));
     }
 
-    for (auto i = 1; i < distances_.size(); ++i) {
+    for (int i = 1; i < distances_.size(); ++i) {
       const auto s = points_[i];
       if (distances_[i] >= interpolant) {
         const auto prev_s = points_[i - 1];
@@ -55,7 +53,9 @@ class DistanceTrajectory : public Trajectory<double, S> {
     throw - 1;
   }
 
-  TrajectoryIterator<double, S>* Iterator() const override { return iterator_; }
+  std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override {
+    return iterator_;
+  }
 
   double FirstInterpolant() const override { return 0; }
   double LastInterpolant() const override {
@@ -68,6 +68,6 @@ class DistanceTrajectory : public Trajectory<double, S> {
  private:
   std::vector<double> distances_;
   std::vector<S> points_;
-  DistanceIterator<S>* iterator_;
+  std::shared_ptr<DistanceIterator<S>> iterator_;
 };
 }  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h

@@ -23,12 +23,10 @@ template <typename S>
 class IndexedTrajectory : public Trajectory<double, S> {
  public:
   explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) {
-    iterator_ = new IndexedIterator<S>();
+    iterator_ = std::make_shared<IndexedIterator<S>>();
     iterator_->SetTrajectory(this);
   }
 
-  ~IndexedTrajectory() { delete iterator_; }
-
   std::vector<S> Points() const override { return points_; }
 
   bool Reversed() const override { return false; }
@@ -63,11 +61,13 @@ class IndexedTrajectory : public Trajectory<double, S> {
   S FirstState() const override { return points_[0]; }
   S LastState() const override { return points_[points_.size() - 1]; }
 
-  TrajectoryIterator<double, S>* Iterator() const override { return iterator_; }
+  std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override {
+    return iterator_;
+  }
 
  private:
   std::vector<S> points_;
-  IndexedIterator<S>* iterator_;
+  std::shared_ptr<TrajectoryIterator<double, S>> iterator_;
 };
 
 }  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h

@@ -73,12 +73,10 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
  public:
   TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
       : points_(points), reversed_(reversed) {
-    iterator_ = new TimedIterator<S>();
+    iterator_ = std::make_shared<TimedIterator<S>>();
     iterator_->SetTrajectory(this);
   }
 
-  ~TimedTrajectory() { delete iterator_; }
-
   std::vector<TimedEntry<S>> Points() const override { return points_; }
   bool Reversed() const override { return reversed_; }
 
@@ -91,7 +89,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
     if (interpolant <= FirstInterpolant()) {
       return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0));
     }
-    for (auto i = 1; i < points_.size(); ++i) {
+    for (int i = 1; i < points_.size(); ++i) {
       const auto s = this->Point(i);
       if (s.state.T() >= interpolant) {
         const auto prev_s = this->Point(i - 1);
@@ -108,7 +106,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
     throw - 1;
   }
 
-  TrajectoryIterator<double, TimedEntry<S>>* Iterator() const override {
+  std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator()
+      const override {
     return iterator_;
   }
 
@@ -122,7 +121,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
  private:
   std::vector<TimedEntry<S>> points_;
   bool reversed_;
-  TimedIterator<S>* iterator_;
+  std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> iterator_;
 };
 
 }  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h

@@ -48,7 +48,7 @@ class Trajectory {
 
   virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
 
-  virtual TrajectoryIterator<U, S>* Iterator() const = 0;
+  virtual std::shared_ptr<TrajectoryIterator<U, S>> Iterator() const = 0;
 
   virtual U FirstInterpolant() const = 0;
   virtual U LastInterpolant() const = 0;

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h

@@ -39,12 +39,10 @@ class TrajectoryGenerator {
       }
     }
 
-    const auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
-
     return TimeParameterizeTrajectory(
-        DistanceTrajectory<Pose2dWithCurvature>(trajectory.Points()),
+        DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
-        constraints, start_velocity, end_velocity, max_velocity,
+        start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
-        max_acceleration, 0.051, reversed);
+        reversed);
   }
 
   static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
@@ -52,7 +50,7 @@ class TrajectoryGenerator {
       const double max_dy, const double max_dtheta) {
     auto size = static_cast<int>(waypoints.size());
     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] =
           new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
     }
@@ -105,7 +103,6 @@ class TrajectoryGenerator {
       std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
       double start_velocity, 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));
 
@@ -114,10 +111,8 @@ class TrajectoryGenerator {
 
     std::vector<S> states(num_states);
     for (auto i = 0; i < num_states; ++i) {
-      states[i] = distance_trajectory
+      states[i] =
-                      .Sample(std::min(i * step_size,
+          distance_trajectory.Sample(std::min(i * step_size, last)).state;
-                                       last))
-                      .state;
     }
 
     // Forward pass. We look at pairs of consecutive states, where the start
@@ -135,7 +130,7 @@ class TrajectoryGenerator {
                                            -max_acceleration, max_acceleration};
     ConstrainedPose<S>* predecessor = &_predecessor;
 
-    for (auto i = 0; i < states.size(); ++i) {
+    for (int i = 0; i < states.size(); ++i) {
       constrained_poses[i] = ConstrainedPose<S>{};
       ConstrainedPose<S>& constrained_pose = constrained_poses.at(i);
 
@@ -204,7 +199,7 @@ class TrajectoryGenerator {
                            end_velocity, -max_acceleration, max_acceleration};
     ConstrainedPose<S>* successor = &_successor;
 
-    for (auto i = states.size() - 1; i >= 0; --i) {
+    for (int i = states.size() - 1; i >= 0; --i) {
       auto state = constrained_poses.at(i);
       const auto ds = state.distance - successor->distance;  // will be negative
 
@@ -254,7 +249,7 @@ class TrajectoryGenerator {
     auto s = 0.;
     auto v = 0.;
 
-    for (auto i = 0; i < states.size(); i++) {
+    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 =
@@ -273,9 +268,9 @@ class TrajectoryGenerator {
 
       v = constrained_pose.max_velocity;
       s = constrained_pose.distance;
-      timed_states[i] = TimedEntry<S>{constrained_pose.state, t,
+      timed_states[i] =
-                                           reversed ? -v : v,
+          TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v,
-                                           reversed ? -accel : accel};
+                        reversed ? -accel : accel};
 
       t += dt;
     }

Tests/trajectory-tests.cpp

@@ -13,21 +13,21 @@ class TrajectoryTest : public ::testing::Test {
            bool backwards = false) {
     auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
         std::vector<frc5190::Pose2d>{initial, final},
-        std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{new frc5190::CentripetalAccelerationConstraint{100.0}},
+        std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{
+            new frc5190::CentripetalAccelerationConstraint{100.0}},
         0.0, 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(),
                 kTestEpsilon);
     EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(),
                 kTestEpsilon);
-    EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Degrees(),
+    EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(),
                 kTestEpsilon);
-    */
 
-    /*
     const auto iterator = trajectory.Iterator();
 
     auto sample = iterator->Advance(0.0);
@@ -51,9 +51,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().Degrees(),
+    EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(),
                 kTestEpsilon);
-      */
   }
 };