A星算法的原理这里就不赘述了,CSDN上有很多讲解的,主要是讲一讲编写思路。
在A星算法里,主要是从openlist里找出当前f值最小的那个点当作下一次拓展的节点,并将这个节点加入closelist,我们可以先定义一个叫point的结构体,记录自身坐标和f值,g值和h值,以及指向父节点的指针。之所以添加指针,主要是为了从终点回溯路径(就是链表)。把这个最小f值的点找出来之后,接下来就是找子节点了,这里直接定义neibor结构体,储存当前点的坐标和八个周围的子节点(在closelist里面的要删去),之后就是判断这些子节点在不在openlist里,在则判断g值,更新父节点,不在,则添加到openlist里面。 依次往复循环,直至找到终点。
程序里定义的closelist和openlist比较大,是因为现在我还没学到动态内存分配,所以就先定义了比整个地图的点的数量还多的结构体数组。学过的可以优化一下。另外,在add_openlist这个函数里 ,
open_list->open[j].parent = &(neibor->pos);
neibor->child[i].parent = &(neibor->pos);
这个父节点应该要指向closelist里面对应坐标的点,不是neibor里面的记录的自身节点。因为neibor这个结构体里面的pos是会变得,而closelist里面的值和地址都是固定的。可以在在这个函数里面再传入一个closelist的指针,接着在这个函数里面判断点坐标和closelist里面相等的点。把这个&(neibor->pos)改成closelist里面的那个点。但就算不改,按这个定义的地图直接运行,好像也能输出正确路径。可能换一个就不行了,我还没试过。总之,这个程序主要是提供了编写A星程序的思路。我也是学习c语言不久,这个程序写的有点庞杂,大家有兴趣也可以看一下改进和指正一下。
#include <stdio.h>
#include <math.h>
#define ROW 9
#define COL 9
#define ROWS ROW+2
#define COLS COL+2
int map[ROWS][COLS] = { {1,1,1,1,1,1,1,1,1,1,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,0,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,0,0,0,0,1,0,0,0,0,1},
{1,1,1,1,1,1,1,1,1,1,1} };//定义地图
struct point {//定义点的坐标
int x;
int y;
double g;//g值
int h;//h值
double f;//f值
struct point* parent;//指向父节点的指针
};
int h_dis(struct point* now,struct point* end) {//计算h值
int dis = abs((*now).x - (*end).x) + abs((*now).y - (*end).y);
return dis;
}
double g_dis(struct point* now,struct point* start) {//计算g值
double dis = sqrt(((*now).x-(*start).x)* ((*now).x - (*start).x)+ ((*now).y - (*start).y) * ((*now).y - (*start).y));
return dis;
}
struct child {//记录当前节点的位置和周围的八个子节点
struct point pos;
struct point child[8];
};
struct closelist {//定义closelist
struct point close[100];
};
struct openlist {//定义openlist
struct point open[100];
};
//struct path {//定义路径
// struct point route[30];
//};
int is_in_open_list(struct point* p, struct openlist* open_list) {//传p点和openlist表,查看p点是否在openlist表中
for (int i = 0; i < 100; i++) {
if ((open_list->open[i].x==0)&& (open_list->open[i].y ==0))//找完openlist都没有,跳出循环
break;
if (open_list->open[i].x == p->x && open_list->open[i].y == p->y) {
*p = open_list->open[i];
return 1; // 找到匹配点
}
}
return 0; // 未找到匹配点
}
int is_in_close_list(struct point* p, struct closelist* close_list) {//传p点和closelist表,查看p点是否在closelist表中
for (int i = 0; i < 100; i++) {
if ((close_list->close[i].x == 0) && (close_list->close[i].y == 0))
break;
if (close_list->close[i].x == p->x && close_list->close[i].y == p->y) {
*p = close_list->close[i];
return 1; // 找到匹配点
}
}
return 0; // 未找到匹配点
}
void find_neibor(struct point* parent, struct closelist* close_list, struct openlist* open_list,struct point* end,struct child* neibor) {//找父节点旁的八个子节点,
neibor->pos = (*parent);
for (int i = 0; i < 8; i++) {//初始化neibor.child
neibor->child[i].x = 0;
neibor->child[i].y = 0;
neibor->child[i].f = 0.0;
neibor->child[i].h = 0;
neibor->child[i].g = 0.0;
neibor->child[i].parent = NULL;
}
//八个子节点
for (int i = 0; i < 8; i++) {
int directions[8][2] = { {-1, -1}, {-1, 0}, {-1, 1}, {0, -1}, {0, 1}, {1, -1}, {1, 0}, {1, 1} };
int x = parent->x + directions[i][0];
int y = parent->y + directions[i][1];
if (map[x][y] != 1) { //判断是否越界或碰到障碍物
neibor->child[i].x = parent->x + directions[i][0];
neibor->child[i].y = parent->y + directions[i][1];
}
}
// 为每个子节点设置父节点,在openlist和closelist中的不设置父节点并且暂时不计算g值,f值和h值
for (int i = 0; i < 8; i++) {
if (!(neibor->child[i].x == 0 && neibor->child[i].y == 0)) {
if (!((is_in_open_list(&(neibor->child[i]), open_list))||(is_in_close_list(&(neibor->child[i]), close_list)))) {
neibor->child[i].parent = parent;
//计算不在openlist中的g,f,h值
double g = g_dis(&(neibor->child[i]), parent);
int h = h_dis(&(neibor->child[i]), end);
neibor->child[i].g = g + (*parent).g;
neibor->child[i].h = h;
neibor->child[i].f = neibor->child[i].g + h;
}
}
}
//判断八个子节点是否在closelist中,如果在则删除(赋值零,保留f,h,g值,还有父节点地址)
int p = 0;
while ((*close_list).close[p].x != 0 || (*close_list).close[p].y != 0) {
for (int j = 0; j < 8; j++) {
if (neibor->child[j].x == (*close_list).close[p].x && neibor->child[j].y == (*close_list).close[p].y) {
neibor->child[j].x = 0;
neibor->child[j].y = 0;
}
}
p++;
}
//重新对neibor.child排序
int insertPos = 0;
for (int i = 0; i < 8; i++) {
if (!(neibor->child[i].x == 0 && neibor->child[i].y == 0)) {
if (i != insertPos) {
neibor->child[insertPos] = neibor->child[i]; // 移动非 {0, 0} 元素到新位置
}
insertPos++;
}
}
// 将剩余的位置设置为 {0, 0}
for (int i = insertPos; i < 8; i++) {
neibor->child[i].x = 0;
neibor->child[i].y = 0;
}
计算每个节点的h,f,g值
//for (int i = 0; i < 8; i++) {
// if (!(a.child[i].x == 0 && a.child[i].y == 0)) {
//
// }
//}
//return a;
}
void add_openlist(struct openlist* open_list, struct child* neibor,struct point* end) {//将没有出现在openlist中的子节点添加至openlist中,出现在openlist中的检查是否更新g值
int temp_h = 0;
double temp_g = 0.0, temp_f = 0.0;
// 检查八个子节点是否已在 openlist 中,并更新g值和父节点
for (int i = 0; i < 8; i++) {
if (!(neibor->child[i].x == 0 && neibor->child[i].y == 0)) {
if (is_in_open_list(&(neibor->child[i]), open_list)) {//若在,则更新估计函数值
//计算新的g,h,f值
double g = g_dis(&(neibor->child[i]), &(neibor->pos));
int h = h_dis(&(neibor->child[i]), end);
temp_g = g + neibor->pos.g;
temp_h = h;
temp_f = g + h;
for (int j = 0; j < 100; j++) {
if (open_list->open[j].x == neibor->child[i].x && open_list->open[j].y == neibor->child[i].y) {
// 如果新的 G 值更小,更新 G、H、F 值和父节点
if (open_list->open[j].g > temp_g) {
open_list->open[j].g = temp_g;
open_list->open[j].h = temp_h;
open_list->open[j].f = temp_f;
neibor->child[i].g = temp_g;
neibor->child[i].f = temp_f;
neibor->child[i].h = temp_h;
open_list->open[j].parent = &(neibor->pos);
neibor->child[i].parent = &(neibor->pos);
break;
}
}
}
}
else {
// 如果不在 openlist 中,添加到 openlist
for (int j = 0; j < 100; j++) {
if (open_list->open[j].x == 0 && open_list->open[j].y == 0) {
open_list->open[j] = neibor->child[i];
break;
}
}
}
}
}
}
struct point* min_f(struct openlist* open_list,struct closelist* close_list) {//挑出openlist中f值最小的,并从openlist删除,添加到closelist
//找出openlist中f值最小的节点的下标index
double min_f = open_list->open[0].f;
int i = 0;
int index = 0;
for (i = 1; i < 99; i++) {
if (open_list->open[i].f < min_f) {
min_f = open_list->open[i].f;
index = i;
}
if ((open_list->open[i+1].x == 0) && (open_list->open[i+1].y == 0)) {
break;
}
}
//将f值最小的加入closelist
int j = 0;
do {
if (close_list->close[j].x == 0 && close_list->close[j].y == 0) {
close_list->close[j] = open_list->open[index];
break;
}
j++;
} while (1);
/* struct point temp = {0};
temp = open_list->open[index];*/
//删除在openlist中f值最小的
open_list->open[index].x=0;
open_list->open[index].y = 0;
/* open_list->open[index].h = 0;
open_list->open[index].g = 0.0;
open_list->open[index].f = 0.0;*/
//重新对openlist排序
int insertPos = 0;
for (int i = 0; i < 100; i++) {
if (!(open_list->open[i].x == 0 && open_list->open[i].y == 0)) {
if (i != insertPos) {
open_list->open[insertPos] = open_list->open[i]; // 移动非 {0, 0} 元素到新位置
}
insertPos++;
}
}
// 将最后一个的位置设置为 {0, 0}
open_list->open[insertPos].x = 0;
open_list->open[insertPos].y = 0;
/* open_list->open[insertPos].h = 0;
open_list->open[insertPos].g = 0.0;
open_list->open[insertPos].f = 0.0;
open_list->open[insertPos].parent = NULL;*/
return &(close_list->close[j]);
}
void add_closelist(struct closelist* close_list) {
}
//打印路径
void print_point(struct point* p) {
printf("(%d, %d)\n", p->x, p->y);
map[p->x][p->y] = 2;
}
void print_path(struct point* target){
struct point* current = target;
while (current != NULL) {
print_point(current);
current = current->parent;
}
}
int main(){
//设置起点和终点
struct point start = { 2,2,0,0,0,NULL };
struct point end = { 8,8 };
//初始化closelist和openlist
struct closelist closed= { {2,2,0,0,0,NULL} };
struct openlist open_list = { 0 };
//int j = 0;
//for (j = 0; j < 100; j++) {//查找第一个a.open[j].x和a.open[j].y都等于零的下标j
// if (b.open[j].x != 0 || b.open[j].y != 0)
// j++;
// if (b.open[j].x == 0 && b.open[j].y == 0) {
// break;
// }
//}
struct child neibor;
find_neibor(&(closed.close[0]), &closed, &open_list, &end, &neibor);
add_openlist(&open_list, &neibor, &end);//初始化openlist
int i = 0;
while(1){
//判断是否到达终点
for (i = 0; i < 100; i++) {
if (closed.close[i].x != 0 || closed.close[i].y != 0) {//找closelist中不为零的点
if (closed.close[i].x == end.x && closed.close[i].y == end.y) {//判断是否到达终点
break;
}
}
if (closed.close[i + 1].x == 0 && closed.close[i + 1].y == 0) {//closelist找完还是没有到终点,则直接退出循环
break;
}
}
if (closed.close[i].x == end.x && closed.close[i].y == end.y) {//判断是否到达终点
break;
}
find_neibor((min_f(&open_list, &closed)), &closed, &open_list, &end, &neibor);//找下一个父节点的子节点,将不在openlist和closelist的子节点的父亲设置为该节点,删除在closelist中的节点
add_openlist(&open_list, &neibor, &end);//将未出现在openlist中的子节点添加至openlist,更新g值和openlist中的父节点,将openlist中最小f值的节点加入closelist
//打印路径
}
print_path(&(closed.close[i]));
for (int i = 0; i < ROWS; i++) {
for (int j = 0; j < COLS; j++) {
printf("%d ", map[i][j]);
}
printf("\n");
}
return 0;
}