Sorted Linked List: оптимизировано добавление / удаление / переупорядочение элементов

Я делаю реализацию алгоритма поиска пути A *, основанного на псевдокоде из Википедии, и пытаюсь оптимизировать производительность. Я отметил, что большую часть времени тратится на обработку узлов в «открытом» наборе узлов (в основном проверяемых узлов), поэтому я делал все возможное, чтобы сделать это быстрее.

(Справочная информация, не стесняйтесь, чтобы перейти к коду)
По сути, у меня есть набор узлов, и я делаю следующее:

• Найти узел с наименьшей оценкой
• Проверьте, содержится ли узел в наборе
• Удалить узел из набора
• Добавить узел в набор

Используя отсортированный связанный список, можно найти наименьшую оценку от проверки всех элементов до выборки первого. Добавление узлов становится более сложным, но обычно не требует обхода всего списка, что экономит время. Удаление одинаково независимо. Для проверки, находится ли узел в наборе, я использую карту массива с прямым доступом, так что это может быть сделано быстро за счет дополнительной памяти.

class OpenNodeSet
{
public:
OpenNodeSet(void);
~OpenNodeSet(void);

void add(GraphNode *n);
void remove(GraphNode *n);
void reinsert(GraphNode *n);
bool contains(GraphNode *n);
GraphNode* top(void);

int size(void)          {   return elements;    }
bool empty(void)        {   return (elements == 0); }

private:
typedef struct listNode {
GraphNode *node;
struct listNode *next;
} listNode_t;

listNode_t *root;
int elements;
unsigned __int8 map[1024][1024];
};

OpenNodeSet::OpenNodeSet(void)
{
root = NULL;
elements = 0;
for (int i = 0; i < 1024; i++)
for (int j = 0; j < 1024; j++)
map[i][j] = 0;
}

OpenNodeSet::~OpenNodeSet(void)
{
while (root != NULL) {
listNode_t *tmp = root->next;
free(root);
root = tmp;
}
}

void OpenNodeSet::add(GraphNode *n)
{
listNode_t **head;
for (head = &root; *head != NULL; head = &(*head)->next)
if ((*head)->node->f_score > n->f_score)
break;

listNode_t *newNode = (listNode_t*)malloc(sizeof(listNode_t));
newNode->node = n;
newNode->next = *head;
*head = newNode;
map[n->x][n->y] = 1;
elements++;
}

void OpenNodeSet::remove(GraphNode *n)
{
listNode_t **head;
for (head = &root; *head != NULL; head = &(*head)->next)
if ((*head)->node == n) {
listNode_t *tmp = *head;
*head = (*head)->next;
free(tmp);
map[n->x][n->y] = 0;
elements--;
return;
}
}

void OpenNodeSet::reinsert(GraphNode *n)
{
listNode_t **head, **bookmark = NULL;
listNode_t *ln = NULL;
int pos = 0;
for (head = &root; *head != NULL; head = &(*head)->next, pos++) {
if (bookmark == NULL && (*head)->node->f_score > n->f_score)
bookmark = head;
if (ln == NULL && (*head)->node == n) {
ln = *head;
*head = (*head)->next;
}
if (bookmark && ln)
break;
}

ln->next = (*bookmark)->next;
*bookmark = ln;
}

bool OpenNodeSet::contains(GraphNode *n)
{
return map[n->x][n->y]==1;
}

GraphNode* OpenNodeSet::top(void)
{
return root->node;
}

Код не такой чистый / эффективный, как должен быть в некоторых местах, сейчас я нахожусь в режиме работы. Проблема в том, что это не работает.

Есть функция повторной вставки, потому что часто мне нужно изменить счет узла, а значит и его позицию в списке. Вместо одного обхода списка для его удаления и другого для его вставки, я использую один обход, чтобы найти и позицию для повторной вставки узла, и сам узел, который должен спасти меня, выделяя память и другой обход. Проблема в том, что иногда функция повторной вставки не может найти узел для повторной установки, то есть узел не существует (и его нет, я проверял это вручную). Это не должно случиться все же. Если я игнорирую это, в конечном итоге я получаю ошибку сегментации (или аналог в Visual Studio), но я не уверен, является ли это просто результатом игнорирования. Вот функция для поиска пути:

