寻路算法初探

https://www.redblobgames.com/pathfinding/a-star/introduction.html

 

1、广搜

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define bool int
#define true 1
#define false 0

struct Point{
	int x;
	int y;
};

struct Node{
	struct Point *point;
	struct Node *next;
};

struct Queue{
	struct Node *head,*tail;
};

struct Queue *QueueCreate(){
	struct Queue *obj = malloc(sizeof(struct Queue));
	obj->head = NULL;
	obj->tail = NULL;
	return obj;
}

void QueuePush(struct Queue * obj,struct Point *point){
	struct Node* node = malloc(sizeof(struct Node));
	node->point = malloc(sizeof(struct Point));
	memcpy(node->point,point,sizeof(struct Point));
	node->next = NULL;
	if(obj->tail == NULL){
		obj->head = node;
		obj->tail = node;
	}else{
		obj->tail->next = node;
		obj->tail = node;
	}
}

void QueuePop(struct Queue * obj){
	if(obj->head == NULL)
		return;
	struct Node* node = obj->head;
	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
}

int QueuePeek(struct Queue *obj,struct Point *point){
	if(obj->head == NULL)
		return false;
	struct Node* node = obj->head;
	memcpy(point,node->point,sizeof(struct Point));

	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
	return true;
}

bool QueueEmpty(struct Queue *obj){
	if(obj->head == NULL)
		return true;
	return false;
}

void QueueFree(struct Queue* obj) {
	while(!QueueEmpty(obj)){
		QueuePop(obj);
	}
	free(obj);
}


int main(){
	int map_walk[10][10] = {0};
    int map_cost[10][10] = {0};
	struct Queue *queue = QueueCreate();
	struct Point start = {5,5},point,point_tmp;
	struct Point end = {0,0};
	map_walk[start.x][start.y] = 1;
    map_cost[start.x][start.y] = 0;
	
	QueuePush(queue,&start);

	while(QueuePeek(queue,&point) == true){
		if(point.x == end.x && point.y == end.y)
            break;

		//up
		if(point.x-1>=0){
			point_tmp.x = point.x-1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0){
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y]+1;
				QueuePush(queue,&point_tmp);
			}
		}

		//right
		if(point.y+1<10){
			point_tmp.x = point.x;
			point_tmp.y = point.y+1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0){
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y]+1;
				QueuePush(queue,&point_tmp);
			}
		}

		//down
		if(point.x+1<10){
			point_tmp.x = point.x+1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0){
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y]+1;
				QueuePush(queue,&point_tmp);
			}
		}
		
		//left
		if(point.x-1>=0){
			point_tmp.x = point.x;
			point_tmp.y = point.y-1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0){
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y]+1;
				QueuePush(queue,&point_tmp);
			}
		}
	}
	QueueFree(queue);

	int i,j;
	for(i=0;i<10;i++){
		for(j=0;j<10;j++){
				printf("%2d,",map_cost[i][j]);
		}
		printf("\n");
	}
	printf("\n\n");

}

2、Dijkstra’s Algorithm。加入cost，表示每次移动的花费；队列换成优先队列PriorityQueue，权值为走到该点的花费，花费越少，权值低，优先级越高。

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define bool int
#define true 1
#define false 0

struct Point{
	int x;
	int y;
};

struct Node{
	struct Point *point;
	struct Node *next;
	int priority;
};

struct Queue{
	struct Node *head,*tail;
};

struct Queue *PriorityQueueCreate(){
	struct Queue *obj = malloc(sizeof(struct Queue));
	obj->head = NULL;
	obj->tail = NULL;
	return obj;
}

void PriorityQueuePush(struct Queue * obj,struct Point *point,int priority){
	struct Node* node = malloc(sizeof(struct Node));
	node->point = malloc(sizeof(struct Point));
	memcpy(node->point,point,sizeof(struct Point));
	node->priority = priority;
	node->next = NULL;

	if(obj->tail == NULL){
		obj->head = node;
		obj->tail = node;
		return;
	}

	struct Node newHead;
	newHead.next = obj->head;

	struct Node* node_tmp;
	for(node_tmp = &newHead;node_tmp->next!=NULL;node_tmp = node_tmp->next)
	{
		if(node->priority < node_tmp->next->priority)
			break;
	}

	node->next = node_tmp->next;
	node_tmp->next = node;
	obj->head = newHead.next;
	if(obj->tail->next != NULL){
		obj->tail = obj->tail->next;
	}
	
}

void PriorityQueuePop(struct Queue * obj){
	if(obj->head == NULL)
		return;
	struct Node* node = obj->head;
	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
}

int PriorityQueuePeek(struct Queue *obj,struct Point *point){
	if(obj->head == NULL)
		return false;
	struct Node* node = obj->head;
	memcpy(point,node->point,sizeof(struct Point));

	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
	return true;
}

bool PriorityQueueEmpty(struct Queue *obj){
	if(obj->head == NULL)
		return true;
	return false;
}

void PriorityQueueFree(struct Queue* obj) {
	while(!PriorityQueueEmpty(obj)){
		PriorityQueuePop(obj);
	}
	free(obj);
}


