a-star(a*)路径搜索算法c实现_印第安爸爸的博客-爱代码爱编程

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

#define null 0
#define true 1
#define false 0

typedef unsigned int bool;

#define TRED "\033[31m"  //Red
#define TGRE "\033[32m"  //Green
#define TYEL "\033[33m"  //Yellow
#define TBLU "\033[34m"  //Blue
#define TPUR "\033[35m"  //Pur
#define TCYA "\033[36m"  //Dark green
#define CLER "\033[0m"

#define PRINT_COLOR(COLOR) printf("%s O ", COLOR)

#define MAP_SIZE 32

#define CALC_H(x, y, targetX, targetY) (abs((x) - (targetX)) + abs((y) - (targetY)))

static char map[][33] = {
"oooooooooo*ooooooooooooooooooooo",
"oooooooooo*ooooooooooooooooooooo",
"ooooo*oooo*ooooooooooooooooooooo",
"ooooo*ooo*oooooooooooooooooooooo",
"ooooo*oX*ooooooooooooooooooooooo",
"ooooo***oooooooooooooooooooooooo",
"oooooooooooooooooooooooooooooooo",
"oooooooooooooooooooooooooo*ooooo",
"ooooooooooooooooooooooooo*oooooo",
"oooooooooooooooooooooooo*ooooooo",
"ooooooooooooooooooooooo*oooooooo",
"oooooooooooooooooooooo*ooooooooo",
"ooooooooooooooooooooo*oooooooooo",
"oooooooooooooooooooo*ooooooooooo",
"ooooooooooooooooooo*oooooooooooo",
"oooooooooooooooooo*ooooooooooooo",
"ooooooooooooooooo*oooooooooooooo",
"oooooooooooooooo*ooooooooooooooo",
"ooooooooooooooo*oooooooooooooooo",
"ooooooooooooo**ooooooooooooooooo",
"oooooooooo***ooooooooooooooooooo",
"ooooooooo*ooooooooooooooooooooo",
"oooooooo*ooooooooooooooooooooooo",
"ooooooo*oooooooooooooooooooooooo",
"oooooo*ooooooooooooooooooooooooo",
"ooooo*ooooooooooooooooo***oooooo",
"oooooooooooooooooooooo*oX*oooooo",
"oooooooooooooooooooooo*oo*oooooo",
"oooooooooooooooooooooo*oo*oooooo",
"oooooooooooooooooooooo*oo*oooooo",
"oooooooooooooooooooooo*oo*oooooo",
"oooooooooooooooooooooooooooooooo"
};

void printMap();

typedef struct _Node {
	int x, y;
	float f, g, h;
	struct _Node* parent;
	struct _Node* pnext;
} Node;

Node* createAndAddNode(Node* list, int x, int y, float g, float h, Node* parent);

Node* addNode(Node* list, Node* node);

Node* removeNode(Node* list, Node* node);

Node* searchMinF(Node* list);

Node* findNodeInList(Node* list, int x, int y);

bool isNaighbourReachable(int x, int y, int dx, int dy);

