int find_closest_on_hm_with_path(struct t_map* map, uint8_t sx, uint8_t sy, uint8_t* heatmap, uint8_t* viewarr, uint8_t* o_x, uint8_t* o_y) { uint16_t temp_patharr [ HEATMAP_SIZE ]; memset(temp_patharr, 255, sizeof temp_patharr); temp_patharr [sy * MAP_WIDTH + sx] = 0; struct pqueue_t* pq = pq_create(); pq_push(pq, (void*)((size_t)sy * MAP_WIDTH + sx + 1),0); // create a queue, set the source area to zero. //assumes the rest is already filled with 0xFFFF while (pq_size(pq)) { // while not empty int yx = (int)(size_t)(pq_pop(pq,NULL)) - 1; int x = (yx % MAP_WIDTH); int y = (yx / MAP_WIDTH); if (heatmap[yx]) { pq_destroy(pq); *o_y = y; *o_x = x; return 0; } int dir=0; //x and y now contain element with lowest priority for (dir=0; dir < MD_COUNT; dir++) { // for all neighbors int nx = x + movediff[dir][0]; int ny = y + movediff[dir][1]; if (!vtile(nx,ny)) continue; //this should be a valid tile! int cost = getcost(map,NULL,nx,ny,viewarr); if (cost == -1) continue; if (dir % 2) { cost = (cost * 3) / 2; } int alt = temp_patharr[y * MAP_WIDTH + x] + cost; if (alt < temp_patharr[ny * MAP_WIDTH + nx]) { temp_patharr[ny * MAP_WIDTH + nx] = alt; pq_push (pq, (void*) (ny * MAP_WIDTH + nx + 1), alt); } } } pq_destroy(pq); *o_x = 255; *o_y = 255; return 0; }
static char *pq_create_test() { struct pq *queue = pq_create(dummy_func); mu_assert("pq_create return NULL pointer", queue != NULL); pq_destroy(queue); queue = pq_create(NULL); mu_assert("pq_return non-NULL on invalid input: NULL func", queue == NULL); pq_destroy(queue); return 0; }
/* our main stub */ int main(int argc, char **argv) { int i = 0, p_tmp = 0; const char *res = NULL, *strdat[] = { "T1", "T2", "T3", "T5", "T6", "T3" }; int strprio[] = { 2, 10, 3, 9, 9, 2 }; pq_ptr pq = NULL; pq = pq_create(2*PQ_MIN_SZ); /* push all 5 tasks into q */ for (i = 0; i < 6; i++) pq_push(pq, (char *)strdat[i], strprio[i], FALSE); pq_decrease_priority(pq, (char *)strdat[2], 0); /* pop them and print them */ for(i = 0; i < 6; i++) { res = pq_pop(pq, &p_tmp); printf("Element: %s with priority: %d\n", res, p_tmp); } /* clear the queue */ pq_destroy(pq); return(EXIT_SUCCESS); }
static char *pq_min_test() { int i; int test_data[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; struct pq *queue = pq_create(int_min_cmp); for (i = 0; i < 10; i++) { pq_insert(queue, &test_data[i]); } for (i = 0; i < 10; i++) { int val = *((int *)pq_remove(queue)); mu_assert("values removed out of order\n", val == test_data[i]); } pq_destroy(queue); return 0; }
void dijkstra(graph_t *graph, int source, int *dist, int *parent) { push_data_t data; pq_elem_t e; int n, i; if (!dist) return; data.dist = dist; data.pq = pq_create(sizeof(pq_elem_t), pq_elem_compare); if (!data.pq) return; n = graph_vertex_count(graph); for (i=0; i<n; i++) dist[i] = INT_MAX; if (parent) { for (i=0; i<n; i++) { parent[i] = DIJKSTRA_NULL_PARENT; } } push(graph, source, source, -INT_MAX, &data); while(! pq_is_empty(data.pq)) { pq_delete_min(data.pq, &e); printf("%d -> %d: %d\n", e.source, e.sink, e.distance); if (dist[e.sink] == INT_MAX) { /* new edge */ dist[e.sink] = e.distance; if (parent) parent[e.sink] = e.source; graph_foreach_weighted(graph, e.sink, push, &data); } } pq_destroy(data.pq); }
int main( int argc, char** argv ) { uint64_t i; // pointers for casting pq_op_create *op_create; pq_op_destroy *op_destroy; pq_op_clear *op_clear; pq_op_get_key *op_get_key; pq_op_get_item *op_get_item; pq_op_get_size *op_get_size; pq_op_insert *op_insert; pq_op_find_min *op_find_min; pq_op_delete *op_delete; pq_op_delete_min *op_delete_min; pq_op_decrease_key *op_decrease_key; //pq_op_meld *op_meld; pq_op_empty *op_empty; // temp dummies for readability pq_type *q;//, *r; pq_node_type *n; if( argc < 2 ) exit( -1 ); int trace_file = open( argv[1], O_RDONLY ); if( trace_file < 0 ) { fprintf( stderr, "Could not open file.\n" ); return -1; } pq_trace_header header; pq_trace_read_header( trace_file, &header ); close( trace_file ); //printf("Header: (%llu,%lu,%lu)\n",header.op_count,header.pq_ids, // header.node_ids); pq_op_blank *ops = calloc( MIN( header.op_count, CHUNK_SIZE ), sizeof( pq_op_blank ) ); pq_type **pq_index = calloc( header.pq_ids, sizeof( pq_type* ) ); pq_node_type **node_index = calloc( header.node_ids, sizeof( pq_node_type* ) ); if( ops == NULL || pq_index == NULL || node_index == NULL ) { fprintf( stderr, "Calloc fail.\n" ); return -1; } #ifdef USE_QUAKE mem_capacities[0] = header.node_ids << 2; #else mem_capacities[0] = header.node_ids; #endif #ifdef USE_EAGER mem_map *map = mm_create( mem_types, mem_sizes, mem_capacities ); #else mem_map *map = mm_create( mem_types, mem_sizes ); #endif uint64_t op_remaining, op_chunk; int status; struct timeval t0, t1; uint32_t iterations = 0; uint32_t total_time = 0; key_type k; //pq_node_type *min; #ifndef CACHEGRIND while( iterations < 5 || total_time < PQ_MIN_USEC ) { mm_clear( map ); iterations++; #endif trace_file = open( argv[1], O_RDONLY ); if( trace_file < 0 ) { fprintf( stderr, "Could not open file.\n" ); return -1; } pq_trace_read_header( trace_file, &header ); op_remaining = header.op_count; while( op_remaining > 0 ) { op_chunk = MIN( CHUNK_SIZE, op_remaining ); op_remaining -= op_chunk; for( i = 0; i < op_chunk; i++ ) { status = pq_trace_read_op( trace_file, ops + i ); if( status == -1 ) { fprintf( stderr, "Invalid operation!" ); return -1; } } #ifndef CACHEGRIND gettimeofday(&t0, NULL); #endif for( i = 0; i < op_chunk; i++ ) { switch( ops[i].code ) { case PQ_OP_CREATE: op_create = (pq_op_create*) ( ops + i ); //printf("pq_create(%d)\n", op_create->pq_id); pq_index[op_create->pq_id] = pq_create( map ); break; case PQ_OP_DESTROY: op_destroy = (pq_op_destroy*) ( ops + i ); //printf("pq_destroy(%d)\n", op_destroy->pq_id); q = pq_index[op_destroy->pq_id]; pq_destroy( q ); pq_index[op_destroy->pq_id] = NULL; break; case PQ_OP_CLEAR: op_clear = (pq_op_clear*) ( ops + i ); //printf("pq_clear(%d)\n", op_clear->pq_id ); q = pq_index[op_clear->pq_id]; pq_clear( q ); break; case PQ_OP_GET_KEY: op_get_key = (pq_op_get_key*) ( ops + i ); //printf("pq_get_key(%d,%d)\n", op_get_key->pq_id, // op_get_key->node_id ); q = pq_index[op_get_key->pq_id]; n = node_index[op_get_key->node_id]; pq_get_key( q, n ); break; case PQ_OP_GET_ITEM: op_get_item = (pq_op_get_item*) ( ops + i ); //printf("pq_get_item(%d,%d)\n", op_get_item->pq_id, // op_get_item->node_id); q = pq_index[op_get_item->pq_id]; n = node_index[op_get_item->node_id]; pq_get_item( q, n ); break; case PQ_OP_GET_SIZE: op_get_size = (pq_op_get_size*) ( ops + i ); //printf("pq_get_size(%d)\n", op_get_size->pq_id); q = pq_index[op_get_size->pq_id]; pq_get_size( q ); break; case PQ_OP_INSERT: op_insert = (pq_op_insert*) ( ops + i ); //printf("pq_insert(%d,%d,%llu,%d)\n", op_insert->pq_id, // op_insert->node_id, op_insert->key, op_insert->item ); q = pq_index[op_insert->pq_id]; node_index[op_insert->node_id] = pq_insert( q, op_insert->item, op_insert->key ); break; case PQ_OP_FIND_MIN: op_find_min = (pq_op_find_min*) ( ops + i ); //printf("pq_find_min(%d)\n", op_find_min->pq_id ); q = pq_index[op_find_min->pq_id]; pq_find_min( q ); break; case PQ_OP_DELETE: op_delete = (pq_op_delete*) ( ops + i ); //printf("pq_delete(%d,%d)\n", op_delete->pq_id, // op_delete->node_id ); q = pq_index[op_delete->pq_id]; n = node_index[op_delete->node_id]; pq_delete( q, n ); break; case PQ_OP_DELETE_MIN: op_delete_min = (pq_op_delete_min*) ( ops + i ); //printf("pq_delete_min(%d)\n", op_delete_min->pq_id); q = pq_index[op_delete_min->pq_id]; //min = pq_find_min( q ); k = pq_delete_min( q ); #ifdef CACHEGRIND if( argc > 2 ) printf("%llu\n",k); #endif break; case PQ_OP_DECREASE_KEY: op_decrease_key = (pq_op_decrease_key*) ( ops + i ); //printf("pq_decrease_key(%d,%d,%llu)\n", op_decrease_key->pq_id, // op_decrease_key->node_id, op_decrease_key->key); q = pq_index[op_decrease_key->pq_id]; n = node_index[op_decrease_key->node_id]; pq_decrease_key( q, n, op_decrease_key->key ); break; /*case PQ_OP_MELD: printf("Meld.\n"); op_meld = (pq_op_meld*) ( ops + i ); q = pq_index[op_meld->pq_src1_id]; r = pq_index[op_meld->pq_src2_id]; pq_index[op_meld->pq_dst_id] = pq_meld( q, r ); break;*/ case PQ_OP_EMPTY: op_empty = (pq_op_empty*) ( ops + i ); //printf("pq_empty(%d)\n", op_empty->pq_id); q = pq_index[op_empty->pq_id]; pq_empty( q ); break; default: break; } //verify_queue( pq_index[0], header.node_ids ); } #ifndef CACHEGRIND gettimeofday(&t1, NULL); total_time += (t1.tv_sec - t0.tv_sec) * 1000000 + (t1.tv_usec - t0.tv_usec); #endif } close( trace_file ); #ifndef CACHEGRIND } #endif for( i = 0; i < header.pq_ids; i++ ) { if( pq_index[i] != NULL ) pq_destroy( pq_index[i] ); } mm_destroy( map ); free( pq_index ); free( node_index ); free( ops ); #ifndef CACHEGRIND printf( "%d\n", total_time / iterations ); #endif return 0; }
/* find the lowest cost path and mark it on the map */ void make_path(struct map *map, int x0, int y0, int x1, int y1) { int start, end, cost, i, index, mdistance; pq_t * pq; int edges[4]; pq = pq_create(); cost = 0; start = y0 * map->width + x0; end = y1 * map->width + x1; map->grid[start].flags |= SQ_FLAG_START; map->grid[end].flags |= SQ_FLAG_GOAL; map->grid[start].glyph = 'A'; map->grid[end].glyph = 'B'; if(map->grid[start].cost == -1 || map->grid[end].cost == -1) { map->cost = -1; return; } curses_draw_map(map); mdistance = man_distance(start, end, map); if(!pq_enqueue(pq, start, map->grid[start].cost + mdistance)) exit(EXIT_FAILURE); while(1) { if(!pq_dequeue(pq, &index, &cost)) exit(EXIT_FAILURE); if(map->grid[index].parent) /* * Kill two birds with one stone (Visited == enqueued in this case) */ map->grid[index].flags |= SQ_FLAG_VISITED; map->grid[index].flags &= ~SQ_FLAG_ENQUEUED; if(index == end) break; get_edges(index, edges, map); curses_draw_map(map); for(i = 0; i < 4; i++) { if(edges[i] != -1) { map->grid[edges[i]].parent = index; mdistance = man_distance(edges[i], end, map); if(!pq_enqueue(pq, edges[i], (cost) + map->grid[edges[i]].cost)) exit(EXIT_FAILURE); map->grid[edges[i]].flags |= SQ_FLAG_ENQUEUED; map->grid[edges[i]].flags |= SQ_FLAG_VISITED; } } } /* * This next section will fill the path array in the map struct by iterating * through the path found in the above code, which is done by following each * node's parent. * It will begin with setting the total cost of the path to the cost of the * end node, then working backwards towards the start node which has a zero cost * (which is actually appended to the total cost). * It will then begin iteration, setting the glyph and flags, then setting the * start and end node's glyphs, as these will be set with 'o' during the loop. * If the start and the end have the same coordinates, there will be a zero cost * because the only node which is appended to the total is the end, which is * the start, which has a zero cost. */ i = end; /* * Instantiate the path array */ map->path = safe_malloc(map->width * map->height * sizeof(int)); map->path_index = 0; map->cost = -1; /* * Dont include the start node's cost in the calculation */ map->grid[start].cost = 0; /* * Make sure the start node's parent is 0; * */ map->grid[start].parent = 0; map->cost = map->grid[i].cost; while(map->grid[i].parent) { /* This will miss the start node, therefore we will append it after the loop */ map->grid[i].flags |= SQ_FLAG_PATH; map->grid[i].glyph = 'o'; map->path[map->path_index++] = i; i = map->grid[i].parent; map->cost += map->grid[i].cost; } map->path[map->path_index++] = i; map->grid[start].glyph = 'A'; map->grid[end].glyph = 'B'; curses_draw_map(map); pq_destroy(pq); }
/* * return 0 if nothing was modidied * return 1 if elevation was modified */ int do_flatarea(int index, CELL ele, CELL *alt_org, CELL *alt_new) { int upr, upc, r, c, ct_dir; CELL is_in_list, is_worked, this_in_list; int index_doer, index_up; int n_flat_cells = 0, counter; CELL ele_nbr, min_ele_diff; int uphill_order, downhill_order, max_uphill_order, max_downhill_order; int last_order; struct pq *up_pq = pq_create(); struct pq *down_pq = pq_create(); struct orders inc_order, *order_found, *nbr_order_found; struct RB_TREE *order_tree = rbtree_create(cmp_orders, sizeof(struct orders)); pq_add(index, down_pq); pq_add(index, up_pq); inc_order.downhill = -1; inc_order.uphill = 0; inc_order.index = index; inc_order.flag = 0; rbtree_insert(order_tree, &inc_order); n_flat_cells = 1; min_ele_diff = INT_MAX; max_uphill_order = max_downhill_order = 0; /* get uphill start points */ G_debug(2, "get uphill start points"); counter = 0; while (down_pq->size) { if ((index_doer = pq_drop(down_pq)) == -1) G_fatal_error("get start points: no more points in down queue"); seg_index_rc(alt_seg, index_doer, &r, &c); FLAG_SET(flat_done, r, c); /* check all neighbours, breadth first search */ for (ct_dir = 0; ct_dir < sides; ct_dir++) { /* get r, c (upr, upc) for this neighbour */ upr = r + nextdr[ct_dir]; upc = c + nextdc[ct_dir]; /* check if r, c are within region */ if (upr >= 0 && upr < nrows && upc >= 0 && upc < ncols) { index_up = SEG_INDEX(alt_seg, upr, upc); is_in_list = FLAG_GET(in_list, upr, upc); is_worked = FLAG_GET(worked, upr, upc); ele_nbr = alt_org[index_up]; if (ele_nbr == ele && !is_worked) { inc_order.downhill = -1; inc_order.uphill = -1; inc_order.index = index_up; inc_order.flag = 0; /* not yet added to queue */ if ((order_found = rbtree_find(order_tree, &inc_order)) == NULL) { n_flat_cells++; /* add to down queue if not yet in there */ pq_add(index_up, down_pq); /* add to up queue if not yet in there */ if (is_in_list) { pq_add(index_up, up_pq); /* set uphill order to 0 */ inc_order.uphill = 0; counter++; } rbtree_insert(order_tree, &inc_order); } } } } } /* flat area too small, not worth the effort */ if (n_flat_cells < 5) { /* clean up */ pq_destroy(up_pq); pq_destroy(down_pq); rbtree_destroy(order_tree); return 0; } G_debug(2, "%d flat cells, %d cells in tree, %d start cells", n_flat_cells, (int)order_tree->count, counter); pq_destroy(down_pq); down_pq = pq_create(); /* got uphill start points, do uphill correction */ G_debug(2, "got uphill start points, do uphill correction"); counter = 0; uphill_order = 1; while (up_pq->size) { int is_in_down_queue = 0; if ((index_doer = pq_drop(up_pq)) == -1) G_fatal_error("uphill order: no more points in up queue"); seg_index_rc(alt_seg, index_doer, &r, &c); this_in_list = FLAG_GET(in_list, r, c); /* get uphill order for this point */ inc_order.index = index_doer; if ((order_found = rbtree_find(order_tree, &inc_order)) == NULL) G_fatal_error(_("flat cell escaped for uphill correction")); last_order = uphill_order - 1; uphill_order = order_found->uphill; if (last_order > uphill_order) G_warning(_("queue error: last uphill order %d > current uphill order %d"), last_order, uphill_order); /* debug */ if (uphill_order == -1) G_fatal_error(_("uphill order not set")); if (max_uphill_order < uphill_order) max_uphill_order = uphill_order; uphill_order++; counter++; /* check all neighbours, breadth first search */ for (ct_dir = 0; ct_dir < sides; ct_dir++) { /* get r, c (upr, upc) for this neighbour */ upr = r + nextdr[ct_dir]; upc = c + nextdc[ct_dir]; /* check if r, c are within region */ if (upr >= 0 && upr < nrows && upc >= 0 && upc < ncols) { index_up = SEG_INDEX(alt_seg, upr, upc); is_in_list = FLAG_GET(in_list, upr, upc); is_worked = FLAG_GET(worked, upr, upc); ele_nbr = alt_org[index_up]; /* all cells that are in_list should have been added * previously as uphill start points */ if (ele_nbr == ele && !is_worked) { inc_order.index = index_up; if ((nbr_order_found = rbtree_find(order_tree, &inc_order)) == NULL) { G_fatal_error(_("flat cell escaped in uphill correction")); } /* not yet added to queue */ if (nbr_order_found->uphill == -1) { if (is_in_list) G_warning("cell should be in queue"); /* add to up queue */ pq_add(index_up, up_pq); /* set nbr uphill order = current uphill order + 1 */ nbr_order_found->uphill = uphill_order; } } /* add focus cell to down queue */ if (!this_in_list && !is_in_down_queue && ele_nbr != ele && !is_in_list && !is_worked) { pq_add(index_doer, down_pq); /* set downhill order to 0 */ order_found->downhill = 0; is_in_down_queue = 1; } if (ele_nbr > ele && min_ele_diff > ele_nbr - ele) min_ele_diff = ele_nbr - ele; } } } /* debug: all flags should be set to 0 */ pq_destroy(up_pq); up_pq = pq_create(); /* got downhill start points, do downhill correction */ G_debug(2, "got downhill start points, do downhill correction"); downhill_order = 1; while (down_pq->size) { if ((index_doer = pq_drop(down_pq)) == -1) G_fatal_error(_("downhill order: no more points in down queue")); seg_index_rc(alt_seg, index_doer, &r, &c); this_in_list = FLAG_GET(in_list, r, c); /* get downhill order for this point */ inc_order.index = index_doer; if ((order_found = rbtree_find(order_tree, &inc_order)) == NULL) G_fatal_error(_("flat cell escaped for downhill correction")); last_order = downhill_order - 1; downhill_order = order_found->downhill; if (last_order > downhill_order) G_warning(_("queue error: last downhill order %d > current downhill order %d"), last_order, downhill_order); /* debug */ if (downhill_order == -1) G_fatal_error(_("downhill order: downhill order not set")); if (max_downhill_order < downhill_order) max_downhill_order = downhill_order; downhill_order++; /* check all neighbours, breadth first search */ for (ct_dir = 0; ct_dir < sides; ct_dir++) { /* get r, c (upr, upc) for this neighbour */ upr = r + nextdr[ct_dir]; upc = c + nextdc[ct_dir]; /* check if r, c are within region */ if (upr >= 0 && upr < nrows && upc >= 0 && upc < ncols) { index_up = SEG_INDEX(alt_seg, upr, upc); is_in_list = FLAG_GET(in_list, upr, upc); is_worked = FLAG_GET(worked, upr, upc); ele_nbr = alt_org[index_up]; if (ele_nbr == ele && !is_worked) { inc_order.index = index_up; if ((nbr_order_found = rbtree_find(order_tree, &inc_order)) == NULL) G_fatal_error(_("flat cell escaped in downhill correction")); /* not yet added to queue */ if (nbr_order_found->downhill == -1) { /* add to down queue */ pq_add(index_up, down_pq); /* set nbr downhill order = current downhill order + 1 */ nbr_order_found->downhill = downhill_order; /* add to up queue */ if (is_in_list) { pq_add(index_up, up_pq); /* set flag */ nbr_order_found->flag = 1; } } } } } } /* got uphill and downhill order, adjust ele */ /* increment: ele += uphill_order + max_downhill_order - downhill_order */ /* decrement: ele += uphill_order - max_uphill_order - downhill_order */ G_debug(2, "adjust ele"); while (up_pq->size) { if ((index_doer = pq_drop(up_pq)) == -1) G_fatal_error("no more points in up queue"); seg_index_rc(alt_seg, index_doer, &r, &c); this_in_list = FLAG_GET(in_list, r, c); /* get uphill and downhill order for this point */ inc_order.index = index_doer; if ((order_found = rbtree_find(order_tree, &inc_order)) == NULL) G_fatal_error(_("flat cell escaped for adjustment")); uphill_order = order_found->uphill; downhill_order = order_found->downhill; /* debug */ if (uphill_order == -1) G_fatal_error(_("adjustment: uphill order not set")); if (!this_in_list && downhill_order == -1) G_fatal_error(_("adjustment: downhill order not set")); /* increment */ if (this_in_list) { downhill_order = max_downhill_order; uphill_order = 0; } alt_new[index_doer] += (uphill_order + (double)(max_downhill_order - downhill_order) / 2.0 + 0.5) / 2.0 + 0.5; /* check all neighbours, breadth first search */ for (ct_dir = 0; ct_dir < sides; ct_dir++) { /* get r, c (upr, upc) for this neighbour */ upr = r + nextdr[ct_dir]; upc = c + nextdc[ct_dir]; /* check if r, c are within region */ if (upr >= 0 && upr < nrows && upc >= 0 && upc < ncols) { index_up = SEG_INDEX(alt_seg, upr, upc); is_in_list = FLAG_GET(in_list, upr, upc); is_worked = FLAG_GET(worked, upr, upc); ele_nbr = alt_org[index_up]; if (ele_nbr == ele && !is_worked) { inc_order.index = index_up; if ((nbr_order_found = rbtree_find(order_tree, &inc_order)) == NULL) G_fatal_error(_("flat cell escaped in adjustment")); /* not yet added to queue */ if (nbr_order_found->flag == 0) { if (is_in_list) G_warning("adjustment: in_list cell should be in queue"); /* add to up queue */ pq_add(index_up, up_pq); nbr_order_found->flag = 1; } } } } } /* clean up */ pq_destroy(up_pq); pq_destroy(down_pq); rbtree_destroy(order_tree); return 1; }