void HeapContainer<NodeType,CostType,PlannerSpecificVariables>::push
( SearchGraphNode<NodeType,CostType,PlannerSpecificVariables>* np,
SearchGraphNode<NodeType,CostType,PlannerSpecificVariables>* searchStart )
{
if (!start && !end) // empty list
{
key_init(np);
start = np;
end = np;
heap_size = 1;
}
else
{
heapItem_p pointer;
if (searchStart)
pointer = searchStart;
else
pointer = key_find(np->f);
while (true)
{
// found position at the beginning
if ( pointer->f <= np->f && !pointer->prev )
{
start->prev = np;
np->nxt = start;
start = np;
break;
}
// found position at the middle
if ( pointer->f <= np->f && pointer->prev->f >= np->f )
{
np->prev = pointer->prev;
np->nxt = pointer;
pointer->prev->nxt = np;
pointer->prev = np;
break;
}
// found position at the end
if ( pointer->f >= np->f && !pointer->nxt )
{
end->nxt = np;
np->prev = end;
end = np;
break;
}
if ( pointer->f <= np->f )
pointer = pointer->prev;
else if ( pointer->f > np->f )
pointer = pointer->nxt;
}
np->inHeap = true;
heap_size++;
key_update(np);
}
}
Handle h_find(H_Type type, uintptr_t key)
{
H_ScopedLock s;
return key_find(key, type);
}
static void key_remove(uintptr_t key, H_Type type)
{
Handle ret = key_find(key, type, KEY_REMOVE);
ENSURE(ret > 0);
}
