【轨迹规划】3：准均匀B样条曲线C++实现

继之前的内容，已知均匀B样条效果并不好。

准均匀B样条就是使两端节点具有重复度 k＋1

这里给出均匀B样条的实现代码

/*
准均匀B样条曲线测试
*/
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
using namespace std;
class Point {
public:
    float x, y,z;
    Point(float x = 0, float y = 0,float z = 0) : x(x), y(y) ,z(z){}
};
class BSplineCurve {
public:
    vector<Point> control_points; // 控制点,数量未定
    vector<float> knots; // 结点向量
    int degree; // 曲线次数
    //Constructor
    BSplineCurve(vector<Point> points, int k) {
        control_points = points;
        degree = k;
        // 初始化结点向量, m = n + 1+ k   ,m+1节点数量, n+1控制点数量 ,k 次数
        int num_knots = control_points.size() + degree+ 1 ;
        double delta = 1.0 /  (double)(num_knots - 2*degree - 1);
        for (int i = 0; i < num_knots; i++) {
            if (i < degree+1) {
               knots.push_back( 0.0);
            } else if (i >= num_knots - degree) {
                knots.push_back(1.0);
            } else {
               knots.push_back(knots.back() + delta);
            }
             cout << "knot"<<i<<"=" << knots[i] <<endl;
        }
    }

    // 计算基函数值
    float BasisFunction(int i, int k, double u) {
        if (k == 0) {
            if (u >= knots[i] && u < knots[i+1]) {            
                return 1;
            }
            else {               
                return 0;
            }
        }
        float a = 0.0, b = 0.0;
        if(knots[i+k] - knots[i] !=0.0){
            a = (u - knots[i]) / (knots[i+k] - knots[i]);
        }
        if(knots[i+k+1] - knots[i+1] !=0.0){
            b = (knots[i+k+1] - u) / (knots[i+k+1] - knots[i+1]);
        }
        return a * BasisFunction(i, k-1, u) + b * BasisFunction(i+1, k-1, u);
    }
  // 计算均匀 B 样条曲线上的点
    Point CalculatePoint(float u) {
        Point res;
        for (int i = 0; i < control_points.size(); ++i) {
            float basis = BasisFunction(i, degree, u);
            res.x += control_points[i].x * basis;
            res.y += control_points[i].y * basis;
            res.z += control_points[i].z * basis;
        }
        return res;
    }
};

int testQUBS(){

    ofstream fout2("D:/data2.txt"); // 打开或创建名为 data.txt 的文件
    if (!fout2) { // 检查文件是否打开成功
        cerr << "File2 open failed!" << endl;
        return -1;
    }
    vector<Point> points = {{50,50,0},{100,300,0},{300,100,0},{380,200,0},{400,600,0}}; // 控制点
    int degree = 2; // 曲线次数
    BSplineCurve curve(points, degree);
    cout << "size:" << curve.knots.size() << endl;
    const float step = 0.005; // 步长
    for (float u = 0; u <= 1; u += step) {
        Point p = curve.CalculatePoint(u);
        cout << "(" << p.x << ", " << p.y << ", " << p.z<<")" << endl;
        fout2 << p.x << ", " << p.y << ", " << p.z<<endl;
    }
    return 0;
}

int main() {
    return testQUBS();
}

注意：当knots[i + k] - knots[i]等于0时，计算B样条基函数时会出现除0的情况，导致程序崩溃。为了避免这种情况，可以在计算B样条基函数前先判断分母是否为0，如果为0则直接返回0。

下面代码是求节点向量

        int num_knots = control_points.size() + degree+ 1 ;
        double delta = 1.0 /  (double)(num_knots - 2*degree - 1);
        for (int i = 0; i < num_knots; i++) {
            if (i < degree+1) {
               knots.push_back( 0.0);
            } else if (i >= num_knots - degree) {
                knots.push_back(1.0);
            } else {
               knots.push_back(knots.back() + delta);
            }
             cout << "knot"<<i<<"=" << knots[i] <<endl;
        }

输出：

knot0=0
knot1=0
knot2=0
knot3=0.333333
knot4=0.666667
knot5=1
knot6=1
knot7=1
size:8

输出的坐标在Matlab显示：

 对比参考大佬的博客

对比均匀B样条，可见效果非常好