贝塞尔曲线计算方法

贝塞尔曲线计算方法

/**
 * @brief The PointF class 基础点结构定义浮点型
 */
class PointF
{
public:
    PointF() {};
    PointF(double _x, double _y) :
        m_x(_x), m_y(_y) {};

    PointF operator-(const PointF& rhs) {
        PointF tem(this->m_x,this->m_y);
        tem.m_x -= rhs.m_x;
        tem.m_y -= rhs.m_y;
        return tem;
    }

    PointF operator+(const PointF& rhs) {
        PointF tem(this->m_x,this->m_y);
        tem.m_x += rhs.m_x;
        tem.m_y += rhs.m_y;
        return tem;
    }

    double mod() const { return std::sqrt(m_x * m_x + m_y * m_y); }
    double x() const { return m_x; }
    double y() const { return m_y; }

public:
    double m_x = 0;
    double m_y = 0;
};
/**
     * @brief SeerBezier    (XG)贝塞尔路线生成方法（3阶）
     * @param path          贝塞尔路段
     * @param length        贝塞尔曲线长度
     * @param PosStart_     路段起点
     * @param ConPoint0_    路段控制点1
     * @param ConPoint1_    路段控制点2
     * @param PosEnd_       路段终点
     * @param num           路段采样数
     */
    static void SeerBezier(std::vector<PointF>& path, double &length,PointF PosStart_, PointF ConPoint0_, PointF ConPoint1_, PointF PosEnd_, int num)
    {
        /*
        输入为路径的4个控制点Point PosStart_, Point ConPoint0_, Point ConPoint1_, Point PosEnd_
            和默认的计算路径总长度的采样间隔 num，程序会先根据默认的采样间隔生成初始路径，然后计算
            路径长度后，以一定的采样距离重新采样，再进行路径的生成
        输出为曲线的路径path
        */
        std::vector<PointF> tmpath;
        tmpath.push_back(PosStart_);
        for (int i = 1; i < num; ++i) {
            double t = (double)i / (double)num;
            double x = pow(1 - t, 3) * PosStart_.x() + 3.0 * t * pow(1 - t, 2) * ConPoint0_.x() +
                3.0 * pow(t, 2) * (1 - t) * ConPoint1_.x() + pow(t, 3) * PosEnd_.x();
            double y = pow(1 - t, 3) * PosStart_.y() + 3.0 * t * pow(1 - t, 2) * ConPoint0_.y() +
                3.0 * pow(t, 2) * (1 - t) * ConPoint1_.y() + pow(t, 3) * PosEnd_.y();
            tmpath.push_back(PointF(x, y));
        }
        tmpath.push_back(PosEnd_);
        double len = 0.0;
        for (int i = 1; i <= num; i++) {
            len = len + (tmpath[i] - tmpath[i - 1]).mod();
        }
        length = len;
        // 25mm是默认采样间隔，可以根据情况修改
        int maxnum = std::max(3, std::min(int(length / 25), num));
        for (int i = 1; i < maxnum; ++i) {
            double t = (double)i / (double)maxnum;
            double x = pow(1 - t, 3) * PosStart_.x() + 3.0 * t * pow(1 - t, 2) * ConPoint0_.x() +
                3.0 * pow(t, 2) * (1 - t) * ConPoint1_.x() + pow(t, 3) * PosEnd_.x();
            double y = pow(1 - t, 3) * PosStart_.y() + 3.0 * t * pow(1 - t, 2) * ConPoint0_.y() +
                3.0 * pow(t, 2) * (1 - t) * ConPoint1_.y() + pow(t, 3) * PosEnd_.y();
            path.push_back(PointF(x, y));
        }
        path.push_back(PosEnd_);
    }

    /**
     * @brief DegenerateBezier  (XG)高级贝塞尔曲线生成方法（5阶）
     * @param path              贝塞尔路段
     * @param length        贝塞尔曲线长度
     * @param PosStart_         路段起点
     * @param ConPoint0_        路段控制点1
     * @param ConPoint1_        路段控制点2
     * @param PosEnd_           路段终点
     * @param num               路段采样数
     */
    static void DegenerateBezier(std::vector<PointF>& path, double &length,PointF PosStart_, PointF ConPoint0_, PointF ConPoint1_, PointF PosEnd_, int num)
    {
        /*
        输入为路径的4个控制点Point PosStart_, Point ConPoint0_, Point ConPoint1_, Point PosEnd_
            和默认的计算路径总长度的采样间隔 num，程序会先根据默认的采样间隔生成初始路径，然后计算
            路径长度后，以一定的采样距离重新采样，再进行路径的生成
        输出为曲线的路径path
        */
        auto getv = [](double p0, double p1, double p2, double p3, double p4, double p5, int num, std::vector<double>& v) {
            v.clear();
            for (int i = 1; i < num; i++) {
                double t = (double)i / (double)num;
                double s = 1 - t;
                double x = p0 * s * s * s * s * s
                    + 5.0 * p1 * s * s * s * s * t
                    + 10.0 * p2 * s * s * s * t * t
                    + 10.0 * p3 * s * s * t * t * t
                    + 5.0 * p4 * s * t *t * t * t
                    + p5 * t * t * t * t * t;
                v.push_back(x);
            }
        };
        std::vector<double> x;
        std::vector<double> y;
        getv(PosStart_.x(), ConPoint0_.x(), ConPoint0_.x(), ConPoint1_.x(), ConPoint1_.x(), PosEnd_.x(), num, x);
        getv(PosStart_.y(), ConPoint0_.y(), ConPoint0_.y(), ConPoint1_.y(), ConPoint1_.y(), PosEnd_.y(), num, y);
        std::vector<PointF> tmpath;
        tmpath.push_back(PosStart_);
        for (int i = 0; i < x.size(); i++) {
            tmpath.emplace_back(PointF(x[i], y[i]));
        }
        tmpath.push_back(PosEnd_);
        double len = 0.0;
        for (int i = 1; i <= num; i++) {
            len = len + (tmpath[i] - tmpath[i - 1]).mod();
        }
        length = len;
        // 25mm是默认采样间隔，可以根据情况修改
        int maxnum = std::max(3, std::min(int(length / 25 ), num));
        x.clear();
        y.clear();
        getv(PosStart_.x(), ConPoint0_.x(), ConPoint0_.x(), ConPoint1_.x(), ConPoint1_.x(), PosEnd_.x(), maxnum, x);
        getv(PosStart_.y(), ConPoint0_.y(), ConPoint0_.y(), ConPoint1_.y(), ConPoint1_.y(), PosEnd_.y(), maxnum, y);
        path.clear();
        for (int i = 0; i < x.size(); i++) {
            path.emplace_back(PointF(x[i], y[i]));
        }
        path.push_back(PosEnd_);
    }