B样条曲线绘制

计算机中绘制曲线，通过贝塞尔曲线已经满足了我们大部分需求，但是其存在某些缺点，比如移动某一个控制点会导致整个曲线发生变化，即无法局部控制曲线的走向。所以 B 样条曲线(B-Spline)为了解决贝塞尔曲线的缺陷应运而生。

什么是 B 样条曲线？
解释 B 样条曲线之前，首先要解释一下什么是样条。样条是通过一组指定点集而生成平滑曲线的柔性带。简单地说，B 样条曲线就是通过控制点局部控制形状的曲线。不太理解的同学可以通过本文底部的 demo 查看 B 样条曲线中，控制点对曲线绘制的影响。

最近工作要用B样条曲线，就花时间研究了下。
生成B样条曲线 $C$ 首先需要有一系列控制点 $p_i(i = 0, \cdots, n-1)$ ，然后在B样条曲线看来，绘制主要就是插值。整体思想是按照一定的顺序把控制点投影到一个一维区间，控制点投影到一维区域所在的位置叫做节点 $knot_i$ ，和 $p_i$ 一一对应。在这个一维区间上均匀取值，然后计算出在原空间对应的位置即可得到 $C$ 。

控制点和节点对应关系

现在有一系列高维空间控制点 $p_0, p_1, \cdots, p_{n - 1}$ ，一种很显然的方式是将他们按照顺序对应为一维空间的 $0, \frac{1}{n - 1}, \frac{2}{n - 1}, \cdots, \frac{n - 2}{n- 1}, 1$ ，考虑到 $m$ 阶B样条插值每个点需要 $m + 1$ 个节点来控制生成，因此为了生成头尾两个控制点，节点的数量为 $n + m + 1$ ，一般节点的生成方法有均匀法，这里为了过头尾两个控制点，将节点设置成 $knot: \bigg\{ 0, \cdots, 0, \frac{1}{n - m}, \frac{2}{n - m}, \cdots, \frac{n - m - 1}{n - m}, 1,\cdots, 1 \bigg\}$ ，前后各有 $m + 1$ 个 $0$ 和 $1$ ，分别用来生成第一个控制点和最后一个控制点。
然后从区间 $[0, 1]$ 里面均匀采样，比如采样到 $t$ 的位置，该位置对应的高维空间点为
$C_{deg}(t) = \sum_{0}^{n - 1} B_{i, deg}(t)p_i$

可以看到其中的关键是求解控制点对应的贡献或者说是权重 $B_{i, deg}(t)$ 。这里首先来看0阶的权重，如下所示
$B_{i, 0}(t) = \begin{cases} 1 & {knot_i \leq t < knot_{i + 1}} \\ 0 &\text{otherwise} \end{cases}$ 这可以认为是一个最近邻插值，实现后的效果类似信号处理里面0阶保持。
高阶的权重通过如下的递推式子得到
$B_{i, deg}(t) = \frac{t - knot_i}{knot_{i + deg} - kont_i} B_{i, deg - 1}(t) + \frac{kont_{i + deg + 1} - t}{knot_{i+deg+1} - knot_{i +1}} B_{i + 1, deg - 1}(t)$

如果把 $knot_i$ 简写成 $k_i$ ，则有
$B_{i, deg}(t) = \frac{t - k_i}{k_{i + deg} - k_i} B_{i, deg - 1}(t) + \frac{k_{i + deg + 1} - t}{k_{i+deg+1} - k_{i +1}} B_{i + 1, deg - 1}(t)$

按照上面的公式得到的C++程序如下所示

