commit bezier

PathPlanning/Bezier/BezierCurve.cpp

@@ -0,0 +1,39 @@
+//
+// Created by chh3213 on 2022/11/25.
+//
+
+#include "BezierCurve.h"
+
+
+/**
+ * 阶乘实现
+ * @param n
+ * @return
+ */
+double factorial(int n) {
+    if(n<=1)return 1;
+    return factorial(n-1)*n;
+}
+
+/**
+ * 贝塞尔公式
+ * @param Ps
+ * @param t
+ * @return
+ */
+Vector2d bezierCommon(vector<Vector2d> Ps, double t) {
+
+    if(Ps.size()==1)return Ps[0];
+
+    Vector2d p_t(0.,0.);
+    int n = Ps.size()-1;
+    for(int i=0;i<Ps.size();i++){
+        double C_n_i = factorial(n)/ (factorial(i)* factorial(n-i));
+        p_t +=  C_n_i*pow((1-t),(n-i))*pow(t,i)*Ps[i];
+        //cout<<t<<","<<1-t<<","<<n-i<<","<<pow((1-t),(n-i))<<endl;
+    }
+    return p_t;
+}
+
+
+

PathPlanning/Bezier/BezierCurve.h

@@ -0,0 +1,22 @@
+//
+// Created by chh3213 on 2022/11/25.
+//
+
+#ifndef CHHROBOTICS_CPP_BEZIERCURVE_H
+#define CHHROBOTICS_CPP_BEZIERCURVE_H
+
+#include <iostream>
+#include <Eigen/Dense>
+#include<vector>
+#include<cmath>
+#include<algorithm>
+using namespace std;
+using namespace Eigen;
+
+double factorial(int n);
+
+Vector2d bezierCommon(vector<Vector2d>Ps,  double t);
+
+Vector2d bezierRecursion(vector<Vector2d>Ps, double t);
+
+#endif //CHHROBOTICS_CPP_BEZIERCURVE_H

PathPlanning/Bezier/main.cpp

@@ -0,0 +1,38 @@
+//
+// Created by chh3213 on 2022/11/25.
+//
+
+#include "BezierCurve.h"
+#include "../../matplotlibcpp.h"
+namespace plt = matplotlibcpp;
+
+int main(){
+    vector<Vector2d>Ps{Vector2d (0,0),Vector2d(1,1),Vector2d(2,1),Vector2d(3,0),Vector2d(4,2)};
+    //vector<Vector2d>Ps{Vector2d (0,0),Vector2d(1,1)};
+
+    vector<double>x_ref,y_ref;
+    for(int i=0;i<Ps.size();i++){
+        x_ref.push_back(Ps[i][0]);
+        y_ref.push_back(Ps[i][1]);
+    }
+    vector<double>x_,y_;
+    for(int t=0;t<100;t++){
+        plt::clf();
+        Vector2d pos = bezierCommon(Ps,(double)t/100);
+        //cout<<pos[0]<<","<<pos[1]<<endl;
+        x_.push_back(pos[0]);
+        y_.push_back(pos[1]);
+
+        //画图
+        //plt::xlim(0,1);
+        plt::plot(x_, y_,"r");
+        plt::plot(x_ref,y_ref);
+        plt::pause(0.01);
+    }
+    // save figure
+    const char* filename = "./bezier_demo.png";
+    cout << "Saving result to " << filename << std::endl;
+    plt::save(filename);
+    plt::show();
+    return 0;
+}