int main(){
    int cost[10][10] = {
        {1,1,1,1,1,1,1,1,1,1},
        {1,1,1,1,1,1,1,1,1,1},
        {1,1,5,1,1,1,1,1,1,1},
        {1,1,5,1,1,1,1,1,1,1},
        {1,1,5,1,1,1,1,1,1,1},
        {1,1,5,1,1,1,1,1,1,1},
        {1,1,5,1,1,1,1,1,1,1},
        {1,1,1,1,1,1,1,1,1,1},
        {1,1,1,1,1,1,1,1,1,1},
        {1,1,1,1,1,1,1,1,1,1},
    };
	int map_walk[10][10] = {0};
    int map_cost[10][10] = {0};
	struct Queue *queue = PriorityQueueCreate();
	struct Point start = {5,5},point,point_tmp;
    struct Point end = {0,0};
	int priority;
	map_walk[start.x][start.y] = 1;
    map_cost[start.x][start.y] = 0;

	PriorityQueuePush(queue,&start,0);

	while(PriorityQueuePeek(queue,&point) == true){
		if(point.x == end.x && point.y == end.y)
            break;

		//up
		if(point.x-1>=0){
			point_tmp.x = point.x-1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//right
		if(point.y+1<10){
			point_tmp.x = point.x;
			point_tmp.y = point.y+1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//down
		if(point.x+1<10)
		{
			point_tmp.x = point.x+1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//left
		if(point.y-1>=0)
		{
			point_tmp.x = point.x;
			point_tmp.y = point.y-1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}
	}
	PriorityQueueFree(queue);

	int i,j;
	for(i=0;i<10;i++){
		for(j=0;j<10;j++){
				printf("%2d,",map_cost[i][j]);
		}
		printf("\n");
	}
	printf("\n\n");

}

3、Heutistic。仍然是优先队列，只不过权值换成了点到点的距离。当点越靠近终点，权值越小，优先级越高。

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define bool int
#define true 1
#define false 0

struct Point{
	int x;
	int y;
};

struct Node{
	struct Point *point;
	struct Node *next;
	int priority;
};

struct Queue{
	struct Node *head,*tail;
};

struct Queue *PriorityQueueCreate(){
	struct Queue *obj = malloc(sizeof(struct Queue));
	obj->head = NULL;
	obj->tail = NULL;
	return obj;
}

void PriorityQueuePush(struct Queue * obj,struct Point *point,int priority){
	struct Node* node = malloc(sizeof(struct Node));
	node->point = malloc(sizeof(struct Point));
	memcpy(node->point,point,sizeof(struct Point));
	node->priority = priority;
	node->next = NULL;

	if(obj->tail == NULL){
		obj->head = node;
		obj->tail = node;
		return;
	}

	struct Node newHead;
	newHead.next = obj->head;

	struct Node* node_tmp;
	for(node_tmp = &newHead;node_tmp->next!=NULL;node_tmp = node_tmp->next)
	{
		if(node->priority < node_tmp->next->priority)
			break;
	}

	node->next = node_tmp->next;
	node_tmp->next = node;
	obj->head = newHead.next;
	if(obj->tail->next != NULL){
		obj->tail = obj->tail->next;
	}
	
}

void PriorityQueuePop(struct Queue * obj){
	if(obj->head == NULL)
		return;
	struct Node* node = obj->head;
	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
}

int PriorityQueuePeek(struct Queue *obj,struct Point *point){
	if(obj->head == NULL)
		return false;
	struct Node* node = obj->head;
	memcpy(point,node->point,sizeof(struct Point));

	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
	return true;
}

bool PriorityQueueEmpty(struct Queue *obj){
	if(obj->head == NULL)
		return true;
	return false;
}

void PriorityQueueFree(struct Queue* obj) {
	while(!PriorityQueueEmpty(obj)){
		PriorityQueuePop(obj);
	}
	free(obj);
}

int heuristic(struct Point *a, struct Point *b){
    return (abs(a->x - b->x) + abs(a->y - b->y));
}

int main(){
	int map_walk[10][10] = {0};
    int map_cost[10][10] = {0};
	struct Queue *queue = PriorityQueueCreate();
	struct Point start = {5,5},point,point_tmp;
    struct Point end = {0,0};
	int priority;
    map_walk[start.x][start.y] = 1;
    map_cost[start.x][start.y] = 0;

	PriorityQueuePush(queue,&start,0);
    
	while(PriorityQueuePeek(queue,&point) == true){
        if(point.x == end.x && point.y == end.y)
            break;
		//效果和上下左右的顺序有关
		//up
		if(point.x-1>=0){
			point_tmp.x = point.x-1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0)
			{
				priority = heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + 1;
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}
		//right
		if(point.y+1<10){
			point_tmp.x = point.x;
			point_tmp.y = point.y+1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0)
			{
				priority = heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + 1;
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//down
		if(point.x+1<10)
		{
			point_tmp.x = point.x+1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0)
			{
				priority = heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + 1;
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//left
		if(point.y-1>=0)
		{
			point_tmp.x = point.x;
			point_tmp.y = point.y-1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0)
			{
				priority = heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + 1;
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}
	}
	PriorityQueueFree(queue);

	int i,j;
	for(i=0;i<10;i++){
		for(j=0;j<10;j++){
				printf("%2d,",map_cost[i][j]);
		}
		printf("\n");
	}
	printf("\n\n");

}

4、A*，Dijkstra’s Algorithm + Heuristic

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define bool int
#define true 1
#define false 0

//https://www.redblobgames.com/pathfinding/a-star/introduction.html
struct Point{
	int x;
	int y;
};

struct Node{
	struct Point *point;
	struct Node *next;
	int priority;
};

struct Queue{
	struct Node *head,*tail;
};

struct Queue *PriorityQueueCreate(){
	struct Queue *obj = malloc(sizeof(struct Queue));
	obj->head = NULL;
	obj->tail = NULL;
	return obj;
}

void PriorityQueuePush(struct Queue * obj,struct Point *point,int priority){
	struct Node* node = malloc(sizeof(struct Node));
	node->point = malloc(sizeof(struct Point));
	memcpy(node->point,point,sizeof(struct Point));
	node->priority = priority;
	node->next = NULL;

	if(obj->tail == NULL){
		obj->head = node;
		obj->tail = node;
		return;
	}

	struct Node newHead;
	newHead.next = obj->head;

	struct Node* node_tmp;
	for(node_tmp = &newHead;node_tmp->next!=NULL;node_tmp = node_tmp->next)
	{
		if(node->priority < node_tmp->next->priority)
			break;
	}

	node->next = node_tmp->next;
	node_tmp->next = node;
	obj->head = newHead.next;
	if(obj->tail->next != NULL){
		obj->tail = obj->tail->next;
	}
	
}

void PriorityQueuePop(struct Queue * obj){
	if(obj->head == NULL)
		return;
	struct Node* node = obj->head;
	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
}

int PriorityQueuePeek(struct Queue *obj,struct Point *point){
	if(obj->head == NULL)
		return false;
	struct Node* node = obj->head;
	memcpy(point,node->point,sizeof(struct Point));

	if(obj->head == obj->tail){
		obj->head = NULL;
		obj->tail = NULL;
	}else{
		obj->head = node->next;
	}
	free(node->point);
	free(node);
	return true;
}

bool PriorityQueueEmpty(struct Queue *obj){
	if(obj->head == NULL)
		return true;
	return false;
}

void PriorityQueueFree(struct Queue* obj) {
	while(!PriorityQueueEmpty(obj)){
		PriorityQueuePop(obj);
	}
	free(obj);
}

int heuristic(struct Point *a, struct Point *b){
    return (abs(a->x - b->x) + abs(a->y - b->y));
}

int main(){
    int cost[10][10] = {
        {1,1,1,5,1,1,1,1,1,1},
        {1,1,1,5,1,1,1,1,1,1},
        {1,5,1,5,1,1,1,1,1,1},
        {1,5,1,5,1,1,1,1,1,1},
        {1,5,1,5,1,1,1,1,1,1},
        {1,5,1,5,1,1,1,1,1,1},
        {1,5,1,1,1,1,1,1,1,1},
        {1,5,1,1,1,1,1,1,1,1},
        {1,1,1,1,1,1,1,1,1,1},
        {1,1,1,1,1,1,1,1,1,1},
    };
	int map_walk[10][10] = {0};
    int map_cost[10][10] = {0};
	struct Queue *queue = PriorityQueueCreate();
	struct Point start = {5,5},point,point_tmp;
    struct Point end = {0,0};
	int priority;
	map_walk[start.x][start.y] = 1;
    map_cost[start.x][start.y] = 0;

	PriorityQueuePush(queue,&start,0);

	while(PriorityQueuePeek(queue,&point) == true){
        if(point.x == end.x && point.y == end.y)
            break;
		
		//up
		if(point.x-1>=0){
			point_tmp.x = point.x-1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y] + heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//right
		if(point.y+1<10){
			point_tmp.x = point.x;
			point_tmp.y = point.y+1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y] + heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//down
		if(point.x+1<10)
		{
			point_tmp.x = point.x+1;
			point_tmp.y = point.y;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y] + heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}

		//left
		if(point.y-1>=0)
		{
			point_tmp.x = point.x;
			point_tmp.y = point.y-1;
			if(map_walk[point_tmp.x][point_tmp.y] == 0 || map_cost[point_tmp.x][point_tmp.y]>map_cost[point.x][point.y]+cost[point_tmp.x][point_tmp.y])
			{
				priority = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y] + heuristic(&point_tmp,&end);
				map_walk[point_tmp.x][point_tmp.y] = 1;
            	map_cost[point_tmp.x][point_tmp.y] = map_cost[point.x][point.y] + cost[point_tmp.x][point_tmp.y];
				PriorityQueuePush(queue,&point_tmp,priority);
			}
		}
	}
	PriorityQueueFree(queue);

	int i,j;
	for(i=0;i<10;i++){
		for(j=0;j<10;j++){
				printf("%2d,",map_cost[i][j]);
		}
		printf("\n");
	}
	printf("\n\n");

}