void BSpline(vector<double> &point_x, vector<double> &point_y,
    vector<double> &plan_path_x, vector<double> &plan_path_y, int order = 3) {
    int knot_parameter = point_x.size() + order;
    vector<double> knot(knot_parameter + 1, 0.0);
    vector<double> b(knot_parameter * (order + 1), 0.0);

    for (int i = order; i < knot.size(); ++i) 
        knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
    
    for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
        double t = i / (plan_path_x.size() - 0.0);

        for (int j = 0; j < knot_parameter; ++j) {
            if (knot[j] <= t && knot[j + 1] > t) b[j] = 1.0;
            else b[j] = 0.0;
        }

        for (int deg = 1; deg <= order; ++deg) {
            for (int j = 0; j + deg < knot_parameter; ++j) {
                b[deg * knot_parameter + j] = 0.0;
                if (knot[j + deg] != knot[j]) 
                    b[deg * knot_parameter + j] += 
                    (t - knot[j]) / (knot[j + deg] - knot[j]) * b[(deg - 1) * knot_parameter + j];
                if (knot[j + deg + 1] != knot[j + 1]) 
                    b[deg * knot_parameter + j] += 
                    (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j + 1]) * b[(deg - 1) * knot_parameter + j + 1];
            }
        }

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = 0; j < point_x.size(); ++j) {
            plan_path_x[i] += point_x[j] * b[order * knot_parameter + j];
            plan_path_y[i] += point_y[j] * b[order * knot_parameter + j];
        }
    }

    plan_path_x.back() = point_x.back();
    plan_path_y.back() = point_y.back();
}

注意到系数矩阵b当前 $deg$ 的值只取决于 $deg - 1$ 的值和前面更前面的值无关，按照动态规划的常规套路可以将系数矩阵进行压缩，优化后的代码如下所示

void BSpline(vector<double> &point_x, vector<double> &point_y,
    vector<double> &plan_path_x, vector<double> &plan_path_y, int order = 3) {
    int knot_parameter = point_x.size() + order;
    vector<double> knot(knot_parameter + 1, 0.0);
    vector<double> b(knot_parameter, 0.0);

    for (int i = order; i < knot.size(); ++i)
        knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);

    for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
        double t = i / (plan_path_x.size() - 0.0);

        for (int j = 0; j < knot_parameter; ++j) {
            if (knot[j] <= t && knot[j + 1] > t) b[j] = 1.0;
            else b[j] = 0.0;
        }

        for (int deg = 1; deg <= order; ++deg) {
            for (int j = 0; j + deg < knot_parameter; ++j) {
                if (knot[j + deg] != knot[j])
                    b[j] = (t - knot[j]) / (knot[j + deg] - knot[j]) * b[j];
                else b[j] = 0.0;
                if (knot[j + deg + 1] != knot[j + 1])
                    b[j] += (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j + 1]) * b[j + 1];
            }
        }

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = 0; j < point_x.size(); ++j) {
            plan_path_x[i] += point_x[j] * b[j];
            plan_path_y[i] += point_y[j] * b[j];
        }
    }

    plan_path_x.back() = point_x.back();
    plan_path_y.back() = point_y.back();
}

注意到根据前面的0阶表达式，每个 $t$ 只有一个 $B_{i, 0}$ 不为0，剩下的均为0，因此在阶数较小的情况下可以考虑直接写出表达式，注意到下面表达式中的 $i$ 均满足 $k_i \leq t < k_{i + 1}$ 。
考虑到 $B_{i, 0}(t) = 1$ ，因此0阶情况下的表达式为
$\begin{aligned} C_0(t) &= B_{i, 0}(t)p_i\\ &= p_i \end{aligned}$

考虑到 $B_{i, 1}(t) = \frac{t - k_i}{k_{i + 1} - k_i} B_{i, 0}(t) + \frac{k_{i + 2} - t}{k_{i+2} - k_{i +1}} B_{i + 1, 0}(t)$ ，因此1阶情况下的表达式为
$\begin{aligned} C_1(t) &= B_{i-1, 1}(t)p_{i-1} + B_{i, 1}(t)p_{i}\\ &= \frac{k_{i+1} - t}{k_{i + 1} - k_i} p_{i - 1} + \frac{t - k_i}{k_{i + 1} - k_i}p_i \end{aligned}$

