c++ - Sorted Linked List: optimized add/remove/reordering of elements -

i'm doing implementation of a* pathfinding algorithm based on pseudocode wikipedia, , i'm trying optimize performance. i've noted of time wasted on handling nodes in "open" set of nodes (basically nodes examine), i've been doing best make faster.

(background, feel free skip code) basically, have set of nodes , following:

• find node lowest score
• check if node contained within set
• remove node set
• add node set

by using sorted linked list, finding lowest score goes checking elements fetching first. adding nodes becomes more complicated, doesn't require traversing entire list, saves time. removing same regardless. checking if node in set, use array map direct access can done @ cost of additional memory.

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;     (int = 0; < 1024; i++)         (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;     (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;     (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;     (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; } 

the code not clean/efficient should in places, right i'm in get-it-to-work-mode. problem is, doesn't work.

the reinsert function there because need change score of node, , position in list. instead of having 1 list traversal removing , inserting it, use single traversal find both position reinsert node, , node should save me allocating memory , traversal. problem is, reinsert function fails find node reinsert, say, node doesn't exist (and doesn't, i've manually checked it). shouldn't happen though. if ignore it, segmentation fault (or equivalent in visual studio), i'm not sure if that's result of ignoring it. here's function finding path:

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 , remove node open list         graphnode *curr = open.top();         open.remove(curr);          //reached goal?         if (curr == goal) {             reconstructpath(curr);             break;         }          //add min cost node closed list         closed.add(curr);          //for each neighbor of node         (int = 0; < 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);             }         }     } } 

something going on list implementation, can't figure out what. when try test other values, behaves should. have no idea if reinsert() works should since gets input shouldn't. interestingly, if use following instead of reinsert()/add() calls:

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

...things seem fine. checked if remove @ point found element wasn't there, , apparently, wasn't. makes me suspect reinsert() function, i'm not i'd be. know, using remove/add instead make program work, give incorrect result. either way, i've been staring myself blind , need perspective.

can see going on here? seems sort of special case since problem rare, can't reproduce it. when i've tested artificial values, reinsert() expect without complaints.

ps. other optimization tips or better heuristic cost estimate distance between nodes welcome well. mostly, specific part speed , less memory usage, both good. oh, , stack overflow virginity gone.

edit:

turns out nice dinner , few hours of spare time needed. last 2 lines of reinsert() should've read:

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

not sure if did right when tested , didn't notice code didn't match, there go. reduced time spent on fixing re-scored node in particular test case 25% compared regular remove+add operation.

still, more ideas on how make better welcome. i'd able binary search find insertion position, can't think of way while not having shovel around ton of memory around in huge fixed-size arrays or constant realloc() calls.