diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
index e136499..5ef9c26 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
@@ -124,6 +124,9 @@
+
+
+
diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
index af4a841..244bc7f 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
@@ -42,5 +42,14 @@
Header Files
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
\ No newline at end of file
diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
index 1c35bed..47653b8 100644
--- a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
+++ b/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 {
: 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 {
diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
index 3de5aa1..a9ef87c 100644
--- a/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
@@ -1,7 +1,6 @@
#pragma once
#include
-#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_);
}
diff --git a/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h
index 4b99c4c..50b7dbe 100644
--- a/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h
@@ -1,8 +1,7 @@
#pragma once
#include
-
-#include "Utilities.h"
+#include "../../Utilities.h"
namespace frc5190 {
class Rotation2d final {
diff --git a/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
index 6248f25..9ce10a5 100644
--- a/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
@@ -2,9 +2,8 @@
#include
-#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 {
: 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());
}
diff --git a/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h b/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h
new file mode 100644
index 0000000..95a02a0
--- /dev/null
+++ b/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
+ static constexpr double BoundRadians(const T radians) {
+ while (radians >= kPi) radians -= 2 * kPi;
+ while (radians < kPi) radians += 2 * kPi;
+ return radians;
+ }
+};
+} // namespace frc5190
diff --git a/FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h b/FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h
new file mode 100644
index 0000000..0190fc7
--- /dev/null
+++ b/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
diff --git a/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h b/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h
new file mode 100644
index 0000000..c72a5d9
--- /dev/null
+++ b/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include
+#include "../geometry/Pose2dWithCurvature.h"
+#include "ParametricSpline.h"
+
+namespace frc5190 {
+constexpr static double kMinSampleSize = 1.;
+
+class SplineGenerator {
+ static std::vector 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(static_cast(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 ParameterizeSplines(
+ std::vector splines, double max_dx, double max_dy,
+ double max_dtheta) {
+ auto rv = std::vector();
+ 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* 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
diff --git a/FalconLibraryCPP/src/types/VaryInterpolatable.h b/FalconLibraryCPP/src/types/VaryInterpolatable.h
index 9ceb2d3..90c94de 100644
--- a/FalconLibraryCPP/src/types/VaryInterpolatable.h
+++ b/FalconLibraryCPP/src/types/VaryInterpolatable.h
@@ -6,6 +6,6 @@ namespace frc5190 {
template
class VaryInterpolatable : public Interpolatable {
public:
- virtual double Distance(const T& other) = 0;
+ virtual double Distance(const T& other) const = 0;
};
} // namespace frc5190