/* qloop takes a vector (or heap) of regions to check (checking) if they don't intersect
* anything. If a region does intersect something, it is broken into
* pieces that don't intersect that thing (if possible) which are
* put back into the vector/heap of regions to check.
* qloop returns false when it finds the first empty region
* it returns true if it has exhausted the region vector/heap and never
* found an empty area.
*/
static void
qloop (struct query_closure *qc, rtree_t * tree, heap_or_vector res, bool is_vec)
{
BoxType *cbox;
#ifndef NDEBUG
int n;
#endif
while (!(qc->desired ? heap_is_empty (qc->checking.h) : vector_is_empty (qc->checking.v)))
{
cbox = qc->desired ? (BoxTypePtr)heap_remove_smallest (qc->checking.h) : (BoxTypePtr)vector_remove_last (qc->checking.v);
if (setjmp (qc->env) == 0)
{
assert (box_is_good (cbox));
qc->cbox = cbox;
#ifndef NDEBUG
n =
#endif
r_search (tree, cbox, NULL, query_one, qc);
assert (n == 0);
/* nothing intersected with this tree, put it in the result vector */
if (is_vec)
vector_append (res.v, cbox);
else
{
if (qc->desired)
heap_append (res.h, qc->desired, cbox);
else
vector_append (res.v, cbox);
}
return; /* found one - perhaps one answer is good enough */
}
}
}
/* Complexity: O(n log n), worst-case if data is located
* at the right-most leaf node on the lowest level of the
* tree.
*/
int heap_remove(Heap *heap, const void *data)
{
unsigned long i;
assert(heap != NULL);
if(heap_is_empty(heap)) {
return -1;
}
for(i = 0; i < heap_size(heap); i++) {
if(darray_index(heap->h, i) == data) {
if(darray_swap(heap->h, i, darray_size(heap->h) - 1) < 0) {
return -1;
}
/* Don't care about the return value */
darray_remove(heap->h, darray_size(heap->h) - 1);
heapify_down(heap, i);
return 0;
}
}
return -1;
}
void dijkstra(graph* g, unsigned int source) {
unsigned int u, v, edge_count;
node *n, *d;
edge *e;
heap *Q;
g->nodes[source].distance = 0;
Q = heap_make(compare, g);
while(!heap_is_empty(Q)) {
u = heap_delete_min(Q);
n = &g->nodes[u];
edge_count = n->edge_count;
for(v = 0; v < edge_count; v++) {
e = &n->edges[v];
d = &g->nodes[e->destination];
if(d->distance > n->distance + e->weight) {
/*
Relajo los vertices
*/
d->distance = n->distance + e->weight;
/*
Actualizo el nodo con la distancia optima a este
*/
d->previous = u;
/*
Actualizo la cola de prioridad (el vertice solo puede
haber subido en prioridad, entonces solo hago heapify-up
*/
heap_heapify_up(Q, d->heap_index);
}
}
}
heap_destroy(Q);
}
int queue_is_empty(event_queue_t* queue) {
int rval;
pthread_mutex_lock(&(queue->lock));
rval = heap_is_empty(&(queue->heap));
pthread_mutex_unlock(&(queue->lock));
return rval;
}
int * dijkstra(graph g, int from, int*tree, int (*add)(int, int)){
int * somme, i, act;/*Commistione linguistica che bonato ama! xD*/
heap_t *heap;
somme = malloc(sizeof(int) * g->v);
assert(somme != NULL);
/*Piazzo tutto a -1 che in binario è 11111111....1111 ... fino alla nausea... 1111*/
memset(somme, 0xff, g->v * sizeof(int));
/*Good! Ora però la prima va a 0!*/
somme[from] = 0;
/*Ci siamo quasi...*/
/*ora le heap!*/
heap = heap_make(somme, g->v, heapcmp, sizeof(int));
/*Metto a TRUE la flag *connected*/
g->connesso = 1;
/*Ultimissima cosa: inizializzo l'albero delle visite ad una foresta di alberi di un solo elemento*/
for(i = 0; i < g->v; i++)
tree[i] = i;
/* Ora il ciclo, prepariamo l'assorbente xD*/
while(!heap_is_empty(heap)){
/* Estraggo il primo elemento*/
from = heap_extract(heap);
/* Se la distanza per arrivare è ancora infinita (-1), significa che questa parte del
* Grafo non è mai stata raggiunta da nessun arco e quindi è disconnessa dalla componente
* In cui è presente il nodo di partenza, e quindi, visto che questo è il massimo possibili
* tutti gli altri elementi nella heap avranno lo stesso valore, quindi interrompo qui
* l'esecuzione */
if(somme[from] == -1) {
g->connesso = 0;/* Segno che il grafo non è connesso*/
/*interrompo il ciclo*/
break;
}
/*E itero tra tutto */
for(i = 0; i < g->v; i++){
if(g->adj[from][i]){
/*Mi calcolo la distanza attuale + quella dell'arco tramite una funzione esterna*/
act = add(somme[from], g->adj[from][i]);
/*Se la somma è a -1 (non è ancora stato aggiornato) oppure il nuovo valore
e' inferiore a quello vecchio, lo metto uguale e aggiorno l'albero delle visite*/
if(somme[i] == -1 || somme[i] > act){
somme[i] = act;
tree[i] = from;
/*Ho aggiornato: devo fare un fix della heap che potrebbe essere andata a farsi fottere!*/
heap_fix(heap, i);
}
}
}
}
heap_free(heap);/*Questa non mi cancella l'array somme, quindi sono contento xD*/
/*Bene! abbiamo finito! :)*/
return somme;
}
/* Complexity: O(1) */
void* heap_top(Heap *heap)
{
assert(heap != NULL);
if(heap_is_empty(heap)) {
return NULL;
}
return darray_index(heap->h, 0);
}
unsigned int heap_delete_min(heap* h) {
unsigned int retval = h->nodes[0];
h->nodes[0] = h->nodes[h->count-1];
h->count--;
if(heap_is_empty(h)) {
free(h->nodes);
} else {
heap_heapify_down(h);
}
return retval;
}
int main()
{
heap_t heap = heap_init(10, is_smaller2);
printf("Testing empty heap...\n");
assert_verbose(heap_is_empty(heap) == true);
printf("Adding an item...\n");
heap_insert(heap, "charlie");
printf("Adding an item...\n");
heap_insert(heap, "alpha");
printf("Adding an item...\n");
heap_insert(heap, "bravo");
printf("Testing non-empty heap...\n");
assert_verbose(heap_is_empty(heap) == false);
printf("Testing output...\n");
assert_verbose(strcmp(heap_delete_min(heap), "alpha") == 0);
assert_verbose(strcmp(heap_delete_min(heap), "bravo") == 0);
assert_verbose(strcmp(heap_delete_min(heap), "charlie") == 0);
printf("Testing empty heap...\n");
assert_verbose(heap_is_empty(heap) == true);
heap_free(heap);
heap = heap_init(10, is_smaller);
for(int i = 30; i > 10; i--)
{
heap_insert(heap, &i);
}
assert_verbose(*((int *)heap_delete_min(heap)) == 10);
heap_free(heap);
printf("Passed all tests\n");
}
long long queue_peek_event_time(event_queue_t* queue) {
long long rval;
pthread_mutex_lock(&(queue->lock));
if (heap_is_empty(&(queue->heap))) {
rval = -1;
}
else {
rval = heap_get_min_key(&(queue->heap));
}
pthread_mutex_unlock(&(queue->lock));
return rval;
}
/* Complexity: O(size(heap1) + 2 * size(heap2)) => O(n) */
int heap_merge(Heap *heap1, Heap* heap2)
{
unsigned long i;
assert(heap1 != NULL);
assert(heap2 != NULL);
if(heap_is_empty(heap1) && heap_is_empty(heap2)) {
return -1;
}
/* O(size(heap2)) */
if(darray_concat(heap1->h, heap2->h) < 0) {
return -1;
}
/* O(size(heap1) + size(heap2)) */
for(i = (darray_size(heap1->h) - 1) / 2; i > 0; i--) {
heapify_down(heap1, i);
}
heapify_down(heap1, 0); /* Edge-case for loop 0 index */
return 0;
}
void *
heap_replace (heap_t * heap, cost_t cost, void *data)
{
assert (heap && __heap_is_good (heap));
if (heap_is_empty (heap))
return data;
assert (heap->size > 0);
assert (heap->max > 1);
heap->element[0].cost = cost;
heap->element[0].data = data;
__downheap (heap, 0); /* ooh, tricky! */
assert (__heap_is_good (heap));
return heap->element[0].data;
}
/* Complexity: O(log n) */
void* heap_pop(Heap *heap)
{
void *ret = NULL;
assert(heap != NULL);
if(heap_is_empty(heap)) {
return NULL;
}
if(darray_swap(heap->h, 0, darray_size(heap->h) - 1) < 0) {
return NULL;
}
ret = darray_remove(heap->h, darray_size(heap->h) - 1);
heapify_down(heap, 0);
return ret;
}
void*
receive_data_messages_thread()
{
while(true)
{
pthread_mutex_lock(&data_message_queue_mutex);
while(heap_is_empty(data_message_queue))
{
debug(5, "wait for data message timeout");
pthread_cond_wait(&data_message_queue_cond, &data_message_queue_mutex);
}
bool wait = true;
struct timespec timeout;
struct data_message* message = *(struct data_message**) heap_first(data_message_queue);
//while(wait && message->state != COMPLETE && message->receive_time + DATA_MESSAGE_TIMEOUT // TODO
}
}
main()
{
int a[]={8,4,11,7,1,3,9,2,13,5,10,12,6};
int i;
int size=sizeof(a)/sizeof(int);
int element;
struct heap myheap;
heap_init(&myheap);
heap_print(&myheap);
for(i=0;i<size;i++)
{
printf("Inserting element %d\n",a[i]);
heap_insert_element(&myheap,a[i]);
heap_print(&myheap);
}
// print_tree(a,size);
while(!heap_is_empty(&myheap))
{
element=heap_remove_element(&myheap);
printf("Removed element is %d\n",element);
//heap_print(&myheap);
}
}
BOOL
priqueue_is_empty(struct PriQueue *priqueue)
{
assert(priqueue != NULL);
return heap_is_empty(priqueue->heap);
}
static void init_task_gen(struct task_base *tbase, struct task_args *targ)
{
struct task_gen_server *task = (struct task_gen_server *)tbase;
const int socket_id = rte_lcore_to_socket_id(targ->lconf->id);
static char name[] = "server_mempool";
name[0]++;
task->mempool = rte_mempool_create(name,
4*1024 - 1, MBUF_SIZE,
targ->nb_cache_mbuf,
sizeof(struct rte_pktmbuf_pool_private),
rte_pktmbuf_pool_init, NULL,
rte_pktmbuf_init, 0,
socket_id, 0);
PROX_PANIC(task->mempool == NULL, "Failed to allocate memory pool with %u elements\n", 4*1024 - 1);
int pop = lua_getfrom(prox_lua(), GLOBAL, targ->streams);
PROX_PANIC(pop < 0, "Failed to find '%s' in lua\n", targ->streams);
lua_len(prox_lua(), -1);
uint32_t n_listen = lua_tointeger(prox_lua(), -1);
lua_pop(prox_lua(), 1);
PROX_PANIC(n_listen == 0, "No services specified to listen on\n");
task->bundle_cfgs = prox_zmalloc(n_listen * sizeof(task->bundle_cfgs[0]), socket_id);
plogx_info("n_listen = %d\n", n_listen);
struct hash_set *hs = prox_sh_find_socket(socket_id, "genl4_streams");
if (hs == NULL) {
/* Expected number of streams per bundle = 1, hash_set
will grow if full. */
hs = hash_set_create(n_listen, socket_id);
prox_sh_add_socket(socket_id, "genl4_streams", hs);
}
const struct rte_hash_parameters listen_table = {
.name = name,
.entries = n_listen * 4,
.key_len = sizeof(struct new_tuple),
.hash_func = rte_hash_crc,
.hash_func_init_val = 0,
.socket_id = socket_id,
};
name[0]++;
task->listen_hash = rte_hash_create(&listen_table);
task->listen_entries = prox_zmalloc(listen_table.entries * sizeof(task->listen_entries[0]), socket_id);
int idx = 0;
lua_pushnil(prox_lua());
while (lua_next(prox_lua(), -2)) {
task->bundle_cfgs[idx].n_stream_cfgs = 1;
task->bundle_cfgs[idx].stream_cfgs = prox_zmalloc(sizeof(*task->bundle_cfgs[idx].stream_cfgs), socket_id);
int ret = lua_to_stream_cfg(prox_lua(), STACK, NULL, socket_id, &task->bundle_cfgs[idx].stream_cfgs[0], hs);
PROX_PANIC(ret, "Failed to load stream cfg\n");
struct stream_cfg *stream = task->bundle_cfgs[idx].stream_cfgs[0];
// TODO: check mask and add to hash for each host
struct new_tuple nt = {
.dst_addr = stream->servers.ip,
.proto_id = stream->proto,
.dst_port = stream->servers.port,
.l2_types[0] = 0x0008,
};
ret = rte_hash_add_key(task->listen_hash, &nt);
PROX_PANIC(ret < 0, "Failed to add\n");
task->listen_entries[ret] = &task->bundle_cfgs[idx];
plogx_dbg("Server = "IPv4_BYTES_FMT":%d\n", IPv4_BYTES(((uint8_t*)&nt.dst_addr)), rte_bswap16(nt.dst_port));
++idx;
lua_pop(prox_lua(), 1);
}
static char name2[] = "task_gen_hash2";
name2[0]++;
plogx_dbg("Creating bundle ctx pool\n");
if (bundle_ctx_pool_create(name2, targ->n_concur_conn * 2, &task->bundle_ctx_pool, NULL, 0, NULL, socket_id)) {
cmd_mem_stats();
PROX_PANIC(1, "Failed to create conn_ctx_pool\n");
}
task->heap = heap_create(targ->n_concur_conn * 2, socket_id);
task->seed = rte_rdtsc();
/* TODO: calculate the CDF of the reply distribution and the
number of replies as the number to cover for 99% of the
replies. For now, assume that this is number is 2. */
uint32_t queue_size = rte_align32pow2(targ->n_concur_conn * 2);
PROX_PANIC(queue_size == 0, "Overflow resulted in queue size 0\n");
task->fqueue = fqueue_create(queue_size, socket_id);
PROX_PANIC(task->fqueue == NULL, "Failed to allocate local queue\n");
uint32_t n_descriptors;
if (targ->nb_txports) {
PROX_PANIC(targ->nb_txports != 1, "Need exactly one TX port for L4 generation\n");
n_descriptors = prox_port_cfg[targ->tx_port_queue[0].port].n_txd;
} else {
PROX_PANIC(targ->nb_txrings != 1, "Need exactly one TX ring for L4 generation\n");
n_descriptors = 256;
}
struct token_time_cfg tt_cfg = {
.bpp = targ->rate_bps,
.period = rte_get_tsc_hz(),
.bytes_max = n_descriptors * (ETHER_MIN_LEN + 20),
};
token_time_init(&task->token_time, &tt_cfg);
}
static void init_task_gen_client(struct task_base *tbase, struct task_args *targ)
{
struct task_gen_client *task = (struct task_gen_client *)tbase;
static char name[] = "gen_pool";
const uint32_t socket = rte_lcore_to_socket_id(targ->lconf->id);
name[0]++;
task->mempool = rte_mempool_create(name,
4*1024 - 1, MBUF_SIZE,
targ->nb_cache_mbuf,
sizeof(struct rte_pktmbuf_pool_private),
rte_pktmbuf_pool_init, NULL,
rte_pktmbuf_init, 0,
socket, 0);
PROX_PANIC(task->mempool == NULL, "Failed to allocate memory pool with %u elements\n", 4*1024 - 1);
/* streams contains a lua table. Go through it and read each
stream with associated imix_fraction. */
uint32_t imix;
uint32_t i = 0;
int pop = lua_getfrom(prox_lua(), GLOBAL, targ->streams);
PROX_PANIC(pop < 0, "Failed to find '%s' in lua\n", targ->streams);
lua_len(prox_lua(), -1);
uint32_t n_bundle_cfgs = lua_tointeger(prox_lua(), -1);
lua_pop(prox_lua(), 1);
PROX_PANIC(n_bundle_cfgs == 0, "No configs specified\n");
plogx_info("loading %d bundle_cfgs\n", n_bundle_cfgs);
struct hash_set *hs = prox_sh_find_socket(socket, "genl4_streams");
if (hs == NULL) {
/* Expected number of streams per bundle = 8, hash_set
will grow if full. */
hs = hash_set_create(n_bundle_cfgs * 8, socket);
prox_sh_add_socket(socket, "genl4_streams", hs);
}
task->bundle_cfgs = prox_zmalloc(n_bundle_cfgs * sizeof(task->bundle_cfgs[0]), socket);
lua_pushnil(prox_lua());
int total_imix = 0;
uint32_t *occur = prox_zmalloc(n_bundle_cfgs * sizeof(*occur), socket);
struct cdf *cdf = cdf_create(n_bundle_cfgs, socket);
while (lua_next(prox_lua(), -2)) {
PROX_PANIC(lua_to_int(prox_lua(), TABLE, "imix_fraction", &imix) ||
lua_to_bundle_cfg(prox_lua(), TABLE, "bundle", socket, &task->bundle_cfgs[i], hs),
"Failed to load bundle cfg:\n%s\n", get_lua_to_errors());
cdf_add(cdf, imix);
occur[i] = imix;
total_imix += imix;
++i;
lua_pop(prox_lua(), 1);
}
lua_pop(prox_lua(), pop);
cdf_setup(cdf);
PROX_PANIC(targ->max_setup_rate == 0, "Max setup rate not set\n");
task->new_conn_cost = rte_get_tsc_hz()/targ->max_setup_rate;
static char name2[] = "task_gen_hash";
name2[0]++;
plogx_dbg("Creating bundle ctx pool\n");
if (bundle_ctx_pool_create(name2, targ->n_concur_conn, &task->bundle_ctx_pool, occur, n_bundle_cfgs, task->bundle_cfgs, socket)) {
cmd_mem_stats();
PROX_PANIC(1, "Failed to create conn_ctx_pool\n");
}
task->heap = heap_create(targ->n_concur_conn, socket);
task->seed = rte_rdtsc();
/* task->token_time.bytes_max = MAX_PKT_BURST * (ETHER_MAX_LEN + 20); */
/* To avoid overflowing the tx descriptors, the token bucket
size needs to be limited. The descriptors are filled most
quickly with the smallest packets. For that reason, the
token bucket size is given by "number of tx descriptors" *
"smallest Ethernet packet". */
PROX_ASSERT(targ->nb_txports == 1);
struct token_time_cfg tt_cfg = {
.bpp = targ->rate_bps,
.period = rte_get_tsc_hz(),
.bytes_max = prox_port_cfg[targ->tx_port_queue[0].port].n_txd * (ETHER_MIN_LEN + 20),
};
token_time_init(&task->token_time, &tt_cfg);
}
static void start_task_gen_client(struct task_base *tbase)
{
struct task_gen_client *task = (struct task_gen_client *)tbase;
token_time_reset(&task->token_time, rte_rdtsc(), 0);
task->new_conn_tokens = 0;
task->new_conn_last_tsc = rte_rdtsc();
}
static void stop_task_gen_client(struct task_base *tbase)
{
struct task_gen_client *task = (struct task_gen_client *)tbase;
struct bundle_ctx *bundle;
while (!heap_is_empty(task->heap)) {
bundle = BUNDLE_CTX_UPCAST(heap_pop(task->heap));
bundle_expire(bundle, &task->bundle_ctx_pool, &task->l4_stats);
}
}
static void start_task_gen_server(struct task_base *tbase)
{
struct task_gen_server *task = (struct task_gen_server *)tbase;
token_time_reset(&task->token_time, rte_rdtsc(), 0);
}
static void stop_task_gen_server(struct task_base *tbase)
{
struct task_gen_server *task = (struct task_gen_server *)tbase;
struct bundle_ctx *bundle;
uint8_t out[MAX_PKT_BURST];
while (!heap_is_empty(task->heap)) {
bundle = BUNDLE_CTX_UPCAST(heap_pop(task->heap));
bundle_expire(bundle, &task->bundle_ctx_pool, &task->l4_stats);
}
if (task->cancelled) {
struct rte_mbuf *mbuf = task->mbuf_saved;
out[0] = OUT_DISCARD;
task->cancelled = 0;
task->base.tx_pkt(&task->base, &mbuf, 1, out);
}
do {
if (task->cur_mbufs_beg == task->cur_mbufs_end) {
task->cur_mbufs_end = fqueue_get(task->fqueue, task->cur_mbufs, MAX_PKT_BURST);
task->cur_mbufs_beg = 0;
if (task->cur_mbufs_end == 0)
break;
}
uint16_t n_pkts = task->cur_mbufs_end - task->cur_mbufs_beg;
struct rte_mbuf **mbufs = task->cur_mbufs + task->cur_mbufs_beg;
if (n_pkts) {
for (uint16_t j = 0; j < n_pkts; ++j) {
out[j] = OUT_DISCARD;
}
task->base.tx_pkt(&task->base, mbufs, n_pkts, out);
}
} while (1);
}
static struct task_init task_init_gen1 = {
.mode_str = "genl4",
.sub_mode_str = "server",
.init = init_task_gen,
.handle = handle_gen_bulk,
.start = start_task_gen_server,
.stop = stop_task_gen_server,
.flag_features = TASK_FEATURE_ZERO_RX,
.size = sizeof(struct task_gen_server),
.mbuf_size = 2048 + sizeof(struct rte_mbuf) + RTE_PKTMBUF_HEADROOM,
};
static struct task_init task_init_gen2 = {
.mode_str = "genl4",
.init = init_task_gen_client,
.handle = handle_gen_bulk_client,
.start = start_task_gen_client,
.stop = stop_task_gen_client,
.flag_features = TASK_FEATURE_ZERO_RX,
.size = sizeof(struct task_gen_client),
.mbuf_size = 2048 + sizeof(struct rte_mbuf) + RTE_PKTMBUF_HEADROOM,
};
__attribute__((constructor)) static void reg_task_gen(void)
{
reg_task(&task_init_gen1);
reg_task(&task_init_gen2);
}
list* search_a_star(void* state,
void* state_world,
search_is_goal state_goal_func,
search_gen_successors state_gen_func,
search_link_parent state_link_func,
search_goal_backtrace state_back_func,
search_trans_cost state_trans_func,
search_heuristic state_heur_func,
search_set_f_cost state_f_cost_set_func,
hash_func state_hash_alg,
generic_comp state_comp_func,
generic_cpy state_copy_func,
generic_op state_free_func,
heap_comp state_heap_func) {
int* g_cost_ptr, *f_cost_ptr, f_cost, tmp_f, g_cost, found;
void* current_state, *successor_state, *heap_memory_location;
list* states_overflow, *successor_list, *path;
hash_table* states_closed_set, *states_open_set;
hash_map* states_g_cost, *states_f_cost, *states_heap_index;
heap* states_heap;
states_overflow = list_create(NULL,
NULL,
state_free_func);
states_closed_set = hash_table_create(89,
.75,
state_hash_alg,
state_comp_func,
state_copy_func,
state_free_func);
states_open_set = hash_table_create(89,
.75,
state_hash_alg,
state_comp_func,
state_copy_func,
state_free_func);
states_g_cost = hash_map_create(89,
.75,
state_hash_alg,
state_comp_func,
NULL,
NULL,
NULL,
state_free_func,
(generic_op)free);
states_f_cost = hash_map_create(89,
.75,
state_hash_alg,
state_comp_func,
NULL,
NULL,
NULL,
state_free_func,
(generic_op)free);
states_heap_index = hash_map_create(89,
.75,
state_hash_alg,
state_comp_func,
NULL,
NULL,
NULL,
NULL,
NULL);
states_heap = heap_create(89,
state_heap_func,
state_comp_func,
state_copy_func,
state_free_func);
current_state = state;
f_cost = state_heur_func(current_state, NULL);
state_f_cost_set_func(current_state, f_cost);
g_cost = 0;
g_cost_ptr = malloc(sizeof(int));
*g_cost_ptr = g_cost;
f_cost_ptr = malloc(sizeof(int));
*f_cost_ptr = f_cost;
hash_map_insert(states_g_cost, current_state, g_cost_ptr, 0);
heap_memory_location = heap_add(states_heap, state_copy_func(current_state));
hash_table_insert(states_open_set, state_copy_func(current_state), 0);
hash_map_insert(states_f_cost, state_copy_func(current_state), f_cost_ptr, 0);
hash_map_insert(states_heap_index, current_state, heap_memory_location, 1);
path = NULL;
found = 0;
while(!heap_is_empty(states_heap) && !found) {
current_state = state_copy_func(heap_peek(states_heap));
heap_remove(states_heap);
hash_table_remove(states_open_set, current_state);
hash_map_remove(states_heap_index, current_state);
if(state_goal_func(current_state, state_world)) {
path = state_back_func(current_state);
found = 1;
} else {
if(!hash_table_insert(states_closed_set, current_state, 0)) {
list_push_front(states_overflow, current_state);
}
successor_list = state_gen_func(current_state, state_world);
while(!list_is_empty(successor_list)) {
successor_state = list_front(successor_list);
g_cost = *(int*)hash_map_get(states_g_cost, current_state) +
state_trans_func(current_state, successor_state, state_world);
f_cost = g_cost + state_heur_func(successor_state, state_world);
tmp_f = hash_map_contains_key(states_f_cost, successor_state) ?
*(int*)hash_map_get(states_f_cost, successor_state) : UINT_MAX;
if(hash_table_contains(states_closed_set, successor_state) && f_cost > tmp_f) {
list_remove_front(successor_list);
continue;
}
if(!hash_table_contains(states_open_set, successor_state) || f_cost < tmp_f) {
state_f_cost_set_func(successor_state, f_cost);
state_link_func(successor_state, current_state);
g_cost_ptr = malloc(sizeof(int));
f_cost_ptr = malloc(sizeof(int));
*g_cost_ptr = g_cost;
*f_cost_ptr = f_cost;
if(!hash_table_contains(states_open_set, successor_state)) {
hash_table_insert(states_open_set, successor_state, 0);
heap_memory_location = heap_add(states_heap, state_copy_func(successor_state));
hash_map_insert(states_heap_index, successor_state, heap_memory_location, 1);
} else {
heap_memory_location = hash_map_get(states_heap_index, successor_state);
heap_up_mod_data(states_heap, heap_memory_location, successor_state);
}
if(!hash_map_set(states_g_cost, successor_state, g_cost_ptr)) {
hash_map_insert(states_g_cost, state_copy_func(successor_state), g_cost_ptr, 0);
}
if(!hash_map_set(states_f_cost, successor_state, f_cost_ptr)) {
hash_map_insert(states_f_cost, state_copy_func(successor_state), f_cost_ptr, 0);
}
list_pop(successor_list);
} else {
list_remove_front(successor_list);
}
}
list_kill(successor_list);
}
}
heap_kill(states_heap);
list_kill(states_overflow);
hash_map_kill(states_g_cost);
hash_map_kill(states_f_cost);
hash_table_kill(states_open_set);
hash_table_kill(states_closed_set);
hash_map_dissolve(states_heap_index);
return path;
}
/* returns some empty spaces in 'region' (or former narrowed regions)
* that may hold a feature with the specified radius and keepaway
* It tries first to find Completely empty regions (which are appended
* to the free_space_vec vector). If that fails, it looks for regions
* filled only by objects generated by the previous pass (which are
* appended to the lo_conflict_space_vec vector). Then it looks for
* regions that are filled by objects generated during *this* pass
* (which are appended to the hi_conflict_space_vec vector). The
* current pass identity is given by 'is_odd'. As soon as one completely
* free region is found, it returns with that answer. It saves partially
* searched regions in vectors "untested", "no_fix", "no_hi", and
* "hi_candidate" which can be passed to future calls of this function
* to search harder for such regions if the computation becomes
* necessary.
*/
vetting_t *
mtspace_query_rect (mtspace_t * mtspace, const BoxType * region,
Coord radius, Coord keepaway,
vetting_t * work,
vector_t * free_space_vec,
vector_t * lo_conflict_space_vec,
vector_t * hi_conflict_space_vec,
bool is_odd, bool with_conflicts, CheapPointType *desired)
{
struct query_closure qc;
/* pre-assertions */
assert (free_space_vec);
assert (lo_conflict_space_vec);
assert (hi_conflict_space_vec);
/* search out to anything that might matter */
if (region)
{
BoxType *cbox;
assert (work == NULL);
assert (box_is_good (region));
assert(vector_is_empty (free_space_vec));
assert(vector_is_empty (lo_conflict_space_vec));
assert(vector_is_empty (hi_conflict_space_vec));
work = (vetting_t *) malloc (sizeof (vetting_t));
work->keepaway = keepaway;
work->radius = radius;
cbox = (BoxType *) malloc (sizeof (BoxType));
*cbox = bloat_box (region, keepaway + radius);
if (desired)
{
work->untested.h = heap_create ();
work->no_fix.h = heap_create ();
work->hi_candidate.h = heap_create ();
work->no_hi.h =heap_create ();
assert (work->untested.h && work->no_fix.h &&
work->no_hi.h && work->hi_candidate.h);
heap_insert (work->untested.h, 0, cbox);
work->desired = *desired;
}
else
{
work->untested.v = vector_create ();
work->no_fix.v = vector_create ();
work->hi_candidate.v = vector_create ();
work->no_hi.v = vector_create ();
assert (work->untested.v && work->no_fix.v &&
work->no_hi.v && work->hi_candidate.v);
vector_append (work->untested.v, cbox);
work->desired.X = work->desired.Y = -SPECIAL;
}
return work;
}
qc.keepaway = work->keepaway;
qc.radius = work->radius;
if (work->desired.X == -SPECIAL && work->desired.Y == -SPECIAL)
qc.desired = NULL;
else
qc.desired = &work->desired;
/* do the query */
do
{
heap_or_vector temporary = {free_space_vec};
/* search the fixed object tree discarding any intersections
* and placing empty regions in the no_fix vector.
*/
qc.checking = work->untested;
qc.touching.v = NULL;
qloop (&qc, mtspace->ftree, work->no_fix, false);
/* search the hi-conflict tree placing intersectors in the
* hi_candidate vector (if conflicts are allowed) and
* placing empty regions in the no_hi vector.
*/
qc.checking.v = work->no_fix.v;
qc.touching.v = with_conflicts ? work->hi_candidate.v : NULL;
qc.touch_is_vec = false;
qloop (&qc, is_odd ? mtspace->otree : mtspace->etree, work->no_hi, false);
/* search the lo-conflict tree placing intersectors in the
* lo-conflict answer vector (if conflicts allowed) and
* placing emptry regions in the free-space answer vector.
*/
qc.checking = work->no_hi;
/* XXX lo_conflict_space_vec will be treated like a heap! */
qc.touching.v = (with_conflicts ? lo_conflict_space_vec : NULL);
qc.touch_is_vec = true;
qloop (&qc, is_odd ? mtspace->etree : mtspace->otree, temporary, true);
/* qloop (&qc, is_odd ? mtspace->etree : mtspace->otree, (heap_or_vector)free_space_vec, true); */
if (!vector_is_empty (free_space_vec))
{
if (qc.desired)
{
if (heap_is_empty (work->untested.h))
break;
}
else
{
if (vector_is_empty (work->untested.v))
break;
}
return work;
}
/* finally check the hi-conflict intersectors against the
* lo-conflict tree discarding intersectors (two types of conflict is real bad)
* and placing empty regions in the hi-conflict answer vector.
*/
if (with_conflicts)
{
heap_or_vector temporary = {hi_conflict_space_vec};
qc.checking = work->hi_candidate;
qc.touching.v = NULL;
qloop (&qc, is_odd ? mtspace->etree : mtspace->otree,
temporary, true);
/* qloop (&qc, is_odd ? mtspace->etree : mtspace->otree, */
/* (heap_or_vector)hi_conflict_space_vec, true); */
}
}
while (!(qc.desired ? heap_is_empty(work->untested.h) : vector_is_empty (work->untested.v)));
if (qc.desired)
{
if (heap_is_empty (work->no_fix.h) &&
heap_is_empty (work->no_hi.h) &&
heap_is_empty (work->hi_candidate.h))
{
mtsFreeWork (&work);
return NULL;
}
}
else
{
if (vector_is_empty (work->no_fix.v) &&
vector_is_empty (work->no_hi.v) && vector_is_empty (work->hi_candidate.v))
{
mtsFreeWork (&work);
return NULL;
}
}
return work;
}
void heap_destroy(heap* h) {
if(!heap_is_empty(h)) {
free(h->nodes);
}
free(h);
}
