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;
}