C语言实现A*算法

原创已于 2025-06-06 18:29:16 修改 · 194 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#c语言 #算法 #开发语言

于 2025-06-06 15:38:08 首次发布

头文件

#include <stdlib.h>
#include <stdio.h>
#include <string.h> // 包含 memcpy 函数的声明

//C++调用C语言必备符号
#ifdef __cplusplus
extern "C" {
#endif

#define		ROW		12
#define		COL		12
#define     STEP	10
#define		OBLIQUE	14
#define		INITIAL_CAPACITY	64
//************************结构体***************************
typedef struct _ASTAR_POINT_
{
	int X;
	int Y;
	int G;
	int H;
	int F;
	struct _ASTAR_POINT_ *ParentPoint;
}ASTAR_POINT;

typedef struct _MAP_MATRIX_
{
	int row;
	int col;
	int	data[ROW][COL];
}MAP_MATRIX;

typedef struct _tPointList_
{
	ASTAR_POINT** data;
	int			  size;
	int			  capacity;
}Pointlist;
//************************结构体***************************




void begin_ASTAR();
//比较函数
int CompF(const ASTAR_POINT *pl, const ASTAR_POINT *pr);

void Map_Init(MAP_MATRIX *arry, MAP_MATRIX *map);
//列表初始化
void Pointlist_init(Pointlist *list);
//添加元素
void Pointlist_push(Pointlist *list, ASTAR_POINT *point);
void Pointlist_free(Pointlist *list);
void Remove_Openlist(Pointlist *listOpen, ASTAR_POINT *point);

void FoundPoint(ASTAR_POINT *temp_start, ASTAR_POINT *point);
void notFoundPoint(ASTAR_POINT *temp_start, ASTAR_POINT *end, ASTAR_POINT *point);

int CalcG(ASTAR_POINT *start, ASTAR_POINT *point);
int CalcH(ASTAR_POINT *end, ASTAR_POINT *point);
void CalcF(ASTAR_POINT *point);

ASTAR_POINT* GetMinFPoint(Pointlist *listOpen);
void Find_Round_Point(Pointlist	*listClose,
						   MAP_MATRIX	*map,
						   ASTAR_POINT	*point,
						   int			 IsIgnoreCorner,
						   Pointlist	*surround);

int CanReach(MAP_MATRIX *map, Pointlist *listClose, ASTAR_POINT *point, int x, int y, int IsIgnoreCorner);
int isobs(MAP_MATRIX *map, int x, int y);
int in_close_list(Pointlist *listClose, int x, int y);
int in_open_list(Pointlist *listOpen, ASTAR_POINT *point);

ASTAR_POINT* FindPath(ASTAR_POINT	*start,
					  ASTAR_POINT	*end,
					  MAP_MATRIX	*map,
					  Pointlist		*listOpen,
					  Pointlist		*listClose,
					  int			 IsIgnoreCorner);

#ifdef __cplusplus
}
#endif

C文件

#include "A_star.h"