考虑到 $B_{i, 2}(t) = \frac{t - k_i}{k_{i + 2} - k_i} B_{i, 1}(t) + \frac{k_{i + 3} - t}{k_{i+3} - k_{i +1}} B_{i+1, 1}(t)$ ，因此2阶情况下的表达式为
$\begin{aligned} C_2(t) &= B_{i - 2, 2}(t)p_{i - 2} + B_{i-1, 2}(t)p_{i-1} + B_{i, 2}(t)p_{i}\\ &= \frac{k_{i+1} - t}{k_{i + 1} - k_{i - 1}} B_{i-1, 1}(t)p_{i - 2} + \\ &\frac{t - k_{i - 1}}{k_{i + 1} - k_{i - 1}}B_{i - 1,1}(t)p_{i - 1} + \frac{k_{i +2} - t}{k_{i +2} - k_i}B_{i,1}(t)p_{i -1}+\\ &\frac{t - k_i}{k_{i+2} - k_i}B_{i,1}(t)p_i\\ &= \frac{k_{i+1} - t}{k_{i + 1} - k_{i - 1}} \frac{k_{i+1} - t}{k_{i+1} - k_i}p_{i - 2} +\\ &\frac{t - k_{i - 1}}{k_{i + 1} - k_{i - 1}}\frac{k_{i+1} - t}{k_{i+1}-k_i}p_{i -1} + \frac{k_{i +2} - t}{k_{i +2} - k_i}\frac{t - k_i}{k_{i+1} - k_i}p_{i -1} +\\ &\frac{t - k_i}{k_{i+2} - k_i}\frac{t - k_i}{k_{i +1}-k_i}p_i \end{aligned}$

考虑到 $B_{i, 3}(t) = \frac{t - k_i}{k_{i + 3} - k_i} B_{i, 2}(t) + \frac{k_{i + 4} - t}{k_{i+4} - k_{i +1}} B_{i + 1, 2}(t)$ ，因此3阶情况下的表达式为
$\begin{aligned} C_3(t) &= B_{i - 3, 3}(t)p_{i - 3} + B_{i-2, 3}(t)p_{i-2} + B_{i - 1, 3}(t)p_{i - 1} + B_{i, 3}(t)p_{i}\\ \end{aligned}$

注意到每个点只由 $m +1$ 个控制点生成，计算系数的时候不需要遍历整个节点和控制点，只需要遍历你所需要的那几个即可，因此前面的代码可以优化为

void BSpline(vector<double> &point_x, vector<double> &point_y,
    vector<double> &plan_path_x, vector<double> &plan_path_y, int order = 3) {
    int cur_index = 0;
    int knot_parameter = point_x.size() + order;
    vector<double> knot(knot_parameter + 1, 0.0);
    vector<double> b(knot_parameter, 0.0);

    for (int i = order; i < knot.size(); ++i) 
        knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
    
    for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
        double t = i / (plan_path_x.size() - 0.0);

        while (knot[cur_index] <= t) ++cur_index;
        b[cur_index - 1] = 1.0;

        for (int deg = 1; deg <= order; ++deg) {
            for (int j = cur_index - deg - 1; j < cur_index; ++j) {
                if (knot[j + deg] != knot[j]) 
                    b[j] = (t - knot[j]) / (knot[j + deg] - knot[j]) * b[j];
                else b[j] = 0.0;
                if (knot[j + deg + 1] != knot[j + 1]) 
                    b[j] += (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j + 1]) * b[j + 1];
            }
        }

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = cur_index - order - 1; j < cur_index; ++j) {
            plan_path_x[i] += point_x[j] * b[j];
            plan_path_y[i] += point_y[j] * b[j];
            b[j] = 0.0;
        }
    }

    plan_path_x.back() = point_x.back();
    plan_path_y.back() = point_y.back();
}

前面的公式
$B_{i, deg}(t) = \frac{t - k_i}{k_{i + deg} - k_i} B_{i, deg - 1}(t) + \frac{k_{i + deg + 1} - t}{k_{i+deg+1} - k_{i +1}} B_{i + 1, deg - 1}(t)$

从数学上来看，系数 $(i, deg)$ 是由上一层 $(i, deg - 1)$ 和 $(i + 1, deg - 1)$ 两者共同组成，从另一个角度来看 $(i, deg)$ 的系数支持了下一层的 $(i - 1, deg + 1)$ 和 $(i, deg + 1)$ 两者。前面的两段代码采用的是前者的写法，考虑到整个数据最开始只有一个地方为1，因此采用后者写法可以进一步减少计算量，只计算已知非0值的位置。后一种写法的数学公式表达如下
$\begin{aligned} \frac{k_{i + deg + 1} - t}{k_{i + deg + 1} - k_i} B_{i, deg}(t) &\to B_{i - 1, deg + 1}(t)\\ \frac{t - k_i}{k_{i+deg+1} - k_{i}} B_{i, deg}(t) &\to B_{i, deg + 1}(t) \end{aligned}$