void astarSearch(int startx, int starty, int endx, int endy) {
	int x, y;
	float g;
	Node *openList = null, *closeList = null;
	Node *node, *existsNode, *resultNode = null;

	// 1st, Put start node into open list.
	openList = createAndAddNode(openList, startx, starty, 0, CALC_H(startx, starty, endx, endy), null);

	while (resultNode == null && openList != null) {
		// Search the node with minimum f value in the open list.
		node = searchMinF(openList);

		x = node->x;
		y = node->y;

		// search neighbour node 
		for (int i = -1; i <= 1 && resultNode == null; i++) {
			for (int j = -1; j <= 1; j++) {
				// Skip itself (0, 0).
				if (i == 0 && j == 0) {
					continue;
				}

                // Ignore the node when it is already in close list.
				if (null != findNodeInList(closeList, x + i, y + j)) {
					continue;
				}

                // Check if the naighbour node is reachable.
				if (isNaighbourReachable(x, y, i, j)) {

					// If the naighbour is the target node, just record current node and stop searching.
					if (x + i == endx && y + j == endy) {
					    resultNode = node;
						break;
				    }

                    // Calculate g value for this naighbour.
					g = (i * j == 0 ? 1 : 1.4) + node->g;

                    // If the naighbour is already exists in open list, we will try to update the parent and g/f value for it.
					if (null != (existsNode = findNodeInList(openList, x + i, y + j))) {
						// If the naigbour access from this node has a lower g, we can update the parent and g/f value for it.
				        if (existsNode->g > g) {
							existsNode->parent = node;
							existsNode->g = g;
							existsNode->f = existsNode->h;
						}
					} else {
						// Add new node into open list.
					    openList = createAndAddNode(openList, x + i, y + j, g, CALC_H(x + i, y + j, endx, endy), node);
					}	
				}
			}
		}

		// remove node from openlist
		openList = removeNode(openList, node);

		// add node into close list.
		closeList = addNode(closeList, node);
    }

    // mark result path on the map.
	while (resultNode != null) {
		map[resultNode->y][resultNode->x] = '#';
		resultNode = resultNode->parent;
	} 
}

int main() {
	astarSearch(7, 4, 24, 26);
	printMap();
	return 0;
}

void printMap() {
	for (int i = 0; i < 32; i++) {
		for (int j = 0; j < 32; j++) {
			if (map[i][j] == '*') {
				// Fence
				PRINT_COLOR(TYEL);
			} else if (map[i][j] == '#') {
				// path
				PRINT_COLOR(TGRE);
			} else if (map[i][j] == 'X') {
				// start/end
				PRINT_COLOR(TRED);
			} else {
				PRINT_COLOR(TPUR);
			}
		}
		printf("\n");
	}
}

Node* createAndAddNode(Node* list, int x, int y, float g, float h, Node* parent) {
	Node* newNode = (Node*)malloc(sizeof(Node));
	newNode->x = x;
	newNode->y = y;
	newNode->g = g;
	newNode->h = h;
	newNode->f = g + h;
	newNode->pnext = list;
	newNode->parent = parent;
	return newNode;
}

Node* addNode(Node* list, Node* node) {
	node->pnext = list;
	return node;
}

Node* removeNode(Node* list, Node* node) {
	Node* nextNode, *currentNode;
	if (list == node) {
		return list->pnext;
	}

	currentNode = list;

	while ((nextNode = currentNode->pnext) != null) {
		if (nextNode == node) {
			// remove node.
			currentNode->pnext = nextNode->pnext;
			break;
		}
		currentNode = nextNode;
	}

	return list;
}

Node* searchMinF(Node* list) {
	float minf = list->f;
	Node* nodeWithMinF = list;

	while (list->pnext != null) {
		list = list->pnext;
		if (list->f < minf) {
			minf = list->f;
			nodeWithMinF = list;
		}
	}	

	return nodeWithMinF;
}

Node* findNodeInList(Node* list, int x, int y) {
	while (list != null) {
		if (list->x == x && list->y == y) {
			return list;
		}
		list = list->pnext;
	}

	return null;	
}

bool isNaighbourReachable(int x, int y, int dx, int dy) {
	int targetx = x + dx, targety = y + dy;
	char targetValue;
	if (dx * dy == 0) {
		if (targetx >= 0 && targetx < MAP_SIZE && targety >= 0 && targety < MAP_SIZE) {
			targetValue = map[targety][targetx];
			return  targetValue == 'o' || targetValue == 'X';
		} else {
			return false;
		}
	}

	if (targetx < 0 || targetx >= MAP_SIZE || targety < 0 || targety >= MAP_SIZE) {
		return false;
	}

	targetValue = map[targety][targetx];

	if (targetValue != 'o' && targetValue != 'X') {
		return false;
	}

	if (map[targety][x] == 'o') {
		return true;
	}

	if (map[y][targetx] == 'o') {
		return true;
	}

	return false;
}