itch.io

struct PathPoint {
	// Node position
	int16_t x, z, y;
	// Parent node position
	int16_t pX, pZ, pY;
}

static int cmpeq(struct PathPoint a, struct PathPoint b) {
	return a.x == b.x && a.z == b.z && a.y == b.y;
}

// This function checks if the node is free (1 = free)
static int walk(struct PathPoint p, int dx, int dy, struct PathPoint *ret) {
	p.pX = p.x, p.pZ = p.z, p.pY = p.y;
	p.x += dx, p.y += dy;
	p.cost++;
	*ret = p;
	
	// Clearly it must be in-bounds
	if(p.x >= 0 && p.x < game_W && p.y >= 0 && p.y < game_H) {
		// Game-specific logic here to determine which tiles can be walked on
		return 1;
	} else return 0;
	
	// My game was 3D so this made entities fall downwards
	size_t ind = GAME_POINTTOINDEX(p.x, p.z, p.y);
	while(!game_Map[ind] && ret->z > 0) {
		ind -= game_H;
		ret->z--;
	}
	if(game_Map[ind]) ret->z++;
	
	return 1;
}

// Returns index if exists, else -1
static int in_list(struct PathPoint p, struct PathPoint *list, size_t listLength) {
	for(size_t i = 0; i < listLength; i++) {
		if(cmpeq(list[i], p)) return i;
	}
	return -1;
}

// My heuristic function (chess distance)
static uint32_t chessdist(struct PathPoint a, struct PathPoint b) {
	return max32(max32(abs32(a.x - b.x), abs32(a.y - b.y)), abs32(a.z - b.z));
}

size_t pathfind(struct PathPoint start, struct PathPoint end, struct PathPoint **result) {
	// List of open nodes, in order from least to most costly
	size_t openCount = 1;
	struct PathPoint *open = malloc(sizeof(*open));
	*open = start;
	
	size_t closedCount = 0;
	struct PathPoint *closed = NULL;
	
	while(openCount) {
		// Get first (least costly) node from open set
		struct PathPoint p = open[0];
		memmove(open, open + 1, sizeof(*open) * --openCount);
		
		closed = realloc(closed, sizeof(*closed) * ++closedCount);
		closed[closedCount - 1] = p;
		
		if(cmpeq(p, end)) { // If p == end (we've found a path, now return it)
			size_t padhCount = 1;
			struct PathPoint *padh = malloc(sizeof(*padh) * padhCount);
			padh[0] = p; // Starting with p (not end cause p has proper parent information)
			
			// Go back through all the parent information to reconstruct the path
			do {
				int idx = in_list((struct PathPoint) {.x = padh->pX, .y = padh->pY, .z = padh->pZ}, closed, closedCount);
				padh = realloc(padh, sizeof(*padh) * (padhCount + 1));
				memmove(padh + 1, padh, sizeof(*padh) * padhCount++);
				*padh = closed[idx];
			} while(!cmpeq(*padh, start));
			
			free(open);
			free(closed);
			
			*result = padh;
			return padhCount;
		}
		
		// Try all eight neighbours
		struct PathPoint candidate;
		for(int dx = -1; dx <= 1; dx++) {
			for(int dy = -1; dy <= 1; dy++) {
				if(walk(p, dx, dy, &candidate) && in_list(candidate, closed, closedCount) == -1) {
					// We can walk here!
					
					candidate.cost += chessdist(candidate, end); // Apply heuristic
					
					// No point in adding to the open list if it is already there
					int openListID = in_list(candidate, open, openCount);
					if(openListID == -1) {
						// Add to the open list
						// Find the correct index to keep it sorted
						int i;
						for(i = 0; i < openCount; i++) {
							if(open[i].cost > candidate.cost) {
								break;
							}
						}
						open = realloc(open, sizeof(*open) * ++openCount);
						memmove(&open[i + 1], &open[i], sizeof(*open) * (openCount - i - 1));
						open[i] = candidate;
					} else {
						// Update parent information if we've found a shorter path to this node
						if(candidate.cost < open[openListID].cost) {
							open[openListID].cost = candidate.cost;
							open[openListID].pX = p.x;
							open[openListID].pZ = p.z;
							open[openListID].pY = p.y;
						}
					}
				}
			}
		}
	}
	
failed:
	free(open);
	free(closed);
	*result = NULL;
	return 1;
}

I’ve added comments, but if I’m honest this isn’t much different to the pseudocode in Wikipedia.

From a C perspective there’s plenty of room for optimization, but it works. In C# there’s almost certainly an automatic sorted list/set or priority queue class in the standard library.