Add splines

FalconLibraryCPP/FalconLibraryCPP.vcxproj

@@ -124,6 +124,9 @@
   </ItemDefinitionGroup>
   <ItemGroup>
     <ClInclude Include="src\FalconLibrary.h" />
+    <ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
+    <ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
+    <ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
     <ClInclude Include="src\types\Interpolatable.h" />
     <ClInclude Include="src\mathematics\geometry\Pose2d.h" />
     <ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />

FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters

@@ -42,5 +42,14 @@
     <ClInclude Include="src\mathematics\geometry\Twist2d.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="src\mathematics\spline\ParametricSpline.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\spline\SplineGenerator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

FalconLibraryCPP/src/mathematics/geometry/Pose2d.h

@@ -1,10 +1,9 @@
 #pragma once
 
-#include "types/VaryInterpolatable.h"
 #include "Translation2d.h"
 #include "Rotation2d.h"
-#include "Utilities.h"
 #include "Twist2d.h"
+#include "../../Utilities.h"
 
 namespace frc5190 {
 class Twist2d;
@@ -20,7 +19,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
       : translation_(Translation2d(x, y)), rotation_(rotation) {}
 
   // Overriden Methods
-  double Distance(const Pose2d& other) override {
+  double Distance(const Pose2d& other) const override {
     return ToTwist(-*this + other).Norm();
   }
   Pose2d Interpolate(const Pose2d& end_value, const double t) override {

FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include <utility>
-#include "types/VaryInterpolatable.h"
 #include "Pose2d.h"
 
 namespace frc5190 {
@@ -13,7 +12,7 @@ class Pose2dWithCurvature final
       : pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
 
   // Overriden Methods
-  double Distance(const Pose2dWithCurvature& other) override {
+  double Distance(const Pose2dWithCurvature& other) const override {
     return pose_.Distance(other.pose_);
   }
 

FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h

@@ -1,8 +1,7 @@
 #pragma once
 
 #include <cmath>
-
-#include "Utilities.h"
+#include "../../Utilities.h"
 
 namespace frc5190 {
 class Rotation2d final {

FalconLibraryCPP/src/mathematics/geometry/Translation2d.h

@@ -2,9 +2,8 @@
 
 #include <cmath>
 
-#include "types/Interpolatable.h"
-#include "types/VaryInterpolatable.h"
 #include "Rotation2d.h"
+#include "../../types/VaryInterpolatable.h"
 
 namespace frc5190 {
 
@@ -17,7 +16,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
       : x_(distance * rotation.Cos()), y_(distance * rotation.Sin()) {}
 
   // Overriden Methods
-  double Distance(const Translation2d& other) override {
+  double Distance(const Translation2d& other) const override {
     return std::hypot(other.X() - X(), other.Y() - Y());
   }
 

FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h

@@ -0,0 +1,123 @@
+#pragma once
+
+#include "ParametricSpline.h"
+
+namespace frc5190 {
+class ParametricQuinticHermiteSpline final : public ParametricSpline {
+ public:
+  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& 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), Dx(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_;
+  double y0_, y1_, dy0_, dy1_, ddy0_, ddy1_;
+
+  double ax_, bx_, cx_, dx_, ex_, fx_;
+  double ay_, by_, cy_, dy_, ey_, fy_;
+
+  Pose2d start_;
+  Pose2d end_;
+
+  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) + 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) +
+           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 frc5190

FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h

@@ -0,0 +1,21 @@
+#pragma once
+#include "../geometry/Pose2dWithCurvature.h"
+
+namespace frc5190 {
+class ParametricSpline {
+ public:
+  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 {
+    return Pose2dWithCurvature{Pose(t), Curvature(t),
+                               DCurvature(t) / Velocity(t)};
+  }
+
+ private:
+  Pose2d Pose(const double t) const { return Pose2d{Point(t), Heading(t)}; }
+};
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <vector>
+#include "../geometry/Pose2dWithCurvature.h"
+#include "ParametricSpline.h"
+
+namespace frc5190 {
+constexpr static double kMinSampleSize = 1.;
+
+class SplineGenerator {
+  static std::vector<Pose2dWithCurvature> ParameterizeSpline(
+      ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
+      const double t0 = 0.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<ParametricSpline*> splines, double max_dx, double max_dy,
+      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(ParametricSpline* spline,
+                            std::vector<Pose2dWithCurvature>* rv,
+                            const double t0, const double t1,
+                            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);
+    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 frc5190

FalconLibraryCPP/src/types/VaryInterpolatable.h

@@ -6,6 +6,6 @@ namespace frc5190 {
 template <typename T>
 class VaryInterpolatable : public Interpolatable<T> {
  public:
-  virtual double Distance(const T& other) = 0;
+  virtual double Distance(const T& other) const = 0;
 };
 }  // namespace frc5190