使用c++-Go服务器开发

在C++中进行路径规划可以使用多种算法。以下是一个示例，演示如何使用Dijkstra算法在二维空间中实现路径规划，同时考虑一个多边形障碍物。这个示例简化了一些细节，以便于理解基本思路。

1. 基本数据结构

我们需要一些基本的结构来表示点和边。

#include <iostream>
#include <vector>
#include <set>
#include <cmath>
#include <limits>

using namespace std;

struct Point {
    double x, y;

    bool operator<(const Point& other) const {
        return tie(x, y) < tie(other.x, other.y);
    }
};

2. 判断线段相交

需要一个函数来判断两条线段是否相交，这对于处理障碍物是非常重要的。

bool onSegment(Point p, Point q, Point r) {
    return (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) &&
            q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y));
}

int orientation(Point p, Point q, Point r) {
    double val = (q.y - p.y) * (r.x - q.x) -
                 (q.x - p.x) * (r.y - q.y);
    
    if (val == 0) return 0; // collinear
    return (val > 0) ? 1 : 2; // clockwise or counterclockwise
}

bool doIntersect(Point p1, Point q1, Point p2, Point q2) {
    int o1 = orientation(p1, q1, p2);
    int o2 = orientation(p1, q1, q2);
    int o3 = orientation(p2, q2, p1);
    int o4 = orientation(p2, q2, q1);

    if (o1 != o2 && o3 != o4)
        return true;
    
    return false;
}

3. Dijkstra算法实现

接下来，使用Dijkstra算法计算从起点到终点的最短路径。

struct Edge {
    Point target;
    double weight;
};

double distance(const Point& a, const Point& b) {
    return sqrt((a.x - b.x)*(a.x - b.x) + (a.y - b.y)*(a.y - b.y));
}

vector<Point> dijkstra(const vector<Point>& points,
                       const vector<vector<bool>>& adjacencyMatrix,
                       const Point& start,
                       const Point& end) {

    set<pair<double, Point>> pq;
    map<Point,double> dist;
    
    for(const auto& point : points){
        dist[point] = numeric_limits<double>::infinity();
    }
    
   dist[start] = 0.0;
   pq.insert(make_pair(0.0,start));

   while (!pq.empty()) {
       auto currentPair = *pq.begin();
       pq.erase(pq.begin());
       
       Point currentNode = currentPair.second;

       if(currentNode == end){
           break; 
       }

       for(size_t i=0; i<points.size(); ++i){
           if(adjacencyMatrix[currentNode][points[i]]){
               double newDist = dist[currentNode] + distance(currentNode , points[i]);
               
               if(newDist < dist[points[i]]){
                   pq.erase(make_pair(dist[points[i]], points[i]));
                   dist[points[i]] = newDist;
                   pq.insert(make_pair(newDist , points[i]));
               }
           }
       }
   }

   vector<Point> path;

   // 后续恢复路径逻辑（如需）
   
   return path;
}

4. 主函数与测试用例

在主函数中，可以设置起始点、终止点以及多边形障碍，并调用上述方法：

int main() {
   // 定义多边形顶点（作为示例）
   vector<Point> polygonPoints = {{1.0 , 1.0}, {3.0 , 3.5}, {5.0 , 3.5}, {6.0 , 1.5}};
   
   // 假设有个起始与结束位置
   Point start{0., 0.};
   Point end{7., 4.};

   // 构建邻接矩阵，根据实际情况决定何时连接两个节点。
   size_t n=polygonPoints.size();
   
   vector<vector<bool>> adjacencyMatrix(n+2 , vector<bool>(n+2 , false));

   
// 检查各个对之间是否可通行以填充邻接矩阵
    
for(size_t i=0;i<n+2;++i){
     for(size_t j=i+1;j<n+2;++j){
         bool canConnect=true;
         if(doIntersect(start,end)){
             canConnect=false; 
         }
         adjacencyMatrix[i][j]=canConnect;
         adjacencyMatrix[j][i]=canConnect;         
     }
 } 

// 调用 Dijkstra 算法找路径。
vector<Point> path=dijkstra(points.adjacencyMatrix,start,end);

// 打印结果，如果找到了有效路径.
if(!path.empty()){
     cout << "找到路径!" << endl ;
     for(auto &p: path)
        cout<<"("<<p.x<<","<<p,y<<")"<<"\n";
}else{
      cout <<"无有效路径！" <<endl ;
}
return 0;
}

以上代码提供了一个基础框架，你可以根据需求扩展功能，比如：