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;
}
ċ
astar.c
(8k)
Sergey Bodrov,
17 апр. 2015 г., 02:31
Comments