void begin_ASTAR()
{
	MAP_MATRIX map;
	MAP_MATRIX array;
	array.row = ROW;
	array.col = COL;
	int temp_map[ROW][COL] = {
	{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
	{ 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1 },
	{ 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1 },
	{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1 },
	{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1 },
	{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },
	{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 },
	{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
	};
	memcpy(array.data, temp_map, sizeof(temp_map));
	Pointlist listOpen;
	Pointlist listClose;
	Pointlist_init(&listOpen);
	Pointlist_init(&listClose);

	ASTAR_POINT start = { 0 }, end = { 0 };
	start.X = 1, start.Y = 1;
	end.X = 6, end.Y = 10;

	Map_Init(&array, &map);

	ASTAR_POINT* parent = FindPath(&start, &end, &map, &listOpen, &listClose, 0);

	while (parent != NULL)
	{
		printf("%d, %d\n", parent->X, parent->Y);
		parent = parent->ParentPoint;
	}

	system("pause");
	Pointlist_free(&listOpen);
	Pointlist_free(&listClose);
	return ;
}
int CompF(const ASTAR_POINT *pl, const ASTAR_POINT *pr)
{
	return pl->F < pr->F;
}

void Map_Init(MAP_MATRIX *arry, MAP_MATRIX *map)
{
	int i = 0, j = 0;
	for (i = 0; i < ROW; i++)
	{
		for (int j = 0; j < COL; j++)
		{
			map->data[i][j] = arry->data[i][j];
		}
	}
}

void Pointlist_init(Pointlist *list)
{
	list->data = (ASTAR_POINT**)malloc(sizeof(ASTAR_POINT*) * INITIAL_CAPACITY);
	list->size = 0;
	list->capacity = INITIAL_CAPACITY;
}

void Pointlist_push(Pointlist *list, ASTAR_POINT *point)
{
	if (list->size >= list->capacity)
	{
		list->capacity *= 2;
		list->data = (ASTAR_POINT**)realloc(list->data, sizeof(ASTAR_POINT*) * list->capacity);
	}
	ASTAR_POINT *new_point = (ASTAR_POINT*)malloc(sizeof(ASTAR_POINT));
	//无法分配更多内存
	if (new_point == NULL)
	{
		return;
	}
	new_point->X = point->X;
	new_point->Y = point->Y;
	new_point->G = point->G;
	new_point->H = point->H;
	new_point->F = point->F;
	new_point->ParentPoint = point->ParentPoint;
	list->data[list->size++] = new_point;
}

void Pointlist_free(Pointlist *list)
{
	int i = 0;
	for ( i = 0; i < list->size; ++i)
	{
		free(list->data[i]);
	}
	free(list->data);
	list->data = NULL;
	list->size = 0;
	list->capacity = 0;
}

ASTAR_POINT* GetMinFPoint(Pointlist *listOpen)
{
	if (listOpen->size == 0)
		return NULL;

	int i = 0;
	ASTAR_POINT *minfnode = listOpen->data[0];
	for ( i = 0; i < listOpen->size; i++)
	{
		if (listOpen->data[i]->F < minfnode->F)
		{
			minfnode = listOpen->data[i];
		}
	}
	return minfnode;
}

void Remove_Openlist(Pointlist *listOpen, ASTAR_POINT *point)
{
	int i = 0, j = 0;
	for ( i = 0; i < listOpen->size; ++i)
	{
		if (listOpen->data[i]->X == point->X && listOpen->data[i]->Y == point->Y)
		{

			free(listOpen->data[i]);

			for ( j = i; j < listOpen->size - 1; ++j)
			{
				listOpen->data[j] = listOpen->data[j + 1];
			}

			listOpen->size--;
			break;
		}
	}
}

int CanReach(MAP_MATRIX *map, Pointlist *listClose, ASTAR_POINT *point, int x, int y, int IsIgnoreCorner)
{
	//障碍物或者关闭列表中不用访问
	if (isobs(map, x, y) || in_close_list(listClose, x, y))
		return 0;
	else
	{
		if (abs(point->X - x) + abs(point->Y - y) == 1)
			return 1;
		else
		{
			if (!isobs(map, point->X, y) && !isobs(map, x, point->Y))
				return 1;
			else
				return IsIgnoreCorner;

		}
	}

}

int isobs(MAP_MATRIX *map, int x, int y)
{
	if (map->data[x][y] == 0)
		return 0;
	else
		return 1;
}

int in_close_list(Pointlist *listClose, int x, int y)
{
	if (listClose == NULL || listClose->data == NULL) {
		return 0;
	}

	int i = 0;
	for ( i = 0; i < listClose->size; ++i)
	{
		if (listClose->data[i]->X == x && listClose->data[i]->Y == y)
		{
			return 1;
		}
	}
	return 0;
}

int in_open_list(Pointlist *listOpen, ASTAR_POINT *point)
{
	if (listOpen == NULL || listOpen->data == NULL) {
		return 0;
	}

	int i = 0;
	for (i = 0; i < listOpen->size; ++i)
	{
		if (listOpen->data[i]->X == point->X && listOpen->data[i]->Y == point->Y)
		{
			point->ParentPoint = listOpen->data[i]->ParentPoint;
			return 1;
		}
	}
	return 0;
}

void Find_Round_Point(Pointlist *listClose, MAP_MATRIX	*map, ASTAR_POINT *point, int IsIgnoreCorner, Pointlist *surround)
{

	int x, y;

	for ( x = point->X - 1; x <= point->X + 1; ++x)
	{
		for (y = point->Y - 1; y <= point->Y + 1; ++y)
		{
			if (x == point->X && y == point->Y) continue;

			if (CanReach(map, listClose, point, x, y, IsIgnoreCorner))
			{
				ASTAR_POINT temp_point = { 0 };

				temp_point.X = x;
				temp_point.Y = y;
				Pointlist_push(surround, &temp_point);
			}

		}
	}

	return;
}

void FoundPoint(ASTAR_POINT *temp_start, ASTAR_POINT *point)
{
	int G = CalcG(temp_start, point);
	if (G < point->G)
	{
		point->ParentPoint = temp_start;
		point->G = G;
		CalcF(point);
	}
}

void notFoundPoint(ASTAR_POINT *temp_start, ASTAR_POINT *end, ASTAR_POINT *point)
{
	point->ParentPoint = temp_start;
	point->G = CalcG(temp_start, point);
	point->H = CalcH(end, point);
	CalcF(point);
}

int CalcG(ASTAR_POINT *start, ASTAR_POINT *point)
{
	int G = (abs(point->X - start->X) + abs(point->Y - start->Y)) == 2 ? OBLIQUE : STEP;
	int parentG = point->ParentPoint != NULL ? point->ParentPoint->G : 0;
	return G + parentG;
}

int CalcH(ASTAR_POINT *end, ASTAR_POINT *point)
{
	int step = abs(end->X - point->X) + abs(end->Y - point->Y);
	return step * STEP;
}

void CalcF(ASTAR_POINT *point)
{
	point->F = point->G + point->H;
}

ASTAR_POINT* FindPath(ASTAR_POINT	*start,
					  ASTAR_POINT	*end,
					  MAP_MATRIX	*map,
					  Pointlist		*listOpen,
					  Pointlist		*listClose,
					  int			 IsIgnoreCorner)
{
	int i = 0;
	ASTAR_POINT *temp_start = NULL;
	Pointlist list_temp;
	Pointlist_init(&list_temp);
	//起点放入开启列表中
	Pointlist_push(listOpen, start);

	while (listOpen->size)
	{
		Pointlist surround;
		Pointlist_init(&surround);

		//找到临时起点,即F值最低的点
		temp_start = GetMinFPoint(listOpen);


		//**********存入临时起点，防止从开启列表中清除后无法找到***********
		Pointlist_push(&list_temp, temp_start);

		//开启列表中删除临时起点
		Remove_Openlist(listOpen, temp_start);


		//**********从开启列表移除后，后面需要该地址，现在取出来***********
		temp_start = list_temp.data[list_temp.size - 1];

		//添加到关闭列表
		Pointlist_push(listClose, temp_start);
		//找到临时起点周围的点
		Find_Round_Point(listClose, map, temp_start, IsIgnoreCorner, &surround);

		//遍历节点进行F值的计算
		for ( i = 0; i < surround.size; ++i)
		{
			//在开放列表中，更新F
			if (in_open_list(listOpen, surround.data[i]))
				FoundPoint(temp_start, surround.data[i]);
			else
			{
				//不在开放列表，计算F更新父节点，放入开放列表
				notFoundPoint(temp_start, end, surround.data[i]);
				Pointlist_push(listOpen, surround.data[i]);
			}

		}
		//目标点在开启列表中
		if (in_close_list(listOpen, end->X, end->Y))
		{
			for ( i = 0; i < listOpen->size; ++i)
			{
				if (listOpen->data[i]->X == end->X && listOpen->data[i]->Y == end->Y)
				{
					return listOpen->data[i];
				}
			}
		}
		Pointlist_free(&surround);
	}
	Pointlist_free(&list_temp);

	return end;
}