三阶B样条生成方法

B样条计算方法

    // 三次B样条基函数
    double F03(double t) { return 1.0 / 6 * (-t * t * t + 3 * t * t - 3 * t + 1); }
    double F13(double t) { return 1.0 / 6 * (3 * t * t * t - 6 * t * t + 4); }
    double F23(double t) { return 1.0 / 6 * (-3 * t * t * t + 3 * t * t + 3 * t + 1); }
    double F33(double t) { return 1.0 / 6 * t * t * t; }

    ///
    /// \brief generateBSplinePoints 生成三阶B样条路线点
    /// \param length               返回路线长度
    /// \param p0                   起点
    /// \param p1                   终点
    /// \param ctrl0                控制点1
    /// \param ctrl1                控制点2
    /// \param dis                  采样距离,单位mm， 默认20mm
    /// \param num                  每一段路线上的插值,总点数为3*num+4
    /// \return                     路线点集合
    ///
    static QList<QPointF> generateBSplinePoints(int &length,
                                                const QPointF &p0,
                                                const QPointF &p1,
                                                const QPointF &ctrl0,
                                                const QPointF &ctrl1,
                                                int dis = 20,
                                                int num = 100)
    {
        QList<QPointF> tmpath, path;
        tmpath.push_back(p0); //先占位
        tmpath.push_back(p0);
        tmpath.push_back(ctrl0);
        tmpath.push_back(ctrl1);
        tmpath.push_back(p1);
        tmpath.push_back(p1); //先占位

        tmpath[0].setX(2 * tmpath[1].x() - tmpath[2].x()); //  将折线延长线上两点加入作为首点和尾点
        tmpath[0].setY(2 * tmpath[1].y() - tmpath[2].y());

        tmpath[5].setX(2 * tmpath[4].x() - tmpath[3].x());
        tmpath[5].setY(2 * tmpath[4].y() - tmpath[3].y());

        QPointF NodePt1, NodePt2, NodePt3, NodePt4;
        double t;
        int totalnum = 0;
        for (int i = 0; i < 3; i++) {
            NodePt1 = tmpath[i];
            NodePt2 = tmpath[i + 1];
            NodePt3 = tmpath[i + 2];
            NodePt4 = tmpath[i + 3];

            double dt = 1.0 / (num + 1);

            for (int j = 0; j < num + 1; j++) {
                t = dt * j;
                QPointF tmp;
                tmp.setX(F03(t) * NodePt1.x() + F13(t) * NodePt2.x() + F23(t) * NodePt3.x()
                         + F33(t) * NodePt4.x());
                tmp.setY(F03(t) * NodePt1.y() + F13(t) * NodePt2.y() + F23(t) * NodePt3.y()
                         + F33(t) * NodePt4.y());
                path.push_back(tmp);
            }

            if (i == num - 2) { //  最后一个尾点
                t = 1;
                QPointF tmp;
                tmp.setX(F03(t) * NodePt1.x() + F13(t) * NodePt2.x() + F23(t) * NodePt3.x()
                         + F33(t) * NodePt4.x());
                tmp.setY(F03(t) * NodePt1.y() + F13(t) * NodePt2.y() + F23(t) * NodePt3.y()
                         + F33(t) * NodePt4.y());
                path.push_back(tmp);
            }
        }
        //补充终点
        path.push_back(p1);
        double len = 0.0;
        for (int i = 1; i < path.size(); i++) {
            auto p = path[i] - path[i - 1];
            len = len + std::sqrt(p.x() * p.x() + p.y() * p.y());
        }

        length = len;
        return std::move(path);
    }