Merge pull request #22 from Kotakku/feat/n-d_spline

Feat/n d spline

CMakeLists.txt

@@ -64,6 +64,7 @@ set(example_files
   path_planning/fmt_star
   path_planning/dwa
   path_planning/navigation_diffbot
+  path_planning/spline
 
   unit/unit_example
 )

docs/example/path_planning/spline.md

@@ -0,0 +1,8 @@
+# スプライン曲線
+
+スプライン曲線による補間  
+CatmullRomとCubicSplineを比較する
+
+![](../fig/spline.png)
+
+{{ include_example("example/path_planning/spline.cpp") }}

example/path_planning/spline.cpp

@@ -0,0 +1,64 @@
+#include <cpp_robotics/path_planning/spline_path.hpp>
+#include <cpp_robotics/matplotlibcpp.hpp>
+#include <Eigen/Dense>
+
+int main()
+{
+    using namespace cpp_robotics;
+    namespace plt = matplotlibcpp;
+
+    std::vector<Eigen::Vector2d> waypoints =
+    {
+        {0, 0},
+        {1, 2},
+        {2, 3},
+        {3, 1},
+        {4, 1},
+        {5, 0},
+        {4, -1},
+        {2, -2},
+        {1, -1}
+    };
+
+    CatumullRomSplinePath2d cutmull_spline(waypoints);
+    CubicSplinePath2d cubic_spline(waypoints);
+
+    std::vector<double> x, y;
+    {
+        const double length = cutmull_spline.length();
+        for (double i = 0; i < length; i += 0.05)
+        {
+            Eigen::Vector2d pos = cutmull_spline.position(i);
+            x.push_back(pos[0]);
+            y.push_back(pos[1]);
+        }
+        plt::named_plot("CatmullRomCurve", x, y);
+    }
+
+    x.clear();
+    y.clear();
+    {
+        const double length = cubic_spline.length();
+        for (double i = 0; i < length; i += 0.05)
+        {
+            Eigen::Vector2d pos = cubic_spline.position(i);
+            x.push_back(pos[0]);
+            y.push_back(pos[1]);
+        }
+        plt::named_plot("CubicSplineCurve", x, y);
+    }
+
+    x.clear();
+    y.clear();
+    // waypoints
+    for (const auto& p : waypoints)
+    {
+        x.push_back(p[0]);
+        y.push_back(p[1]);
+    }
+    plt::named_plot("Waypoint", x, y, "o");
+    plt::legend();
+    plt::show();
+
+    return 0;
+}