注意到上式中的系数满足 $\frac{k_{i + deg + 1} - t}{k_{i + deg + 1} - k_i} +\frac{t - k_i}{k_{i+deg+1} - k_{i}} = 1$ ，在计算时可以利用该性质，减少计算量。
进一步优化过后的代码如下

void BSpline(vector<double> &point_x, vector<double> &point_y,
    vector<double> &plan_path_x, vector<double> &plan_path_y, int order = 3) {
    int cur_index = 0;
    int knot_parameter = point_x.size() + order;
    vector<double> knot(knot_parameter + 1, 0.0);
    vector<double> b(knot_parameter, 0.0);

    for (int i = order; i < knot.size(); ++i) 
        knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
    
    for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
        double t = i / (plan_path_x.size() - 0.0);

        while (knot[cur_index] <= t) ++cur_index;
        b[cur_index - 1] = 1.0;

        for (int deg = 0; deg < order; ++deg) {
            for (int j = cur_index - deg - 1; j < cur_index; ++j) {
                double p = 0.0;
                if (knot[j + deg + 1] != knot[j])
                    p = (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j]);

                b[j - 1] += p * b[j];
                b[j] *= (1.0 - p);
            }
        }

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = cur_index - order - 1; j < cur_index; ++j) {
            plan_path_x[i] += point_x[j] * b[j];
            plan_path_y[i] += point_y[j] * b[j];
            b[j] = 0.0;
        }
    }

    plan_path_x.back() = point_x.back();
    plan_path_y.back() = point_y.back();
}

继续优化得到代码

void BSpline(vector<double>& point_x, vector<double>& point_y,
    vector<double>& plan_path_x, vector<double>& plan_path_y, int order = 3) {
    int cur_index = 0;
    double step = (point_x.size() - order + 0.0) / plan_path_x.size();
    int knot_parameter = point_x.size() + order;
    vector<int> knot(knot_parameter + 1, 0.0);
    vector<double> b(knot_parameter, 0.0);

    for (int i = order; i < knot.size(); ++i) knot[i] = min(i, (int)point_x.size()) - order;

    for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
        double t = i * step;

        while (knot[cur_index] <= t) ++cur_index;
        b[cur_index - 1] = 1.0;

        for (int deg = 0; deg < order; ++deg) {
            for (int j = cur_index - deg - 1; j < cur_index; ++j) {
                int knot_temp = knot[j + deg + 1];
                double p = knot_temp - knot[j];
                if (p) p = (knot_temp - t) / p;

                b[j - 1] += p * b[j];
                b[j] *= (1.0 - p);
            }
        }

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = cur_index - order - 1; j < cur_index; ++j) {
            plan_path_x[i] += point_x[j] * b[j];
            plan_path_y[i] += point_y[j] * b[j];
            b[j] = 0.0;
        }
    }

    plan_path_x.back() = point_x.back();
    plan_path_y.back() = point_y.back();
}

注意到设置 $knot$ 的时候，除了头尾添加的0和1以外，节点 $knot$ 等分了区间 $[0,1]$ ，利用该性质对前面的递推公式做变换可以得到
$\begin{aligned} B_{i, deg}(t) &= \frac{t - k_i}{k_{i + deg} - k_i} B_{i, deg - 1}(t) + \frac{k_{i + deg + 1} - t}{k_{i+deg+1} - k_{i +1}} B_{i + 1, deg - 1}(t) \\ &= \frac{t - k_i}{k_{i + 1} - k_i}\frac{k_{i + 1} - k_i}{k_{i + deg} - k_i}B_{i, deg - 1}(t) + \\ & \bigg(1 + \frac{k_{i + 1} - t}{k_{i+deg+1} - k_{i +1}} \bigg) B_{i + 1, deg - 1}(t) \\ &= \frac{t - k_i}{k_{i + 1} - k_i}\frac{k_{i + 1} - k_i}{k_{i + deg} - k_i}B_{i, deg - 1}(t) + \\ & \bigg(1 + \frac{k_{i + 1} - t}{k_{i + 1} - k_i}\frac{k_{i + 1} - k_i}{k_{i+deg+1} - k_{i +1}} \bigg) B_{i + 1, deg - 1}(t) \\ &= \frac{t - k_i}{k_{i + 1} - k_i}\frac{1}{deg}B_{i, deg - 1}(t) + \bigg(1 + \frac{k_{i + 1} - t}{k_{i + 1} - k_i}\frac{1}{deg} \bigg) B_{i + 1, deg - 1}(t) \end{aligned}$
令$$

