ZBLOG

使用c++

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

1. 基本数据结构

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

  1. #include <iostream>
  2. #include <vector>
  3. #include <set>
  4. #include <cmath>
  5. #include <limits>
  6. using namespace std;
  7. struct Point {
  8. double x, y;
  9. bool operator<(const Point& other) const {
  10. return tie(x, y) < tie(other.x, other.y);
  11. }
  12. };

2. 判断线段相交

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

  1. bool onSegment(Point p, Point q, Point r) {
  2. return (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) &&
  3. q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y));
  4. }
  5. int orientation(Point p, Point q, Point r) {
  6. double val = (q.y - p.y) * (r.x - q.x) -
  7. (q.x - p.x) * (r.y - q.y);
  8. if (val == 0) return 0; // collinear
  9. return (val > 0) ? 1 : 2; // clockwise or counterclockwise
  10. }
  11. bool doIntersect(Point p1, Point q1, Point p2, Point q2) {
  12. int o1 = orientation(p1, q1, p2);
  13. int o2 = orientation(p1, q1, q2);
  14. int o3 = orientation(p2, q2, p1);
  15. int o4 = orientation(p2, q2, q1);
  16. if (o1 != o2 && o3 != o4)
  17. return true;
  18. return false;
  19. }

3. Dijkstra算法实现

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

  1. struct Edge {
  2. Point target;
  3. double weight;
  4. };
  5. double distance(const Point& a, const Point& b) {
  6. return sqrt((a.x - b.x)*(a.x - b.x) + (a.y - b.y)*(a.y - b.y));
  7. }
  8. vector<Point> dijkstra(const vector<Point>& points,
  9. const vector<vector<bool>>& adjacencyMatrix,
  10. const Point& start,
  11. const Point& end) {
  12. set<pair<double, Point>> pq;
  13. map<Point,double> dist;
  14. for(const auto& point : points){
  15. dist[point] = numeric_limits<double>::infinity();
  16. }
  17. dist[start] = 0.0;
  18. pq.insert(make_pair(0.0,start));
  19. while (!pq.empty()) {
  20. auto currentPair = *pq.begin();
  21. pq.erase(pq.begin());
  22. Point currentNode = currentPair.second;
  23. if(currentNode == end){
  24. break;
  25. }
  26. for(size_t i=0; i<points.size(); ++i){
  27. if(adjacencyMatrix[currentNode][points[i]]){
  28. double newDist = dist[currentNode] + distance(currentNode , points[i]);
  29. if(newDist < dist[points[i]]){
  30. pq.erase(make_pair(dist[points[i]], points[i]));
  31. dist[points[i]] = newDist;
  32. pq.insert(make_pair(newDist , points[i]));
  33. }
  34. }
  35. }
  36. }
  37. vector<Point> path;
  38. // 后续恢复路径逻辑(如需)
  39. return path;
  40. }

4. 主函数与测试用例

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

  1. int main() {
  2. // 定义多边形顶点(作为示例)
  3. vector<Point> polygonPoints = {{1.0 , 1.0}, {3.0 , 3.5}, {5.0 , 3.5}, {6.0 , 1.5}};
  4. // 假设有个起始与结束位置
  5. Point start{0., 0.};
  6. Point end{7., 4.};
  7. // 构建邻接矩阵,根据实际情况决定何时连接两个节点。
  8. size_t n=polygonPoints.size();
  9. vector<vector<bool>> adjacencyMatrix(n+2 , vector<bool>(n+2 , false));
  10. // 检查各个对之间是否可通行以填充邻接矩阵
  11. for(size_t i=0;i<n+2;++i){
  12. for(size_t j=i+1;j<n+2;++j){
  13. bool canConnect=true;
  14. if(doIntersect(start,end)){
  15. canConnect=false;
  16. }
  17. adjacencyMatrix[i][j]=canConnect;
  18. adjacencyMatrix[j][i]=canConnect;
  19. }
  20. }
  21. // 调用 Dijkstra 算法找路径。
  22. vector<Point> path=dijkstra(points.adjacencyMatrix,start,end);
  23. // 打印结果,如果找到了有效路径.
  24. if(!path.empty()){
  25. cout << "找到路径!" << endl ;
  26. for(auto &p: path)
  27. cout<<"("<<p.x<<","<<p,y<<")"<<"\n";
  28. }else{
  29. cout <<"无有效路径!" <<endl ;
  30. }
  31. return 0;
  32. }

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

  • 完整恢复已找到的最短路径。
  • 优化图构造过程以提高效率。
  • 支持动态添加障碍物等。

希望这能帮助你理解如何在C++中进行基本的路径规划!如果有其他问题,欢迎继续提问。


内容由零声教学AI助手提供,问题来源于学员提问

本站部分文章来源于网络,版权归原作者所有,如有侵权请联系站长删除。
转载请注明出处:https://golang.0voice.com/?id=20004

分享:
扫描分享到社交APP
上一篇
下一篇
发表列表
游客 游客
此处应有掌声~
评论列表

还没有评论,快来说点什么吧~

联系我们

在线咨询: 点击这里给我发消息

微信号:3007537140

上班时间: 10:30-22:30

关注我们
x

注册

已经有帐号?