A-Star pathfinding

#include <stdio.h> #include <stdlib.h> #include <time.h> // Размеры карты #define MAX_X 24 #define MAX_Y 24 // Скорость поиска пути, чем выше скорость, тем ниже точность (0..4) #define SPEED 0.5 // Номер лабиринта. Если лабиринт непроходим, ставьте другой номер #define MAZE 1 // Точка на карте struct mapPoint { int x; // положение по оси x int y; // положение по оси y int z; // проходимость точки (0-непроходима, 99-путь) int px; // x предыдущей точки int py; // y предыдущей точки int rating; // рейтинг точки int trip; // время прохождения к этой точке (с учетом проходимости) }; // Для оптимизации лучше использовать список точек в виде int list[MAX_X*MAX_Y] // точки представлять в виде числа (x+(y*MAX_X)) // активнее использовать ссылки // это сэкономит кучу машинного времени и памяти.. // Но вариант со списком точек (как структур) и координатами X,Y // выглядит более наглядным // Список точек struct pointList { int count; struct mapPoint list[MAX_X*MAX_Y]; }; struct mapPoint map[MAX_X][MAX_Y]; // карта struct pointList aOpen; // список точек-кандидатов struct mapPoint startPoint; // начальная точка struct mapPoint finalPoint; // конечная точка //============================================= // Задержка в миллисекундах void wait(int mseconds){ clock_t endwait; endwait=clock()+mseconds*(CLOCKS_PER_SEC/1000); while (clock()<endwait); } //-------------------------------------------- // Проверка на одинаковость точек int same_point(struct mapPoint point_a, struct mapPoint point_b) { return ((point_a.x == point_b.x) && (point_a.y == point_b.y)); } //-------------------------------------------- // Добавление точки в список int add_point(struct pointList *list, struct mapPoint point) { list->list[list->count]=point; return list->count++; } //-------------------------------------------- // Удаление точки из списка int del_point(struct pointList *list, struct mapPoint point) { int i, n; for (i=0; i < list->count; i++) { if (same_point(list->list[i], point)) { // slide list list->count--; for (n=i; n < list->count; n++) { list->list[n] = list->list[n+1]; } return i; } } return 0; } //============================================= // Начальное заполнение карты случайными значениями проходимости // и установка начальных значений свойств точек void fill_map() { int x, y, z; for (y=0; y<MAX_Y; y++) { for (x=0; x<MAX_X; x++) { z=rand() % 4; map[x][y].x=x; map[x][y].y=y; map[x][y].z=z; map[x][y].rating=0; } } } //-------------------------------------------- // Чтение карты из файла void read_map() { FILE *fp; int x, y; char s[128]; fp=fopen("map.dat", "rt"); if (fp) { for (y=0; y<MAX_Y; y++) { fgets(s, 80, fp); for (x=0; x<MAX_Y; x++) map[x][y].z = s[x]-'0'; } fclose(fp); } } //-------------------------------------------- // Отображение карты void show_map(int showOpen) { int x, y; // variables for iteration char c; printf("\n"); for (y=0; y<MAX_Y; y++) { for (x=0; x<MAX_X; x++) { // Установка символа точки на карте (для наглядности) switch (map[x][y].z) { case 0: c='#'; break; case 1: c=' '; break; case 2: c='.'; break; case 3: c='*'; break; case 99: c='0'; break; default: c='?'; } // Если на этой точке кандидат, то покажем его if (showOpen && (map[x][y].rating>0)) c='$'; // show character printf("%c", c); } // line feed, caret return printf("\n"); } // Задержка для анимации wait(100); } //-------------------------------------------- // Проверка точки int point_check(struct mapPoint point, int x, int y) { int d; // Проверка на проходимость if (map[x][y].z == 0) return 0; // Проверка на принадлежность точки к кандидатам if (map[x][y].rating > 0) return 0; // d обратно пропорционален расстоянию до финиша d=(MAX_X-(finalPoint.x-x))+(MAX_Y-(finalPoint.y-y)); // Установка рейтинга точки. Чем больше рейтинг точки, тем она дальше // от старта и ближе к цели. // От этой строки зависит качество работы всего алгоритма в целом map[x][y].rating = 10000 + d*SPEED - point.trip - map[x][y].z; // Установка времени прохождения в точку, нужно для расчета рейтинга map[x][y].trip = point.trip + map[x][y].z; // Установка координат предыдущей точки map[x][y].px = point.x; map[x][y].py = point.y; // Добавляем точку в список add_point(&aOpen, map[x][y]); return 1; } //-------------------------------------------- // Построение пути из финиша к старту по цепочке предыдущих точек // Сделано рекурсией (так проще), но можно переделать на цикл void build_path(struct mapPoint point) { map[point.x][point.y].z=99; if (same_point(point, startPoint)) return; build_path(map[point.px][point.py]); } //-------------------------------------------- // Поиск пути void find_path() { struct mapPoint point; // текущая точка int i; // Итератор int mr, mri; // Максимальный рейтинг и его индекс aOpen.count=0; // помещаем в список кандидатов начальную точку point = startPoint; add_point(&aOpen, point); // Пока есть кандидаты, делаем цикл while (aOpen.count > 0) { // удаляем текущего кандидата из списка кандидатов del_point(&aOpen, point); // Если текущий кандидат - это финиш, то строим путь и выходим if (same_point(point, finalPoint)) { build_path(point); return; } // обрабатываем соседние точки if (point.y > 0) point_check(point, point.x, point.y-1); // top if (point.y+1 < MAX_Y) point_check(point, point.x, point.y+1); // bottom if (point.x+1 < MAX_X) point_check(point, point.x+1, point.y); // right if (point.x > 0) point_check(point, point.x-1, point.y); // left // Выбираем из списка кандидатов точку с наибольшим рейтингом // и делаем ее текущей mr=0; for (i=0; i<aOpen.count; i++) { if (aOpen.list[i].rating > mr) { mr = aOpen.list[i].rating; mri = i; } } point=aOpen.list[mri]; // покажем карту с кандидатами show_map(1); } // если до финиша не дошли, построим путь в тупик.. build_path(point); } //============================================= // основной цикл int main(int argc, char *argv[]) { int i, n; // Создаем и показываем карту srand(MAZE); fill_map(); read_map(); show_map(0); printf("Press Enter"); getchar(); // Устанавливаем начальную и конечную точки в разные углы карты startPoint.x = 0; startPoint.y = 0; finalPoint.x = MAX_X-1; finalPoint.y = MAX_Y-1; // Находим путь и показываем карту find_path(); show_map(0); printf("Press Enter"); getchar(); return 0; }