fix 分段b spline bug

CHH3213 · May 6, 2023 · 1d480cd · 1d480cd
1 parent 91750c2
commit 1d480cd
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 4 deletions.
diff --git a/PathPlanning/B-spline/BSpline.cpp b/PathPlanning/B-spline/BSpline.cpp
@@ -71,10 +71,13 @@ vector<double> u_piecewise_B_Spline(int n, int k) {
     vector<double> node_vector(n+k+1);
     if(n%(k-1)==0&&(k-1)>0){//满足n是k-1的整数倍且k-1为正整数
         for(int i=n+1;i<n+k+1;i++)node_vector[i]=1;//末尾n+1到n+k+1的数重复
-        double piecewise = n / (k-1);  //设定内节点的值
+        int piecewise = n / (k-1);  //设定内节点的值
         if(piecewise>1){
             //内节点重复k-1次
-            for(int i=k;i<n+1;i++)node_vector[i]=1/piecewise;
+            for(int i=1;i<piecewise;i++){
+                for(int j=0;j<k-1;j++)node_vector[(k-1)*i+j+1]=i/piecewise;
+            }
+
         }
     }else{
         cout<<"error!需要满足n是k-1的整数倍且k-1为正整数"<<endl;

diff --git a/PathPlanning/B-spline/main.cpp b/PathPlanning/B-spline/main.cpp
@@ -7,8 +7,8 @@
 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 (9.036145, 51.779661),Vector2d(21.084337, 70.084746),Vector2d(37.607573, 50.254237),Vector2d(51.893287, 69.745763),Vector2d(61.187608,  49.576271)};
+//    vector<Vector2d>Ps{Vector2d (0,0),Vector2d(1,1),Vector2d(2,1),Vector2d(3,0),Vector2d(4,2)};
+    vector<Vector2d>Ps{Vector2d (9.036145, 51.779661),Vector2d(21.084337, 70.084746),Vector2d(37.607573, 50.254237),Vector2d(51.893287, 69.745763),Vector2d(61.187608,  49.576271)};
 
     vector<double>x_ref,y_ref;
     for(int i=0;i<Ps.size();i++){