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

具体原理：转天真的和感伤的想象家的博客的好文。

C++代码

#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> knot_vector; // 结点向量
    int degree; // 曲线次数
    //Constructor
    BSplineCurve(vector<Point> points, int k) {
        control_points = points;
        degree = k;
        // 初始化结点向量, m = n + 1+ k   ,m节点数量, n+1控制点数量 ,k 次数
        for (int i = 0; i <= control_points.size() +degree +1; i++) {         
            knot_vector.push_back(i/ (float)(control_points.size() + degree + 1.0));            
            cout << "knot"<<i<<"=" << knot_vector[i] <<endl;
        } 
    }
    // 计算基函数值
    float BasisFunction(int i, int k, float u) {
        if (k == 0) {
            if (u >= knot_vector[i] && u < knot_vector[i+1]) {            
                return 1;
            }
            else {               
                return 0;
            }
        }
        float a = (u - knot_vector[i]) / float(knot_vector[i+k] - knot_vector[i]);
        float b = (knot_vector[i+k+1] - u) / float(knot_vector[i+k+1] - knot_vector[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 main() {
    ofstream fout("D:/data.txt"); // 打开或创建名为 data.txt 的文件
    if (!fout) { // 检查文件是否打开成功
        cerr << "File open failed!" << endl;
        return -1;
    }
    vector<Point> points = {{50,50,50},{100,300,150},{300,100,200},{380,200,80},{400,600,500}}; // 控制点
    int degree = 4; // 曲线次数
    BSplineCurve curve(points, degree);
    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;
        fout << p.x << ", " << p.y << ", " << p.z<<endl;
    }

    return 0;
}

 可以看到曲线并不理想。