void
trickle_up (int index, struct pqueue *queue)
{
void *tmp;
/* Save current node as tmp node. */
tmp = queue->array[index];
/* Continue until the node reaches top or the place where the parent
node should be upper than the tmp node. */
while (index > 0 &&
(*queue->cmp) (tmp, queue->array[PARENT_OF (index)]) < 0)
{
/* actually trickle up */
queue->array[index] = queue->array[PARENT_OF (index)];
if (queue->update != NULL)
(*queue->update) (queue->array[index], index);
index = PARENT_OF (index);
}
/* Restore the tmp node to appropriate place. */
queue->array[index] = tmp;
if (queue->update != NULL)
(*queue->update) (tmp, index);
}
void
astar_heap_add (asheap_t * heap, uint32_t val, square_t * square)
{
assert (heap != NULL);
// Is is full?
if (heap->length == heap->alloc) {
heap->alloc += heap->delta;
heap->data = (uint32_t *) realloc (heap->data, sizeof (uint32_t) * heap->alloc);
check_null (heap->data, "heap_add(), growing data block");
heap->squares = (square_t **) realloc (heap->squares,
sizeof (square_t *) * heap->alloc);
check_null (heap->squares, "heap_add(), growing payload block");
}
// Is it empty? Trivial case.
if (heap->length == 0) {
heap->data[0] = val;
heap->squares[0] = square;
heap->length = 1;
return;
}
// Stick the new value at the end.
heap->data [heap->length] = val;
heap->squares [heap->length] = square;
// Bubble up.
register uint32_t parent_val, parent_ofs;
register uint32_t i = heap->length;
void * parent_payload;
while (i > 0) {
// Get parent value.
parent_ofs = PARENT_OF(i);
parent_val = heap->data [parent_ofs];
parent_payload = heap->squares [parent_ofs];
// If the parent is greater then val, swap them.
if (parent_val > val) {
heap->data [i] = parent_val;
heap->data [parent_ofs] = val;
heap->squares [i] = parent_payload;
heap->squares [parent_ofs] = square;
// Loop again.
i = parent_ofs;
} else {
// Not greater, end bubbling up.
break;
}
}
// Increase the number of elements.
heap->length++;
}
inline static uint32_t
astar_heap_bubble_up (asheap_t * heap, int32_t heapofs)
{
// Bubble up. This keeps the heap consistent (all children
// have greater values).
register uint32_t parent_val, parent_ofs;
register uint32_t val;
register int32_t i = heapofs;
// Let's do this thang.
while (i > 0) {
#ifdef HEAP_DEBUG
__debug("*** BUBBLING UP, i=%d\n", i);
astar_heap_fprint (heap, __heap_debugfp);
#endif // HEAP_DEBUG
// Get the current value of f.
val = heap->data[i];
// Get parent value.
parent_ofs = PARENT_OF(i);
parent_val = heap->data [parent_ofs];
__debug ("*** THIS: i=%d val=%d, ofs=%u\n", i, val, heap->squares[i]->ofs);
__debug ("*** PARENT: i=%d val=%d, ofs=%u\n", parent_ofs, parent_val, heap->squares[parent_ofs]->ofs);
// If the parent is greater then val, swap them.
if (parent_val > val) {
// Swap the values.
heap->data [i] = parent_val;
heap->data [parent_ofs] = val;
// Swap the payload (grid offset).
square_t * tmp = heap->squares [i];
heap->squares [i] = heap->squares [parent_ofs];
heap->squares [parent_ofs] = tmp;
__debug ("*** SWAPPED\n\n");
__debug ("*** THIS: i=%d val=%d, ofs=%u\n", i, val, heap->squares[i]->ofs);
__debug ("*** PARENT: i=%d val=%d, ofs=%u\n", parent_ofs, parent_val, heap->squares[parent_ofs]->ofs);
// Loop again.
i = parent_ofs;
} else {
// Not greater, stop here.
break;
}
}
// Return the final offset of the value in the heap.
return i;
}
void pqueue_remove_at(int index, struct pqueue *queue)
{
queue->array[index] = queue->array[--queue->size];
if (index > 0
&& (*queue->cmp)(queue->array[index],
queue->array[PARENT_OF(index)])
< 0) {
trickle_up(index, queue);
} else {
trickle_down(index, queue);
}
}
void
astar_heap_highlight (asheap_t * heap, uint32_t highlight)
{
uint32_t i;
for (i=0; i<heap->length; i++) {
uint32_t val = heap->data[i];
int p = i > 0? PARENT_OF(i): 0;
if (val == highlight) printf ("%2d -> \033[0;7m%3d\033[0m", i, val, p);
else printf ("%2d -> %3d", i, val, p);
if (i > 0) printf(" <- %3d", p);
#ifdef SQUARE_HAS_OFS
printf(" ofs=%u \n", heap->squares[i]->ofs);
#endif
}
printf("\n");
}
/* =============================================================================
* fixAfterDeletion
* =============================================================================
*/
static void
fixAfterDeletion (rbtree_t* s, node_t* x)
{
while (x != LDNODE(s,root) && COLOR_OF(x) == BLACK) {
if (x == LEFT_OF(PARENT_OF(x))) {
node_t* sib = RIGHT_OF(PARENT_OF(x));
if (COLOR_OF(sib) == RED) {
SET_COLOR(sib, BLACK);
SET_COLOR(PARENT_OF(x), RED);
ROTATE_LEFT(s, PARENT_OF(x));
sib = RIGHT_OF(PARENT_OF(x));
}
if (COLOR_OF(LEFT_OF(sib)) == BLACK &&
COLOR_OF(RIGHT_OF(sib)) == BLACK) {
SET_COLOR(sib, RED);
x = PARENT_OF(x);
} else {
if (COLOR_OF(RIGHT_OF(sib)) == BLACK) {
SET_COLOR(LEFT_OF(sib), BLACK);
SET_COLOR(sib, RED);
ROTATE_RIGHT(s, sib);
sib = RIGHT_OF(PARENT_OF(x));
}
SET_COLOR(sib, COLOR_OF(PARENT_OF(x)));
SET_COLOR(PARENT_OF(x), BLACK);
SET_COLOR(RIGHT_OF(sib), BLACK);
ROTATE_LEFT(s, PARENT_OF(x));
/* TODO: consider break ... */
x = LDNODE(s,root);
}
} else { /* symmetric */
node_t* sib = LEFT_OF(PARENT_OF(x));
if (COLOR_OF(sib) == RED) {
SET_COLOR(sib, BLACK);
SET_COLOR(PARENT_OF(x), RED);
ROTATE_RIGHT(s, PARENT_OF(x));
sib = LEFT_OF(PARENT_OF(x));
}
if (COLOR_OF(RIGHT_OF(sib)) == BLACK &&
COLOR_OF(LEFT_OF(sib)) == BLACK) {
SET_COLOR(sib, RED);
x = PARENT_OF(x);
} else {
if (COLOR_OF(LEFT_OF(sib)) == BLACK) {
SET_COLOR(RIGHT_OF(sib), BLACK);
SET_COLOR(sib, RED);
ROTATE_LEFT(s, sib);
sib = LEFT_OF(PARENT_OF(x));
}
SET_COLOR(sib, COLOR_OF(PARENT_OF(x)));
SET_COLOR(PARENT_OF(x), BLACK);
SET_COLOR(LEFT_OF(sib), BLACK);
ROTATE_RIGHT(s, PARENT_OF(x));
/* TODO: consider break ... */
x = LDNODE(s, root);
}
}
}
if (x != NULL && LDF(x,c) != BLACK) {
STF(x, c, BLACK);
}
}
/* =============================================================================
* fixAfterInsertion
* =============================================================================
*/
static void
fixAfterInsertion (rbtree_t* s, node_t* x)
{
STF(x, c, RED);
while (x != NULL && x != LDNODE(s, root)) {
node_t* xp = LDNODE(x, p);
if (LDF(xp, c) != RED) {
break;
}
/* TODO: cache g = ppx = PARENT_OF(PARENT_OF(x)) */
if (PARENT_OF(x) == LEFT_OF(PARENT_OF(PARENT_OF(x)))) {
node_t* y = RIGHT_OF(PARENT_OF(PARENT_OF(x)));
if (COLOR_OF(y) == RED) {
SET_COLOR(PARENT_OF(x), BLACK);
SET_COLOR(y, BLACK);
SET_COLOR(PARENT_OF(PARENT_OF(x)), RED);
x = PARENT_OF(PARENT_OF(x));
} else {
if (x == RIGHT_OF(PARENT_OF(x))) {
x = PARENT_OF(x);
ROTATE_LEFT(s, x);
}
SET_COLOR(PARENT_OF(x), BLACK);
SET_COLOR(PARENT_OF(PARENT_OF(x)), RED);
if (PARENT_OF(PARENT_OF(x)) != NULL) {
ROTATE_RIGHT(s, PARENT_OF(PARENT_OF(x)));
}
}
} else {
node_t* y = LEFT_OF(PARENT_OF(PARENT_OF(x)));
if (COLOR_OF(y) == RED) {
SET_COLOR(PARENT_OF(x), BLACK);
SET_COLOR(y, BLACK);
SET_COLOR(PARENT_OF(PARENT_OF(x)), RED);
x = PARENT_OF(PARENT_OF(x));
} else {
if (x == LEFT_OF(PARENT_OF(x))) {
x = PARENT_OF(x);
ROTATE_RIGHT(s, x);
}
SET_COLOR(PARENT_OF(x), BLACK);
SET_COLOR(PARENT_OF(PARENT_OF(x)), RED);
if (PARENT_OF(PARENT_OF(x)) != NULL) {
ROTATE_LEFT(s, PARENT_OF(PARENT_OF(x)));
}
}
}
}
node_t* ro = LDNODE(s, root);
if (LDF(ro, c) != BLACK) {
STF(ro, c, BLACK);
}
}