void Graph::findPath(int start_x, int start_y, int goal_x, int goal_y)
{
GraphNode *start = map[start_x][start_y];
GraphNode *goal = map[goal_x][goal_y];
OpenNodeSet open;
NodeSet closed;

open.add(start);
start->g_score = 0;
start->f_score = distance(start, goal);

while (!open.empty()) {
//FIND MIN F_SCORE NODE AND REMOVE NODE FROM OPEN LIST
GraphNode *curr = open.top();
open.remove(curr);

//REACHED GOAL?
if (curr == goal) {
reconstructPath(curr);
break;
}

//ADD MIN COST NODE TO CLOSED LIST
closed.add(curr);

//FOR EACH NEIGHBOR OF NODE
for (int i = 0; i < curr->neighbor_count; i++) {
GraphNode *neighbor = curr->neighbors[i].node;
float cost = curr->neighbors[i].cost;

float tmp_g = curr->g_score + cost;
if (closed.contains(neighbor) && tmp_g >= neighbor->g_score)
continue;

bool inOpenSet = open.contains(neighbor);
if (!inOpenSet || tmp_g < neighbor->g_score) {
neighbor->parent = curr;
neighbor->g_score = tmp_g;
neighbor->f_score = tmp_g + distance(neighbor, goal);

if (inOpenSet)
open.reinsert(neighbor);
else
open.add(neighbor);
}
}
}
}

Что-то явно происходит с моей реализацией списка, но я не могу понять, что. Когда я пытаюсь проверить его с другими значениями, он ведет себя так, как должен. Я понятия не имею, если reinsert() работает так, как должно, так как он получает ввод, он не должен. Интересно, если я буду использовать следующее вместо reinsert()/add() звонки:

        if (inOpenSet) {
open.remove(neighbor);
}
open.add(neighbor);

…все вроде бы хорошо. Я даже проверил, нашел ли удалить в какой-то момент элемент, которого там не было, и, очевидно, его не было. Это заставляет меня подозревать reinsert() функции, но я не так уверен, как хотелось бы. Насколько я знаю, использование вместо этого удаления / добавления может просто заставить программу работать, но дать неверный результат. В любом случае, я смотрю вслепую и мне нужна другая перспектива.

Кто-нибудь может увидеть, что здесь происходит? Кажется, это особый случай, потому что проблема редкая, но я не могу ее воспроизвести. Когда я проверил это с искусственными ценностями, reinsert() делает то, что я ожидаю, без жалоб.

PS. Любые другие советы по оптимизации или лучшая эвристическая оценка стоимости, чем расстояние между узлами, также приветствуются. В основном, эта специфическая часть — все о скорости и меньше об использовании памяти, но оба хороши. Ох, и переполнение стека девственности ушло.

РЕДАКТИРОВАТЬ:

Получился хороший ужин, и несколько часов свободного времени — все, что мне было нужно. Последние две строки reinsert () должны были прочитать:

ln->next = *bookmark;
*bookmark = ln;

Не уверен, что я сделал это правильно, когда я тестировал его и не заметил, что код не совпадает, но все готово. Уменьшено время, затрачиваемое на исправление переоцененного узла в моем конкретном тестовом примере, на 25% по сравнению с обычной операцией удаления + добавления.

Тем не менее, всегда приветствуются новые идеи о том, как сделать его лучше. Я хотел бы иметь возможность выполнять бинарный поиск, чтобы найти позицию вставки, но я не могу придумать хороший способ сделать это, не прибегая к куче тонны памяти вокруг в огромных массивах фиксированного размера или постоянном перераспределении. () звонки.