一、b样条插值
简单粗暴:B-样条曲线入门 - 知乎 (zhihu.com)
【轨迹规划】3:准均匀B样条曲线C++实现_向蓝的博客-CSDN博客
上图红色为折线;黄色为贝塞尔曲线,根据所有控制点确定最终曲线;绿色是b样条曲线,它会根据控制点去弯折。
二、代码
//b样条插值
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;
}
};
用法
//绘制b样条插值曲线
int degree = 2; // 曲线次数
BSplineCurve curve(pts, degree);
step = 0.005;
for (float u = 0; u <= 1; u += step) {
Point p = curve.CalculatePoint(u);
}