List *Path(World *world, Point *source, Point *destination)
{
int tentative_gscore;
List *result = NULL;
int tries = 0;
Hashmap *nodes = Hashmap_create(cmp, hash);
Hashmap *closedset = Hashmap_create(cmp, hash);
PQueue *openset = PQueue_create(cmp, hash);
Node *current;
Node *start = Node_create(source, 0, 0, NULL);
Hashmap_set(nodes, start, start);
start->fscore = start->gscore + heuristic_cost_estimate(start->point, destination);
PQueue_push(openset, start, start->fscore);
while(!PQueue_empty(openset) &&
tries < 300) {
tries++;
current = PQueue_pop(openset);
Hashmap_set(closedset, current, current);
if(POINT_EQ(current->point, destination)) {
result = reconstruct_path(current);
break;
} else {
List *neighbours = neighbours_list(world, current->point, destination, nodes);
LIST_FOREACH(neighbours, first, next, cur) {
Node *neighbour = cur->value;
if(Hashmap_get(closedset, neighbour) == NULL) {
tentative_gscore = current->gscore + 1;
if(!PQueue_contains(openset, neighbour) ||
tentative_gscore > neighbour->gscore) {
if(!PQueue_contains(openset, neighbour)) {
neighbour->came_from = current;
neighbour->gscore = tentative_gscore;
neighbour->fscore = neighbour->gscore + heuristic_cost_estimate(neighbour->point, destination);
PQueue_push(openset, neighbour, neighbour->fscore);
}
}
}
}
List_destroy(neighbours);
}
}
int main(void){
register guint i;
register PQueueSet *pq_set = PQueueSet_create();
register PQueue *pq = PQueue_create(pq_set, test_comp_low_int,
NULL);
register Data *dn;
PQueue_Node *acc[DATA_TOTAL];
Data data[DATA_TOTAL] = {
{ "REMOVE-1", -1, 300, 200, 50},
{ "jumps", -1, 39, 38, 32}, { "dog", -1, 72, 61, 60},
{ "brown", -1, 99, 40, 8}, { "over", -1, 68, 35, 33},
{ "REMOVE-2", -1, 25, 20, 15},
{ "fox", -1, 20, 19, 17}, { "quick", -1, 99, 19, 3},
{ "lazy", -1, 99, 67, 51}, { "the", -1, 3, 2, 1},
{ "a", -1, 82, 41, 34},
{ "REMOVE-3", -1, 40, 30, 20}
};
gint r[3] = {0, 5, 11};
gchar *answer[DATA_TOTAL-3] = {
"the", "quick", "brown", "fox", "jumps",
"over", "a", "lazy", "dog"};
g_print("Adding words with initial priorities\n");
for(i = 0; i < DATA_TOTAL; i++){
g_print("Adding [%s]\n", data[i].word);
data[i].real_priority = data[i].priority1;
acc[i] = PQueue_push(pq, (gpointer)&data[i]);
}
g_print("Set intermediate priorities\n");
for(i = 0; i < DATA_TOTAL; i++){
g_print("Raising [%s] from %d to %d\n",
data[i].word, data[i].priority1, data[i].priority2);
data[i].real_priority = data[i].priority2;
PQueue_raise(pq, acc[i]);
}
g_print("Set final priorities\n");
for(i = 0; i < DATA_TOTAL; i++){
g_print("Raising [%s] from %d to %d\n",
data[i].word, data[i].priority2, data[i].priority3);
data[i].real_priority = data[i].priority3;
PQueue_raise(pq, acc[i]);
}
g_print("Testing traverse\n");
PQueue_traverse(pq, test_traverse_func, NULL);
for(i = 0; i < 3; i++){
g_print("Testing remove of \"%s\"\n", data[r[i]].word);
PQueue_remove(pq, acc[r[i]]);
g_print("Testing traverse\n");
PQueue_traverse(pq, test_traverse_func, NULL);
}
g_print("Popping data\n");
for(i = 0; i < (DATA_TOTAL-3); i++){
dn = PQueue_pop(pq);
g_print("Word: [%s] Answer: [%s]\n", dn->word, answer[i]);
g_assert(!strcmp(dn->word, answer[i]));
}
PQueue_destroy(pq, NULL, NULL);
PQueueSet_destroy(pq_set);
return 0;
}
void JobQueue_submit(JobQueue *jq, JobQueue_Func job_func,
gpointer job_data, gint priority){
#ifdef USE_PTHREADS
register JobQueue_Task *task;
g_assert(jq);
pthread_mutex_lock(&jq->queue_lock);
task = JobQueue_Task_create(job_func, job_data, priority);
PQueue_push(jq->pq, task);
pthread_mutex_unlock(&jq->queue_lock);
#else /* USE_PTHREADS */
job_func(job_data); /* when no threads available, just run the job */
#endif /* USE_PTHREADS */
return;
}