Conception/任务规划.h

#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>

using namespace std;

// 坐标类
class Point {
public:
    double x, y;
    Point(double nx, double ny) : x(nx), y(ny) {}
};

// 图类
class Graph {
public:
    Graph(vector<Point> points) {
        for (int i = 0; i < points.size(); i++) {
            vector<double> row;
            for (int j = 0; j < points.size(); j++) {
                if (i == j) {
                    row.push_back(0);
                } else {
                    double dis = sqrt(pow(points[i].x - points[j].x, 2) + pow(points[i].y - points[j].y, 2));
                    row.push_back(dis);
                }
            }
            matrix.push_back(row);
        }
    }

    // Dijkstra算法求解最短路径，网上嫖的
    vector<Point> get_shortest_path(int start_index, int end_index) {
        int node_num = matrix.size();
        vector<bool> visited(node_num, false);
        vector<int> prev(node_num, -1);
        vector<double> dist(node_num, INFINITY);
        dist[start_index] = 0;

        // Dijkstra算法
        for (int i = 0; i < node_num; i++) {
            int u = -1;
            double min_dist = INFINITY;
            // 找到未访问过的距离最小的节点u
            for (int j = 0; j < node_num; j++) {
                if (!visited[j] && dist[j] < min_dist) {
                    u = j;
                    min_dist = dist[j];
                }
            }
            if (u == -1) break;
            visited[u] = true;
            // 更新u的邻接节点v的距离和前驱
            for (int v = 0; v < node_num; v++) {
                if (!visited[v] && matrix[u][v] < INFINITY) {
                    double new_dist = dist[u] + matrix[u][v];
                    if (new_dist < dist[v]) {
                        dist[v] = new_dist;
                        prev[v] = u;
                    }
                }
            }
        }

        // 输出最短路径
        vector<Point> path;
        int u = end_index;
        while (prev[u] != -1) {
            path.push_back(points[u]);
            u = prev[u];
        }
        reverse(path.begin(), path.end());

        return path;
    }

private:
    vector<vector<double>> matrix; // 邻接矩阵，用来存放
    vector<Point> points; // 顶点坐标列表
    const double INFINITY = std::numeric_limits<double>::infinity(); // 无穷大
};

// 顶点序列类
class VertexSequence {
public:
    // 构造函数
    VertexSequence(vector<Point> points, Graph graph) {
        this->points = points;
        this->graph = graph;
        start_index = 0; // 初始起点为第一个点
        end_index = -1; // 结尾没有定义
        sequence = graph.get_shortest_path(start_index, end_index); // 计算最短路径
    }

    // 重新排序方法
    void sort(Point current_point) {
        // 计算起点到各顶点的距离
        vector<double> distances;
        for (int i = 0; i < sequence.size(); i++) {
            double dis = sqrt(pow(current_point.x - sequence[i].x, 2) + pow(current_point.y - sequence[i].y, 2));
            distances.push_back(dis);
        }
        // 对顶点序列进行重新排序
        sort(sequence.begin(), sequence.end(), [&distances](const Point& lhs, const Point& rhs) {
            int index_lhs = &lhs - &distances[0];
            int index_rhs = &rhs - &distances[0];
            return distances[index_lhs] < distances[index_rhs];
        });
    }

    Point pop() {
        Point next_point = sequence.front();
        sequence.erase(sequence.begin());
        return next_point;
    }

private:
    vector<Point> points; // 顶点坐标
    Graph graph; // 连通图
    int start_index; // 起点
    int end_index; // 结尾
    vector<Point> sequence; // 顶点序列
};