Example #1
0
static inline double get_parent_value(queue the_queue, int index)
{
  int parent_index;

  parent_index = carmen_trunc(index/2)-1;
  return the_queue->data_array[parent_index]->utility;
}
Example #2
0
/* incrememnts the size of the brush */
gint 
brush_incr(void)
{
  char brush_text[10];
  brush_size ++;
  
  sprintf(brush_text, "%d", carmen_trunc(2.0 * brush_size));
  gtk_label_set_text(GTK_LABEL(brush_label), brush_text);
  return 1;
}
Example #3
0
/* increments the line size */
gint 
line_incr(void)
{
  char line_text[10];
  line_size ++;
  
  sprintf(line_text, "%d", carmen_trunc(line_size));
  gtk_label_set_text(GTK_LABEL(line_label), line_text);
  return 1;
}
Example #4
0
static carmen_inline double
get_parent_value(queue the_queue, int index)
{
  int parent_index;
  int x, y;

  parent_index = carmen_trunc(index/2)-1;
  x = the_queue->data_array[parent_index]->x;
  y = the_queue->data_array[parent_index]->y;

  return *utility_value(x, y);
}
Example #5
0
/* decrememnts the size of the brush */
gint 
brush_decr(void)
{
  char brush_text[10];
  brush_size --;
  if(brush_size < .5)
    brush_size = .5;
  
  sprintf(brush_text, "%d", carmen_trunc(2.0 * brush_size));
  gtk_label_set_text(GTK_LABEL(brush_label), brush_text);
  return 1;
}
Example #6
0
static inline void insert_into_queue(state_ptr new_state, queue the_queue) 
{
  int index;

  if (!the_queue->queue_size || 
      the_queue->queue_size == the_queue->num_elements) 
    resize_queue(the_queue);

  the_queue->data_array[the_queue->num_elements] = new_state;
  the_queue->num_elements++;

  /* Fix up priority queue */

  index = the_queue->num_elements;
  
  while (index > 1 && get_parent_value(the_queue, index) > new_state->utility) 
    {
      swap_entries(carmen_trunc(index/2), index, the_queue);
      index = carmen_trunc(index/2);
    }
}
Example #7
0
static int
in_map(double x, double y, carmen_map_p map)
{
  int map_x, map_y;
  double occupancy;

  map_x = carmen_trunc(x / map->config.resolution);
  map_y = carmen_trunc(y / map->config.resolution);

  if (map_x < 0 || map_x > map->config.x_size ||
      map_y < 0 || map_y > map->config.y_size) {
    return 0;
  }

  occupancy = map->map[map_x][map_y];
 
  if (occupancy > 0.15 || occupancy < 0) 
    return 0;

  return 1;
}
Example #8
0
static inline void lower_cost(int id, int parent_id, double cost, 
			      double new_utility, queue state_queue)
{
  int i;

  for (i = 0; i < state_queue->num_elements; i++) {
    if (state_queue->data_array[i]->id == id) {
      state_queue->data_array[i]->parent_id = parent_id;
      state_queue->data_array[i]->cost = cost;
      state_queue->data_array[i]->utility = new_utility;      
    }
  }

  /* Fix up priority queue */

  i = state_queue->num_elements;
  
  while (i > 1 && get_parent_value(state_queue, i) > new_utility) {
    swap_entries(carmen_trunc(i/2), i, state_queue);
    i = carmen_trunc(i/2);
  }
}
Example #9
0
void
carmen_map_util_change_resolution(carmen_map_p map, double new_resolution)
{
  double *new_complete_map;
  double **new_map;
  carmen_map_config_t new_config;

  double **padded_map;
  double *padded_backing;

  int padded_x_size, padded_y_size;
  double scale_factor;

  int x, y;
  double f_x, f_y, a, b, tmp, r1, r2;
  int i_x, i_y, m, n;

  new_config.x_size = map->config.resolution/new_resolution * 
    map->config.x_size;
  new_config.y_size = map->config.resolution/new_resolution * 
    map->config.y_size;
  new_config.resolution = new_resolution;
  new_config.map_name = map->config.map_name;

  new_complete_map = (double *)
    calloc(new_config.x_size*new_config.y_size, sizeof(double));
  carmen_test_alloc(new_complete_map);

  new_map = (double **)calloc(new_config.x_size, sizeof(double *));
  carmen_test_alloc(new_map);
  for (x = 0; x < new_config.x_size; x++)
    new_map[x] = new_complete_map+x*new_config.y_size;

  padded_x_size = map->config.x_size+4;
  padded_y_size = map->config.y_size+4;

  padded_backing = (double *)calloc(padded_x_size*padded_y_size, sizeof(double));
  carmen_test_alloc(padded_backing);

  padded_map = (double **)calloc(padded_x_size, sizeof(double *));
  carmen_test_alloc(padded_map);
  for (x = 0; x < padded_x_size; x++)
    padded_map[x] = padded_backing+x*padded_y_size;

  for (x = 2; x < padded_x_size-2; x++) {
    memcpy(padded_map[x]+2, map->map[x-2], map->config.y_size*sizeof(double));
    memcpy(padded_map[x], padded_map[x]+2, 2*sizeof(double));
    memcpy(padded_map[x]+2+map->config.y_size, 
	   padded_map[x]+map->config.y_size, 
	   2*sizeof(double));
  }

  memcpy(padded_map[0], padded_map[2], padded_y_size*sizeof(double));
  memcpy(padded_map[1], padded_map[2], padded_y_size*sizeof(double));

  memcpy(padded_map[padded_x_size-2], padded_map[padded_x_size-3], 
	 padded_y_size*sizeof(double));
  memcpy(padded_map[padded_x_size-1], padded_map[padded_x_size-3], 
	 padded_y_size*sizeof(double));

  scale_factor = map->config.resolution / new_resolution;

  for (y = 0; y < new_config.y_size; y++) {
    f_y = y / scale_factor;
    i_y = carmen_trunc(f_y);
    a   = f_y - carmen_trunc(f_y);		
    for (x = 0; x < new_config.x_size; x++) {
      f_x = x / scale_factor;
      i_x = carmen_trunc(f_x);
      b   = f_x - carmen_trunc(f_x);
        
      // Implement EQ 14.5-3 here
      tmp = 0.0;
      for(m = -1; m < 3; m++) {
	r1 = cubic_bspline((double) m - a);
          
	for(n = -1; n < 3; n++) {
	  r2 = cubic_bspline(-1.0*((double)n - b));
	  tmp += padded_map[i_x+n+2][i_y+m+2] * r1 * r2;
	}
      }

      if (tmp < 0)
	tmp = -1;
      if (tmp > 1.0)
	tmp = 1.0;
      new_map[x][y] = tmp;
    }		
  }

  free(padded_map);
  free(padded_backing);

  free(map->map);
  free(map->complete_map);

  map->map = new_map;
  map->complete_map = new_complete_map;
  map->config = new_config;
}