没时间写了，先放上程序再说

void BSpline(vector<double>& point_x, vector<double>& point_y,
    vector<double>& plan_path_x, vector<double>& plan_path_y, int order = 3) {
    int cur_index = 0;
    double step = (point_x.size() - 1.0) / (plan_path_x.size() - 1.0);
    vector<int> knot(point_x.size() + 2 * order, 0.0);
    vector<double> b(point_x.size() + order, 0.0);

    for (int i = 0; i < knot.size(); ++i) knot[i] = i + 1 - order;

    for (int i = 1; i + 1 < plan_path_x.size(); ++i) {
        double t = i * step;

        while (knot[cur_index] <= t) ++cur_index;
        b[cur_index] = 1.0;

        for (int deg = 0; deg < order; ++deg) {
            for (int j = cur_index - deg; j <= cur_index; ++j) {
                int knot_temp = knot[j + deg];
                double p = (knot_temp - t) / (knot_temp - knot[j - 1]);

                b[j - 1] += p * b[j];
                b[j] *= (1.0 - p);
            }
        }

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = cur_index - order; j <= cur_index; ++j) {
            int point_index = min(max(j - order / 2, 0), (int)point_x.size() - 1);
            plan_path_x[i] += point_x[point_index] * b[j];
            plan_path_y[i] += point_y[point_index] * b[j];
            b[j] = 0.0;
        }
    }

    plan_path_x[0] = point_x[0];
    plan_path_y[0] = point_y[0];

    plan_path_x.back() = point_x.back();
    plan_path_y.back() = point_y.back();
}

void BSpline3(vector<double>& point_x, vector<double>& point_y,
    vector<double>& plan_path_x, vector<double>& plan_path_y, int order = 3) {
    int cur_index = 0;
    double step = (point_x.size() - 1.0) / (plan_path_x.size() - 1.0);
    vector<double> point_x_extended(point_x.size() + order, 0.0);
    vector<double> point_y_extended(point_y.size() + order, 0.0);
    vector<int> knot(point_x.size() + order, 0.0);
    vector<double> b(order + 1, 0.0);

    for (int i = 0; i < knot.size(); ++i) knot[i] = i + 1 - order;

    for (int i = 0; i < point_x_extended.size(); ++i) {
        int ii = i - order / 2;

        if (ii < 0) {
            point_x_extended[i] = 2 * point_x[0] - point_x[1];
            point_y_extended[i] = 2 * point_y[0] - point_y[1];
            continue;
        }

        if (ii >= point_x.size()) {
            point_x_extended[i] = 2 * point_x[point_x.size() - 1] - point_x[point_x.size() - 2];
            point_y_extended[i] = 2 * point_y[point_x.size() - 1] - point_y[point_x.size() - 2];
            continue;
        }

        point_x_extended[i] = point_x[ii];
        point_y_extended[i] = point_y[ii];
    }

    for (int i = 0; i < plan_path_x.size(); ++i) {
        double t = i * step;

        while (knot[cur_index] <= t) ++cur_index;

        t = t - knot[cur_index - 1];

        b[0] = (-t * t * t + 3 * t * t - 3 * t + 1) / 6.0;
        b[1] = (3 * t * t * t - 6 * t * t + 4) / 6.0;
        b[2] = (-3 * t * t * t + 3 * t * t + 3 * t + 1) / 6.0;
        b[3] = t * t * t / 6.0;

        plan_path_x[i] = 0.0;
        plan_path_y[i] = 0.0;
        for (int j = 0; j <= order; ++j) {
            plan_path_x[i] += point_x_extended[cur_index - order + j] * b[j];
            plan_path_y[i] += point_y_extended[cur_index - order + j] * b[j];
        }
    }
}