void put_weights(t_map *map)
{
t_list queue;
t_room *curr;
ft_list_init(&queue, NULL);
map->end->weight = 0;
ft_list_add_front(&queue, map->end, sizeof(t_room));
while (queue.size >= 1)
{
curr = ft_list_remove_back(&queue);
add_neighbours_to_queue(curr, &queue);
}
cut(map);
map->start->ants = map->ants;
}
void
carmen_conventional_dynamic_program(int goal_x, int goal_y)
{
double *utility_ptr;
int index;
double max_val, min_val;
int num_expanded;
int done;
struct timeval start_time, end_time;
int delta_sec, delta_usec;
static double last_time, cur_time, last_print;
queue state_queue;
state_ptr current_state;
if (costs == NULL)
return;
gettimeofday(&start_time, NULL);
cur_time = carmen_get_time();
if ((int)(cur_time - last_print) > 10) {
carmen_verbose("Time since last DP: %d secs, %d usecs\n", (int)(cur_time - last_time),
((int)((cur_time-last_time)*1e6)) % 1000000);
last_print = cur_time;
}
last_time = cur_time;
if (utility == NULL) {
utility = (double *)calloc(x_size*y_size, sizeof(double));
carmen_test_alloc(utility);
}
utility_ptr = utility;
for (index = 0; index < x_size * y_size; index++)
*(utility_ptr++) = -1;
if (is_out_of_map(goal_x, goal_y))
return;
max_val = -MAXDOUBLE;
min_val = MAXDOUBLE;
done = 0;
state_queue = make_queue();
current_state = carmen_conventional_create_state(goal_x, goal_y, 0);
max_val = MAX_UTILITY;
*(utility_value(goal_x, goal_y)) = max_val;
add_neighbours_to_queue(goal_x, goal_y, state_queue);
num_expanded = 1;
while ((current_state = pop_queue(state_queue)) != NULL) {
num_expanded++;
if (current_state->cost <= *(utility_value(current_state->x, current_state->y)))
add_neighbours_to_queue(current_state->x, current_state->y, state_queue);
if (current_state->cost < min_val)
min_val = current_state->cost;
free(current_state);
}
delete_queue(&state_queue);
gettimeofday(&end_time, NULL);
delta_usec = end_time.tv_usec - start_time.tv_usec;
delta_sec = end_time.tv_sec - start_time.tv_sec;
if (delta_usec < 0) {
delta_sec--;
delta_usec += 1000000;
}
// carmen_warn("Elasped time for dp: %d secs, %d usecs\n", delta_sec, delta_usec);
}
static void search(carmen_roadmap_vertex_t *start_node,
carmen_roadmap_t *roadmap)
{
int num_expanded;
state_ptr current_state;
int i, j;
double *fwd_utilities, *open_list;
int *parent_ptrs;
carmen_roadmap_vertex_t *node_list;
carmen_roadmap_edge_t *edges;
double min_neighbour_utility;
int min_neighbour, neighbour_id;
int min_edge;
double best_utility, utility;
int best_neighbour;
queue the_state_queue = NULL;
the_state_queue = make_queue();
node_list = (carmen_roadmap_vertex_t *)(roadmap->nodes->list);
fwd_utilities = (double *)calloc(roadmap->nodes->length, sizeof(double));
carmen_test_alloc(fwd_utilities);
for (i = 0; i < roadmap->nodes->length; i++)
fwd_utilities[i] = FLT_MAX;
open_list = (double *)calloc(roadmap->nodes->length, sizeof(double));
carmen_test_alloc(open_list);
for (i = 0; i < roadmap->nodes->length; i++)
open_list[i] = -1;
parent_ptrs = (int *)calloc(roadmap->nodes->length, sizeof(int));
carmen_test_alloc(parent_ptrs);
for (i = 0; i < roadmap->nodes->length; i++)
parent_ptrs[i] = -1;
push_state(start_node->id, start_node->id, 0, 0, the_state_queue);
fwd_utilities[start_node->id] = 0;
node_list[roadmap->goal_id].utility = 0;
num_expanded = 0;
while ((current_state = pop_queue(the_state_queue)) != NULL) {
num_expanded++;
if (0)
carmen_warn("Popped %d\n", current_state->id);
fwd_utilities[current_state->id] =
fwd_utilities[current_state->parent_id] + current_state->cost;
parent_ptrs[current_state->id] = current_state->parent_id;
open_list[current_state->id] = -2;
assert (fwd_utilities[current_state->id] < 1e5);
if (current_state->id == roadmap->goal_id) {
if (node_list[roadmap->goal_id].edges->length == 0)
construct_edges(roadmap, roadmap->goal_id);
empty_queue(the_state_queue);
} else {
add_neighbours_to_queue(roadmap, current_state->id,
roadmap->goal_id, fwd_utilities, open_list,
the_state_queue);
}
free(current_state);
}
carmen_warn("Num Expanded %d\n", num_expanded);
if (fwd_utilities[roadmap->goal_id] > FLT_MAX/2)
carmen_warn("PROBLEM\n");
else if (0) {
node_list[roadmap->goal_id].utility = 0;
i = roadmap->goal_id;
while (i != start_node->id) {
edges = (carmen_roadmap_edge_t *)(node_list[i].edges->list);
assert (node_list[i].edges->length > 0);
min_neighbour_utility = FLT_MAX;
min_neighbour = -1;
min_edge = -1;
for (j = 0; j < node_list[i].edges->length; j++) {
if (edges[j].cost > 1e5 || edges[j].blocked)
continue;
neighbour_id = edges[j].id;
assert (fwd_utilities[neighbour_id] >= 0);
if (fwd_utilities[neighbour_id] < min_neighbour_utility) {
min_neighbour_utility = fwd_utilities[neighbour_id];
min_neighbour = neighbour_id;
min_edge = j;
}
}
assert (min_neighbour >= 0);
assert (min_neighbour_utility < fwd_utilities[i]);
assert(node_list[min_neighbour].utility > FLT_MAX/2 ||
node_list[min_neighbour].utility ==
node_list[i].utility + edges[min_edge].cost);
if (node_list[min_neighbour].utility > FLT_MAX/2) {
node_list[min_neighbour].utility = node_list[i].utility +
edges[min_edge].cost;
assert(node_list[i].utility < FLT_MAX/2);
assert(fwd_utilities[i] < FLT_MAX/2);
assert(fwd_utilities[min_neighbour] < FLT_MAX/2);
assert(node_list[min_neighbour].utility < 1e5);
}
assert (node_list[i].utility < node_list[min_neighbour].utility);
// carmen_warn("%d %d %f %f\n", node_list[i].x, node_list[i].y, node_list[i].utility, FLT_MAX/2);
i = min_neighbour;
assert(node_list[i].utility < FLT_MAX/2);
}
} else {
node_list[roadmap->goal_id].utility = 0;
i = roadmap->goal_id;
while (i != start_node->id) {
min_neighbour = parent_ptrs[i];
assert (min_neighbour >= 0);
min_neighbour_utility = fwd_utilities[min_neighbour];
assert (min_neighbour_utility < fwd_utilities[i]);
edges = (carmen_roadmap_edge_t *)(node_list[i].edges->list);
assert (node_list[i].edges->length > 0);
for (min_edge = 0; min_edge < node_list[i].edges->length; min_edge++) {
if (edges[min_edge].id == min_neighbour)
break;
}
assert (min_edge < node_list[i].edges->length);
assert(node_list[min_neighbour].utility > FLT_MAX/2 ||
fabs(node_list[min_neighbour].utility -
(node_list[i].utility + edges[min_edge].cost)) < 1e6);
if (node_list[min_neighbour].utility > FLT_MAX/2) {
carmen_roadmap_edge_t *edges2;
edges2 = (carmen_roadmap_edge_t *)node_list[min_neighbour].edges->list;
node_list[min_neighbour].utility = node_list[i].utility +
edges[min_edge].cost;
best_neighbour = -1;
best_utility = FLT_MAX;
for (j = 0; j < node_list[min_neighbour].edges->length; j++) {
if (edges2[j].cost > 1e5 || edges2[j].blocked)
continue;
if (node_list[edges2[j].id].utility > FLT_MAX/2)
continue;
utility = node_list[edges2[j].id].utility;
if (utility < best_utility) {
best_utility = utility;
best_neighbour = edges2[j].id;
}
}
if (best_neighbour < 0)
continue;
assert (best_utility < FLT_MAX/2);
assert (node_list[best_neighbour].utility <
node_list[min_neighbour].utility);
assert(node_list[i].utility < FLT_MAX/2);
assert(fwd_utilities[i] < FLT_MAX/2);
assert(fwd_utilities[min_neighbour] < FLT_MAX/2);
assert(node_list[min_neighbour].utility < 1e5);
}
assert (node_list[i].utility < node_list[min_neighbour].utility);
// carmen_warn("%d %d %f %f\n", node_list[i].x, node_list[i].y, node_list[i].utility, FLT_MAX/2);
i = min_neighbour;
assert(node_list[i].utility < FLT_MAX/2);
}
}
/* Check to make sure that no node is a local minimum. The only node that's
allowed to be a local minimum is the goal.
*/
for (i = 0; i < roadmap->nodes->length; i++) {
if (i == roadmap->goal_id)
continue;
edges = (carmen_roadmap_edge_t *)(node_list[i].edges->list);
if (node_list[i].utility > FLT_MAX/2)
continue;
best_neighbour = -1;
best_utility = FLT_MAX;
for (j = 0; j < node_list[i].edges->length; j++) {
if (edges[j].cost > 1e5 || edges[j].blocked)
continue;
if (node_list[edges[j].id].utility > FLT_MAX/2)
continue;
utility = node_list[edges[j].id].utility;
if (utility < best_utility) {
best_utility = utility;
best_neighbour = edges[j].id;
}
}
if (best_neighbour < 0)
continue;
assert (best_utility < FLT_MAX/2);
assert (node_list[best_neighbour].utility < node_list[i].utility);
}
free(fwd_utilities);
delete_queue(&the_state_queue);
}
