roboforum.ru

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