三次多项轨迹规划C++ qt中的代码

以下是一个简单的三次多项式轨迹规划的C++代码，使用Qt框架绘制轨迹图形：

#include <QtGui/QApplication>
#include <QtGui/QPainter>
#include <QtGui/QWidget>
#include <QtCore/QDebug>

#include <vector>
#include <cmath>

const double kStepTime = 0.05;
const double kMaxVelocity = 1.0;
const double kMaxAcceleration = 1.0;

struct Point {
    double x;
    double y;
};

struct Trajectory {
    std::vector<Point> points;
    std::vector<double> times;
};

Trajectory GenerateTrajectory(const Point& start, const Point& end) {
    double dx = end.x - start.x;
    double dy = end.y - start.y;
    double distance = std::sqrt(dx * dx + dy * dy);

    double v_max = kMaxVelocity;
    double a_max = kMaxAcceleration;
    double t_ramp = v_max / a_max;
    double ramp_distance = 0.5 * a_max * t_ramp * t_ramp;

    double t_coast = (distance - 2.0 * ramp_distance) / v_max;
    if (t_coast < 0.0) {
        v_max = std::sqrt(distance * a_max);
        t_ramp = v_max / a_max;
        ramp_distance = 0.5 * a_max * t_ramp * t_ramp;
        t_coast = 0.0;
    }

    double total_time = 2.0 * t_ramp + t_coast;
    int num_steps = static_cast<int>(std::ceil(total_time / kStepTime));
    double actual_step_time = total_time / num_steps;

    std::vector<double> times(num_steps);
    std::vector<Point> points(num_steps);

    for (int i = 0; i < num_steps; ++i) {
        double t = i * actual_step_time;
        times[i] = t;

        if (t < t_ramp) {
            double a = a_max;
            double v = a * t;
            double x = start.x + 0.5 * a * t * t;
            double y = start.y + v * (dy / distance) * t;
            points[i] = { x, y };
        } else if (t < t_ramp + t_coast) {
            double x = start.x + v_max * (dx / distance) * (t - t_ramp);
            double y = start.y + v_max * (dy / distance) * (t - t_ramp);
            points[i] = { x, y };
        } else {
            double a = -a_max;
            double v = v_max + a * (t - t_ramp - t_coast);
            double x = end.x - 0.5 * a * (total_time - t) * (total_time - t);
            double y = end.y + v * (dy / distance) * (total_time - t);
            points[i] = { x, y };
        }
    }

    return { points, times };
}

class TrajectoryWidget : public QWidget {
public:
    TrajectoryWidget(QWidget* parent = nullptr) : QWidget(parent) {}

    void SetTrajectory(const Trajectory& trajectory) {
        trajectory_ = trajectory;
        update();
    }

protected:
    void paintEvent(QPaintEvent*) override {
        QPainter painter(this);
        painter.setRenderHint(QPainter::Antialiasing);

        if (trajectory_.points.empty()) {
            return;
        }

        QColor color(0, 0, 255);
        painter.setPen(QPen(color, 2.0));

        for (int i = 1; i < trajectory_.points.size(); ++i) {
            const Point& p1 = trajectory_.points[i - 1];
            const Point& p2 = trajectory_.points[i];
            painter.drawLine(QPointF(p1.x, p1.y), QPointF(p2.x, p2.y));
        }
    }

private:
    Trajectory trajectory_;
};

int main(int argc, char* argv[]) {
    QApplication app(argc, argv);

    TrajectoryWidget widget;
    widget.setGeometry(100, 100, 600, 600);
    widget.show();

    Point start = { 100.0, 100.0 };
    Point end = { 500.0, 500.0 };
    Trajectory trajectory = GenerateTrajectory(start, end);
    widget.SetTrajectory(trajectory);

    return app.exec();
}

在这个例子中，我们首先定义了一个 `Point` 结构体来表示二维平面上的一个点，另外还定义了一个 `Trajectory` 结构体来表示整个轨迹。然后，我们实现了一个 `GenerateTrajectory()` 函数来计算从起点到终点的三次多项式轨迹。该函数首先根据起点和终点的坐标计算出两点之间的距离，然后根据给定的最大速度和最大加速度计算出过渡段（即加速段和减速段）的时间和距离，以及匀速段的时间。接着，我们将整个轨迹分为若干个时间步长，并在每个时间步长内计算出对应的位置和时间。最后，我们将所有的位置和时间存储在 `Trajectory` 结构体中并返回。

为了在界面中显示轨迹，我们使用了一个 `TrajectoryWidget` 类来绘制轨迹图形。该类继承自 `QWidget`，重载了 `paintEvent()` 函数，在其中使用 `QPainter` 类来绘制轨迹。我们还实现了一个 `SetTrajectory()` 函数来设置要显示的轨迹，该函数将轨迹存储在成员变量 `trajectory_` 中并调用 `update()` 函数来触发重绘。

在 `main()` 函数中，我们首先创建了一个 `TrajectoryWidget` 对象，并设置其大小和位置，然后调用 `GenerateTrajectory()` 函数生成轨迹，并将其传递给 `TrajectoryWidget` 对象来显示。

注意：这只是一个简单的示例，实际的三次多项式轨迹规划可能需要更复杂的算法和数据结构来处理各种情况。