路径规划算法代码c语言
时间: 2023-08-24 10:41:19 浏览: 40
以下是一个简单的路径规划算法的 C 语言代码示例:
```c
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define MAX 100 // 最大节点数
typedef struct {
int x, y;
} Point; // 节点结构体
Point points[MAX]; // 存储节点数组
int n; // 节点数
int path[MAX]; // 存储路径
int bestPath[MAX]; // 存储最优路径
double bestDist; // 最优路径长度
// 计算两个节点之间的距离
double distance(Point p1, Point p2) {
double dx = p1.x - p2.x;
double dy = p1.y - p2.y;
return sqrt(dx * dx + dy * dy);
}
// 计算当前路径的长度
double getDist(int path[]) {
double dist = 0;
for (int i = 1; i < n; i++) {
dist += distance(points[path[i - 1]], points[path[i]]);
}
dist += distance(points[path[n - 1]], points[path[0]]);
return dist;
}
// 搜索当前节点的下一个节点
void search(int cur, double dist) {
if (cur == n) { // 已经遍历完所有节点
if (dist < bestDist) { // 更新最优路径
bestDist = dist;
for (int i = 0; i < n; i++) {
bestPath[i] = path[i];
}
}
return;
}
for (int i = cur; i < n; i++) {
double tmpDist = dist + distance(points[path[cur - 1]], points[path[i]]);
swap(path[cur], path[i]);
search(cur + 1, tmpDist);
swap(path[cur], path[i]);
}
}
int main() {
scanf("%d", &n);
for (int i = 0; i < n; i++) {
scanf("%d%d", &points[i].x, &points[i].y);
path[i] = i;
}
bestDist = getDist(path);
search(1, 0);
printf("最短路径长度为:%.2f\n", bestDist);
printf("最短路径为:");
for (int i = 0; i < n; i++) {
printf("%d ", bestPath[i] + 1);
}
printf("\n");
return 0;
}
```
以上代码是一个暴力搜索的路径规划算法,对于节点数较少的情况可以得到较好的结果,但节点数过多时会变得非常耗时。实际上,路径规划算法有很多种,如 A* 算法、Dijkstra 算法等,可以根据具体问题的特点选择不同的算法进行优化。