Принцип работы алгоритма поиска пути Астар (A*).

const int c_HeapSizeForAstarList = 8192;

typedef struct AstarListIt
{
  int        idx;  // узел, в данном случае его индекс
  AstarListIt*  prev; // предыдущий итератор
  AstarListIt*  next; // следующий итератор
} AstarListIt;

class AstarList
{
public:
  AstarList()
  {
    iSize = 0;
    iHeap = 0;
    heap.push_back(new AstarListIt[c_HeapSizeForAstarList]);
    head = tail = heap[iHeap]; tail->prev = head;
  }
  ~AstarList()
  {
      clear();
    delete[] heap[0];
  }
  AstarListIt* begin() {return head;}  // получить головной итератор
  AstarListIt* end() {return tail;}    // получить хвостовой итератор
  AstarListIt* push_back(int& idx);    // добавить элемент в конец списка
  int  front(); // pop                  // взять элемент из головы списка
  void erase(AstarListIt* it);         // удалить итератор
  bool empty() { return tail == head; } // проверить пустой ли список
  AstarListIt* insert(AstarListIt* itNext, int idx); // вставить элемент
  void clear();                                      // очистить список

protected:
    void resize();                        // увеличить размер

  AstarListIt*        head;     // головной итератор (начало списка)
  AstarListIt*        tail;     // хвостовой итератор (конец списка)
  std::vector<AstarListIt*>  heap;     // куча зарезервированной памяти
  int              iSize;    // размер текущей кучи
  int              iHeap;    // индекс текущей кучи
};

class AstarHash
{
public:
  typedef struct PathHashTable{ // элемент таблицы
    AstarListIt*  it;       // указатель на итератор
    int        value;    // проверочное значение
    bool      closed;   // относится к закрытому или открытому списку
  } PathHashTable;

  PathHashTable*  Table;        // таблица
  int        Size;         // размер
  int        Power;        // кол-во идентификационных бит
  int        Mask;         // маска

  AstarHash(int power): Power(power), Size(1<<power)
  { Table = new PathHashTable[Size]; Clear(); }
  ~AstarHash(){delete [] Table;}
  void Clear() { ZeroMemory(Table, Size*sizeof(PathHashTable)); } // очистка
  void Add(int value, AstarListIt* it, bool closed);   // добавить элемент
  void Resize();                                       // увеличить размер
  AstarListIt* Find(int value, bool closed);        // найти элемент
  void Erase(int value);                            // удалить
  void Check();                                    // проверка (для отладки)
};

// поиск пути
bool FindPath(int idxFrom, int idxTo, float fUnitSize, int iUnitType)
{
  // лист узлов для найденого пути
  list<int> lPath;
  int idxCur = idxFrom;

    // вычислить и запомнить оценку расстояния до конечного узла
  m_pCells[idxCur].CalcCost(0, m_pCells[idxTo]);
    // пометить как начало цепочки, для последующего построения пути
  m_pCells[idxCur].m_ParentIdx = -1;

    // поместить начальный узел в сортированный список
  m_lOpen.push_back(idxCur);
    // добавить запись в хэш таблицу
  m_pAstarHash->Add(idxCur, m_lOpen.begin(), false);

    // получение текущего времени для последующей проверки на критическое время
  DWORD dwTime = timeGetTime();
  DWORD dwCriticalTime = 200;

    // начало главного цикла
  while(!m_lOpen.empty())
  {
      // получение текущего узла для обработки
    idxCur = m_lOpen.front();
        // удаление записи из хэш таблицы
    m_pAstarHash->Erase(idxCur);

        // проверка на достижение конечного узла
    if (idxCur == idxTo)
    {
            // построение пути с конца в начало
      for (int p = idxTo; p >= 0; p = m_pCells[p].m_ParentIdx)
        lPath.push_front(p);
      break;
    }
        // использовать смартЛОД, если до конечного узла достаточно большое 
        // расстояние, больше чем максимальный размер области при 
        // построении смарт линков
        bool useSmart = m_pCells[idxCur].m_fToFinish > m_fMaxSmartArea;
    int iSize;
        // обработка восьми соседей
    for (int iDir = 0; iDir < 8; iDir++)
    {
          // получить дистанцию до соседа, если нет смартлинков, то - 1
      if (useSmart)
        iSize = m_pCells[idxCur].m_pSmartLinks[iDir].SmartSize;
      else
        iSize = 1;
            // получить индекс соседа
      int idx = m_pCells.GetNeighborIdx(idxCur, iDir, iSize);
            // а есть ли сосед?
      if (idx < 0)
        continue;

            // расчет нового оценочного расстояния для текущего соседа 
            // от стартового узла
      float fFromStart;
      if (useSmart)
        fFromStart = m_pCells[idxCur].m_fFromStart + 
          m_pCells[idxCur].m_pSmartLinks[iDir].fCost;
      else
        fFromStart = m_pCells[idxCur].m_fFromStart + 
          m_pCells[idxCur].m_pLinks[iDir].fCost;

            // поиск узла в списках при помощи хэш таблицы
      AstarListIt* pItO = m_pAstarHash->Find(idx, false);
      AstarListIt* pItC = m_pAstarHash->Find(idx, true);

            // если нашли то сравнить старое оценочное расстояние с новым
      if (pItO || pItC)
      {
        if (m_pCells[idx].m_fFromStart <= fFromStart)
          continue;

              // очистка списков и записей в хэш таблице
              if (pItC)
              {
                  m_lClose.erase(pItC);
                  m_pAstarHash->Erase(idx);
              }
              if (pItO)
              {
                  m_lOpen.erase(pItO);
                  m_pAstarHash->Erase(idx);
              }
      }
            else
            {
                // проверка узла на проходимость (занятость)
              if (CheckBusyWayable(idx, iUnitType, fUnitSize))
                  continue;
            }

            // обновление оценочного расстояния до конца пути
      m_pCells[idx].CalcCost(fFromStart, m_pCells[idxTo]);
      m_pCells[idx].m_ParentIdx = idxCur;

            // вставка узла в сортированный список
            AstarListIt* itOpen = m_lOpen.begin();
      for (; itOpen != m_lOpen.end(); itOpen = itOpen->next)
      {
        if ( m_pCells[itOpen->idx].m_fCost >= m_pCells[idx].m_fCost)
          break;
      }
      m_pAstarHash->Add(idx, m_lOpen.insert(itOpen, idx), false);
    }

        // поместить обработанный узел в close список
    m_pAstarHash->Add(idxCur, m_lClose.push_back(idxCur), true);

        // проверка времени (дабы пусть лучше путь не будет найден,
        // чем будет лаг из-за его поиска
    if (timeGetTime() - dwTime > dwCriticalTime)
      break;
  }
    // очистка списков и хэш таблицы
  m_lOpen.clear();
  m_lClose.clear();
  m_pAstarHash->Clear();

    // вызов функции сглаживания пути
  OptimizeWay(lPath, unitPath);

  // вернуть результат работы
  return !unitPath.m_lWayPoints.empty();
}