示例#1
0
文件: Navigate.c 项目: omtinez/Robot
// given an obstacle, calculate adecuate speed based on how close we are to any of the edges
float Navigate_find_optimal_speed(Polygon* obstacle) {
	printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_speed start: %lu\n",
			timestamp(), CURRENT_MEMORY_USAGE);
	float goal_speed = 0;

	Point* current_position = Point_new(
		Vector_get(ROBOT_STATUS, 0), Vector_get(ROBOT_STATUS, 1));
	float distance_min = Geometry_distance_between_point_and_polygon(obstacle, current_position);

	// when the distance to obstacle is reasonably big floor the gas pedal
	if (distance_min > 2 * DISTANCE_MIN) {
		goal_speed = 1;

	// when the distance is too small and we need to stop
	} else if (distance_min < DISTANCE_MIN) {
		goal_speed = SPEED_MIN;

	// when distance to obstacle requires a maneuver
	} else {
		goal_speed = ((distance_min * SPEED_MIN)
				/ (DISTANCE_MIN - DISTANCE_CRITICAL)
				- (SPEED_MIN * DISTANCE_CRITICAL)
				/ (DISTANCE_MIN - DISTANCE_CRITICAL));
	}

	// cleanup and return
	Point_destroy(current_position);
	printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_speed end: %lu\n",
			timestamp(), CURRENT_MEMORY_USAGE);
	return goal_speed;
}
示例#2
0
文件: Navigate.c 项目: omtinez/Robot
// given a desired end angle, current robot status and an obstacle, return the closest possible angle
// to the desired one that also avoids having the obstacle in its trayectory. This function will break
// the problem down into cases and then call the corresponding functions for each case
float Navigate_find_optimal_angle(float goal_angle, Polygon* obstacle) {
	float optimal_angle = Vector_get(ROBOT_STATUS, 2);
	printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_angle start: %lu\n",
			timestamp(), CURRENT_MEMORY_USAGE);

	// find current position
	Point* current_position = Point_new(
			Vector_get(ROBOT_STATUS, 0), Vector_get(ROBOT_STATUS, 1));

	// find closest edge and distance to it
	Edge* closest_edge = Geometry_closest_polygon_edge_to_point(obstacle, current_position);
	float distance_min = Geometry_distance_between_point_and_edge(closest_edge, current_position);

	// case 1: when there are no obstacles nearby so we can turn to desired angle
	if (distance_min >= 2 * DISTANCE_MIN) {
		printf("%s[NAVIGATION] Navigation strategy case 1: No obstacles in sight\n", timestamp());
		optimal_angle = Navigate_find_optimal_angle_case_1(
				goal_angle, obstacle, current_position, closest_edge);

	// case 2: when there is an obstacle nearby so we need to keep our distance
	} else if (distance_min > DISTANCE_MIN && distance_min < 2 * DISTANCE_MIN) {
		printf("%s[NAVIGATION] Navigation strategy case 2: Obstacles nearby\n", timestamp());
		optimal_angle = Navigate_find_optimal_angle_case_2(
				goal_angle, obstacle, current_position, closest_edge);

	// case 3: when there is an obstacle too close so we need to move away
	} else if (distance_min <= DISTANCE_MIN) {
		printf("%s[NAVIGATION][WARNING] Navigation strategy case 3: Obstacles too close\n", timestamp());
		optimal_angle = Navigate_find_optimal_angle_case_3(
				Vector_get(ROBOT_STATUS, 2), obstacle, current_position, closest_edge);
	}

	// make sure the chosen angle passes the min turn and the max turn check
//	float delta_angle = fabs(Vector_get(SIMULATION_ROBOT_STATUS, 2) - optimal_angle);
//	if (delta_angle > SERVO_MAX_TURN) {
//		optimal_angle =
//			Vector_get(SIMULATION_ROBOT_STATUS, 2) + (delta_angle < 0 ? -SERVO_MAX_TURN : SERVO_MAX_TURN);
//	}

	// cleanup
	Point_destroy(current_position);
	
	// output and return
	if (optimal_angle == FLT_MAX) {
		printf("%s[NAVIGATION][ERROR] No optimal angle found\n", timestamp());
		// TODO: dump info here
	} else {
		printf("%s[NAVIGATION] Distance to closest obstacle: %f, optimal angle: %f\n",
				timestamp(), distance_min, optimal_angle);
	}

	printf("%s[MEMORY][DEBUG] Memory at Navigate_find_optimal_angle end: %lu\n",
			timestamp(), CURRENT_MEMORY_USAGE);
	return optimal_angle;
}
示例#3
0
/*
 * display all the entrants based on status/duration
 */
void display_results(Event* event) {
  Entrant* entrant;
  int i = 0;

  entrants_sort(event);
  /* since the entrants are now sorted, we know that all entrants with a particular
   * status will be grouped together */

  /* display finished */
  if (count_by_status(event, FINISHED) > 0) {
    printf("\n\tFinished:\n");
    for (; i < Vector_size(event->entrants); i++) {
      Vector_get(event->entrants, i, &entrant);
      if (entrant->status != FINISHED) break; /* if entrant hasn't finished, skip to
                                                 the next block of entrants */
      printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
      printf("\t\t\tCourse: %c Total time: %3d mins\n", entrant->course->id, entrant->duration);
    }
  }
  /* display started and stopped */
  if (count_by_status(event, STARTED) + count_by_status(event, STOPPED) > 0) {
    printf("\n\tRunning:\n");
    for (; i < Vector_size(event->entrants); i++) {
      Vector_get(event->entrants, i, &entrant);
      if (entrant->status != STARTED && entrant->status != STOPPED) break;

      printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
      printf("\t\t\tCourse: %c Track: %2d Run time: %3d mins\n", entrant->course->id,
          entrant->curr_track->id, entrant->duration);
    }
  }
  /* display disqualified */
  if (count_by_status(event, DISQUAL_SAFETY) + count_by_status(event, DISQUAL_INCORR) > 0) {
    printf("\n\tDisqualified:\n");
    for (; i < Vector_size(event->entrants); i++) {
      Vector_get(event->entrants, i, &entrant);
      if (entrant->status != DISQUAL_SAFETY && entrant->status != DISQUAL_INCORR) break;

      printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
      printf("\t\t\tCourse: %c ", entrant->course->id);
      if (entrant->status == DISQUAL_SAFETY) printf("Disqualified for safety\n");
      else printf("Disqualified for incorrect route\n");
    }
  }
  /* display waiting to start */
  if (count_by_status(event, NOT_STARTED) > 0) {
    printf("\n\tWaiting to start:\n");
    for (; i < Vector_size(event->entrants); i++) {
      Vector_get(event->entrants, i, &entrant);

      printf("\t\t%3d: %-50s\n", entrant->id, entrant->name);
      printf("\t\t\tCourse: %c\n", entrant->course->id);
    }
  }
}
示例#4
0
static HandlerResult ColorsPanel_eventHandler(Panel* super, int ch) {
   ColorsPanel* this = (ColorsPanel*) super;
   
   HandlerResult result = IGNORED;
   int mark = Panel_getSelectedIndex(super);

   switch(ch) {
   case 0x0a:
   case 0x0d:
   case KEY_ENTER:
   case KEY_MOUSE:
   case KEY_RECLICK:
   case ' ':
      for (int i = 0; ColorSchemeNames[i] != NULL; i++)
         CheckItem_set((CheckItem*)Panel_get(super, i), false);
      CheckItem_set((CheckItem*)Panel_get(super, mark), true);
      this->settings->colorScheme = mark;
      result = HANDLED;
   }

   if (result == HANDLED) {
      this->settings->changed = true;
      const Header* header = this->scr->header;
      CRT_setColors(mark);
      clear();
      Panel* menu = (Panel*) Vector_get(this->scr->panels, 0);
      Header_draw(header);
      RichString_setAttr(&(super->header), CRT_colors[PANEL_HEADER_FOCUS]);
      RichString_setAttr(&(menu->header), CRT_colors[PANEL_HEADER_UNFOCUS]);
      ScreenManager_resize(this->scr, this->scr->x1, header->height, this->scr->x2, this->scr->y2);
   }
   return result;
}
示例#5
0
文件: map.c 项目: shurizzle/beard
void
test_map_pairs (void* data)
{
	Map* map = Map_new(runtime);

	Map_put(map, hash_for(NIL), NIL, TRUE);
	Map_put(map, hash_for(TRUE), TRUE, FALSE);
	Map_put(map, hash_for(FALSE), FALSE, NIL);

	Vector* pairs = Map_pairs(map);

	for (uint64_t i = 0; i < Vector_length(pairs); i++) {
		Tuple* pair = Vector_get(pairs, i);

		if (is_nil(Tuple_get(pair, 0))) {
			tt_assert(is_true(Tuple_get(pair, 1)));
		}
		else if (is_true(Tuple_get(pair, 0))) {
			tt_assert(is_false(Tuple_get(pair, 1)));
		}
		else if (is_false(Tuple_get(pair, 0))) {
			tt_assert(is_nil(Tuple_get(pair, 1)));
		}
	}

end:
	Map_destroy(map);
}
示例#6
0
static HandlerResult MetersPanel_eventHandler(Panel* super, int ch) {
   MetersPanel* this = (MetersPanel*) super;
   
   int selected = Panel_getSelectedIndex(super);
   HandlerResult result = IGNORED;

   switch(ch) {
      case 0x0a:
      case 0x0d:
      case KEY_ENTER:
      case KEY_F(4):
      case 't':
      {
         Meter* meter = (Meter*) Vector_get(this->meters, selected);
         int mode = meter->mode + 1;
         if (mode == LAST_METERMODE) mode = 1;
         Meter_setMode(meter, mode);
         Panel_set(super, selected, (Object*) Meter_toListItem(meter));
         result = HANDLED;
         break;
      }
      case KEY_F(7):
      case '[':
      case '-':
      {
         Vector_moveUp(this->meters, selected);
         Panel_moveSelectedUp(super);
         result = HANDLED;
         break;
      }
      case KEY_F(8):
      case ']':
      case '+':
      {
         Vector_moveDown(this->meters, selected);
         Panel_moveSelectedDown(super);
         result = HANDLED;
         break;
      }
      case KEY_F(9):
      case KEY_DC:
      {
         if (selected < Vector_size(this->meters)) {
            Vector_remove(this->meters, selected);
            Panel_remove(super, selected);
         }
         result = HANDLED;
         break;
      }
   }
   if (result == HANDLED) {
      Header* header = this->settings->header;
      this->settings->changed = true;
      Header_calculateHeight(header);
      Header_draw(header);
      ScreenManager_resize(this->scr, this->scr->x1, header->height, this->scr->x2, this->scr->y2);
   }
   return result;
}
示例#7
0
文件: Navigate.c 项目: omtinez/Robot
void Navigate_test_sensor_data_parse_1() {
	Polygon* obstacle = NULL;
	float distance = 200.0f, angle = 0, x, y;
	char test_name[100], label[20], i;

	// using binary for combinatorics with k = 2
	unsigned char sensor_switch;
	for (sensor_switch = 0; sensor_switch < 32; sensor_switch++) {
		sprintf(label, "[ %s, %s, %s, %s, %s ]",
			(sensor_switch & 0b0000001) ? "1" : "0",
			(sensor_switch & 0b0000010) ? "1" : "0",
			(sensor_switch & 0b0000100) ? "1" : "0",
			(sensor_switch & 0b0001000) ? "1" : "0",
			(sensor_switch & 0b0010000) ? "1" : "0");

		sprintf(test_name, "Navigate_test_sensor_data_parse_%s", label);
		Draw* TEST_CANVAS = Draw_new(test_name, NULL, WORLD_MAX_X, WORLD_MAX_Y);


		TEST_CANVAS->style = "stroke:red;fill:red;stroke-width:2";
		for (i = 0; i < SENSOR_ORIENTATION->n; i++) {
			if (sensor_switch & (1 << i)) {
				angle = Vector_get(SENSOR_ORIENTATION, i);
				x = distance * cosf((90 - angle) * M_PI / 180.0f);
				y = distance * sinf((90 - angle) * M_PI / 180.0f);
				Draw_line(TEST_CANVAS, 0, 0, x, y);
				Draw_circle(TEST_CANVAS, x, y , 3);
			}
		}

		int sensor_data[5] = {
			(sensor_switch & 0b0000001) ? distance : NAVIGATION_DISTANCE_SAFE,
			(sensor_switch & 0b0000010) ? distance : NAVIGATION_DISTANCE_SAFE,
			(sensor_switch & 0b0000100) ? distance : NAVIGATION_DISTANCE_SAFE,
			(sensor_switch & 0b0001000) ? distance : NAVIGATION_DISTANCE_SAFE,
			(sensor_switch & 0b0010000) ? distance : NAVIGATION_DISTANCE_SAFE };

		char sensor_data_raw[32];
		sprintf(sensor_data_raw, "[%d,%d,%d,%d,%d]",
			sensor_data[0],
			sensor_data[1],
			sensor_data[2],
			sensor_data[3],
			sensor_data[4]);

		obstacle = Navigate_parse_sensor_data(sensor_data_raw);

		if (obstacle != NULL) {
			TEST_CANVAS->style = "stroke:blue;fill:none;stroke-width:2";
			Polygon_draw(obstacle);
			Polygon_destroy(obstacle);
		}
		TEST_CANVAS->style = "stroke:black;fill:black;stroke-width:2";
		Draw_text(TEST_CANVAS, -125, 450, 40, label);
		Draw_destroy(TEST_CANVAS);

	}
}
示例#8
0
bool compile(AstNode *ast, FILE *outfp)
{
    if (ast == NULL || ast->type != AST_ROOT) {
        trigger_error(0, "Invalid AstNode found. Stop.\n");
    }

    SNodeArray *sna = SNodeArray_create();

    compile_declaration_list(ast->children[0], sna);
    compile_stmt_list(ast->children[1], sna);

    for (size_t i = 0; i < sna->size; i++) {
        SNode *node = (SNode *) Vector_get(sna, i);
        if (node->kind == SNODE_VAR) {
            fprintf(outfp, "load %d\n", node->sym->location);
        } else if (node->kind == SNODE_TMP) {

        } else if (node->kind == SNODE_LOCATION) {
            fprintf(outfp, "L%d:\n", node->location);
        } else if (node->kind == SNODE_CONST) {
            fprintf(outfp, "push %d\n", node->sval.ival);
        } else {
            switch (node->op) {
                case SNODE_ASSIGN:  fprintf(outfp, "store %d\n", node->result->sym->location); break;
                case SNODE_IADD:    fprintf(outfp, "add\n");    break;
                case SNODE_ISUB:    fprintf(outfp, "sub\n");    break;
                case SNODE_IMUL:    fprintf(outfp, "mul\n");    break;
                case SNODE_IDIV:    fprintf(outfp, "div\n");    break;
                case SNODE_IMOD:    fprintf(outfp, "mod\n");    break;
                case SNODE_IPRINT:  fprintf(outfp, "print\n");  break;
                case SNODE_AND:     fprintf(outfp, "and\n");    break;
                case SNODE_OR:      fprintf(outfp, "or\n");     break;
                case SNODE_LT:      fprintf(outfp, "cmplt\n");  break;
                case SNODE_GT:      fprintf(outfp, "cmpgt\n");  break;
                case SNODE_EQ:      fprintf(outfp, "cmpeq\n");  break;
                case SNODE_NOT:     fprintf(outfp, "not\n");    break;
                case SNODE_NOTEQ:   fprintf(outfp, "cmpeq\n"); fprintf(outfp, "not\n"); break;
                case SNODE_JMP_FALSE:
                    fprintf(outfp, "jmpz L%d\n", node->arg->location);
                    break;
                case SNODE_GOTO:
                    fprintf(outfp, "goto L%d\n", node->arg->location);
                    break;
                default:
                    break;
            }
        }
    }

    fprintf(outfp, "halt\n");

    SNodeArray_destroy(sna);

    return true;
}
示例#9
0
文件: stack.c 项目: smaeul/algorithms
int
Stack_pop(Stack s, void *value)
{
	int ret = Vector_get(&s->vector, Vector_length(&s->vector) - 1, value);

	/* Only truncate the vector (shrink by 1) if getting the value succeeded. */
	if (!ret)
		ret = Vector_truncate(&s->vector, -1);

	return ret;
}
示例#10
0
/*
 * returns how many entrants have a particular status
 */
int count_by_status(Event* event, entrant_status status) {
  Entrant* entrant;
  int ret_val = 0;
  int i = 0;

  for (i = 0; i < Vector_size(event->entrants); i++) {
    Vector_get(event->entrants, i, &entrant);
    if (entrant->status == status) ret_val++;
  }

  return ret_val;
}
示例#11
0
文件: Navigate.c 项目: omtinez/Robot
void Navigate_update_robot_status(int speed, int direction) {
	float adjusted_speed = speed * 0.3;

	// get coordinates of current position and next immediate position given current speed
	Point* p0 = Point_new(Vector_get(ROBOT_STATUS, 0), Vector_get(ROBOT_STATUS, 1));
	Point* p1 = Point_new(Vector_get(ROBOT_STATUS, 0)
		+ cosf((90 - Vector_get(ROBOT_STATUS, 2)) * M_PI / 180.0f) * adjusted_speed,
		Vector_get(ROBOT_STATUS, 1)
		+ sinf((90 - Vector_get(ROBOT_STATUS, 2)) * M_PI / 180.0f) * adjusted_speed);

	// get the coordinates of goal position with given goal speed and direction
	Point* p2 = Point_new(p1->x + cosf((90 - direction) * M_PI / 180.0f) * adjusted_speed,
		p1->y + sinf((90 - direction) * M_PI / 180.0f) * adjusted_speed);

	// update global robot status
	Vector_set_val(ROBOT_STATUS, 0, p2->x);
	Vector_set_val(ROBOT_STATUS, 1, p2->y);
	Vector_set_val(ROBOT_STATUS, 2, direction);
	Vector_set_val(ROBOT_STATUS, 3, speed);

	// TODO: draw it in canvas?

	// cleanup
	Point_destroy(p0);
	Point_destroy(p1);
	Point_destroy(p2);
}
示例#12
0
MetersPanel* MetersPanel_new(Settings* settings, const char* header, Vector* meters, ScreenManager* scr) {
   MetersPanel* this = AllocThis(MetersPanel);
   Panel* super = (Panel*) this;
   Panel_init(super, 1, 1, 1, 1, Class(ListItem), true);

   this->settings = settings;
   this->meters = meters;
   this->scr = scr;
   Panel_setHeader(super, header);
   for (int i = 0; i < Vector_size(meters); i++) {
      Meter* meter = (Meter*) Vector_get(meters, i);
      Panel_add(super, (Object*) Meter_toListItem(meter));
   }
   return this;
}
示例#13
0
/*
 * list entrants excluded for getting lost
 */
void list_excluded_incorrect(Event* event) {
  Entrant* entrant;
  int i = 0;

  /* make sure there's at least one */
  if (count_by_status(event, DISQUAL_INCORR) == 0) {
    printf("\tNo entrants disqualified for incorrect route\n");
  } else {
    for (i = 0; i < Vector_size(event->entrants); i++) {
      Vector_get(event->entrants, i, &entrant);
      if (entrant->status == DISQUAL_INCORR) {
        printf("\t%2d: %-50s\n", entrant->id, entrant->name);
        printf("\t\tCourse: %c Last node: %2d\n", entrant->course->id,
            entrant->last_cp_node->id);
      }
    }
  }
}
示例#14
0
文件: map.c 项目: shurizzle/beard
void
test_map_values (void* data)
{
	Map* map = Map_new(runtime);

	Map_put(map, hash_for(NIL), NIL, TRUE);
	Map_put(map, hash_for(TRUE), TRUE, FALSE);
	Map_put(map, hash_for(FALSE), FALSE, NIL);

	Vector* values = Map_values(map);

	for (uint64_t i = 0; i < Vector_length(values); i++) {
		Value* value = Vector_get(values, i);

		tt_assert(is_nil(value) || is_true(value) || is_false(value));
	}

end:
	Map_destroy(map);
}
示例#15
0
/*
 * update entrants from a file
 */
int refresh(Event* event, char* filename, int t_index) {
  Vector* lines = read_file(filename);
  char* line;
  char* token;
  char type;
  int node_id;
  int entrant_id;
  Time* time;
  int i;

  for (i = t_index; i < Vector_size(lines); i++) {
    Vector_get(lines, i, &line);

    /* type */
    token = strtok(line, " ");
    type = token[0];

    /* node id */
    token = strtok(NULL, " ");
    node_id = atoi(token);

    /* entrant id */
    token = strtok(NULL, " ");
    entrant_id = atoi(token);

    /* time */
    token = strtok(NULL, "\n");
    time = str_to_time(token);

    update_time(event, time, entrant_id);
    entrant_update_location(event, type, entrant_id, node_id);
  }

  Vector_dispose(lines);
	return i;
}
示例#16
0
// Test your vector here
int main() { 
	Vector* temp = Vector_create(); //create a vector
	
	Vector_append( temp, "a");//1
	Vector_append( temp, "b");//2
	Vector_append( temp, "c");//3
	Vector_append( temp, "d");//4
	Vector_append( temp, NULL);//5
	Vector_append( temp, "f");//6
	Vector_append( temp, "g");//7
	Vector_append( temp, NULL);//8
	Vector_append( temp, "");//9
	Vector_append( temp, "h");//10
	Vector_append( temp, "i");//11
	Vector_append( temp, NULL);//12
	
	Vector_resize( temp, 20 );
	
	if( Vector_size(temp) != 20 || strcmp( Vector_get( temp, 10 ), "i" ) || Vector_get( temp, 15 ) != NULL )
		perror( "something wrong.\n");
	
	//done for append, resize, get, size
	
	Vector_set( temp, 19, "caibi" );
	Vector_insert( temp, 20, "niubi" );
	Vector_insert( temp, 30, "wori" );
	
	if( Vector_size(temp) != 31 || strcmp( Vector_get( temp, 19 ), "caibi" ) || strcmp( Vector_get( temp, 20 ), "niubi" ) ||  Vector_get( temp, 15 ) != NULL || strcmp( Vector_get( temp, 30 ), "wori" ) )
		perror( "something wrong.\n");
	
	Vector_delete( temp, 11 );
	Vector_delete( temp, 27 );
	Vector_delete( temp, 1 );
	Vector_delete( temp, 18 );
	
	if( Vector_size(temp) != 27 || strcmp( Vector_get( temp, 4 ), "f" ) || strcmp( Vector_get( temp, 26 ), "wori") || Vector_get( temp, 18 ) !=NULL || strcmp( Vector_get( temp, 17 ), "caibi") )
		perror( "something wrong.\n");
	
	
	Vector_destroy( temp );

	return 0; 
}
示例#17
0
文件: Navigate.c 项目: omtinez/Robot
/**
 * This function takes the raw input from the sensors and constructs a polygon
 * by generating edges between each of the points known to be obstacles
 */
Polygon* Navigate_parse_sensor_data(char* raw_data) {
	printf("%s[MEMORY][DEBUG] Memory at Navigate_parse_sensor_data start: %lu\n",
			timestamp(), CURRENT_MEMORY_USAGE);

	// parse the string into an array of integers
	int sensor_data[5] = { 0, 0, 0, 0, 0 };
	sscanf(raw_data, "[%d,%d,%d,%d,%d]",
		&sensor_data[0], &sensor_data[1], &sensor_data[2], &sensor_data[3], &sensor_data[4]);

	// loop through each of the sensors to generate the edges between each point
	int i;
	float angle, angle_next;
	Edge *first_edge = NULL, *edge_tmp, *edge_prev = NULL;
	for (i = 0; i < 5; i++) {
		angle = Vector_get(ROBOT_STATUS, 2) + Vector_get(SENSOR_ORIENTATION, i);
		angle_next = Vector_get(ROBOT_STATUS, 2) + Vector_get(SENSOR_ORIENTATION, i + 1);

		// nothing in sight of this sensor and the next sensor
		if (i < 5 - 1
		&& sensor_data[i] >= NAVIGATION_DISTANCE_SAFE
		&& sensor_data[i + 1] >= NAVIGATION_DISTANCE_SAFE) continue;

		// this sensor picked something up but the ones around it didn't: draw a square around it
		if ((i > 0 && i < 5 - 1
			&& sensor_data[i] < NAVIGATION_DISTANCE_SAFE
			&& sensor_data[i + 1] >= NAVIGATION_DISTANCE_SAFE
			&& sensor_data[i - 1] >= NAVIGATION_DISTANCE_SAFE)
		// special case 1: first sensor
		|| (i == 0
			&& sensor_data[0] < NAVIGATION_DISTANCE_SAFE
			&& sensor_data[1] >= NAVIGATION_DISTANCE_SAFE)
		// special case 2: last sensor
		|| (i == 5 - 1
			&& sensor_data[5 - 1] < NAVIGATION_DISTANCE_SAFE
			&& sensor_data[5 - 2] >= NAVIGATION_DISTANCE_SAFE)){

			// get coordinates of obstacle
			float x = Vector_get(ROBOT_STATUS, 0) +
					cosf((90 - angle) * M_PI / 180.0f) * sensor_data[i];
			float y = Vector_get(ROBOT_STATUS, 1) +
					sinf((90 - angle) * M_PI / 180.0f) * sensor_data[i];

			// create a square around it
			Vertice* v1 = Vertice_new(x - OBSTACLE_MIN_SIZE / 2, y - OBSTACLE_MIN_SIZE / 2);
			Vertice* v2 = Vertice_new(x + OBSTACLE_MIN_SIZE / 2, y - OBSTACLE_MIN_SIZE / 2);
			Vertice* v3 = Vertice_new(x + OBSTACLE_MIN_SIZE / 2, y + OBSTACLE_MIN_SIZE / 2);
			Vertice* v4 = Vertice_new(x - OBSTACLE_MIN_SIZE / 2, y + OBSTACLE_MIN_SIZE / 2);

			// join the edges of the square
			edge_tmp = Edge_new_given_vertices(v1, v2);
			edge_tmp->next = Edge_new_given_vertices(v2, v3);
			edge_tmp->next->next = Edge_new_given_vertices(v3, v4);
			edge_tmp->next->next->next = Edge_new_given_vertices(v4, v1);

			// cleanup unused vertices
			Vertice_destroy(v1);
			Vertice_destroy(v2);
			Vertice_destroy(v3);
			Vertice_destroy(v4);

			// prepare next step in loop
			if (first_edge == NULL) first_edge = edge_tmp;
			else edge_prev->next = edge_tmp; // link edges to one another
			edge_prev = edge_tmp->next->next->next;

		// this sensor and the next one picked something up
		} else if (i < 5 - 1 && sensor_data[i] < NAVIGATION_DISTANCE_SAFE
		&& sensor_data[i + 1] < NAVIGATION_DISTANCE_SAFE) {
			edge_tmp = Edge_new_given_coordinates(
				Vector_get(ROBOT_STATUS, 0) +
					cosf((90 - angle) * M_PI / 180.0f) * sensor_data[i],
				Vector_get(ROBOT_STATUS, 1) +
					sinf((90 - angle) * M_PI / 180.0f) * sensor_data[i],
				Vector_get(ROBOT_STATUS, 0) +
					cosf((90 - angle_next) * M_PI / 180.0f) * sensor_data[i + 1],
				Vector_get(ROBOT_STATUS, 1) +
					sinf((90 - angle_next) * M_PI / 180.0f) * sensor_data[i + 1]);

			// prepare next step in loop
			if (first_edge == NULL) first_edge = edge_tmp;
			else edge_prev->next = edge_tmp; // link edges to one another
			edge_prev = edge_tmp;
		}
	}

	printf("%s[MEMORY][DEBUG] Memory at Navigate_parse_sensor_data end: %lu\n",
			timestamp(), CURRENT_MEMORY_USAGE);

	Polygon* res = Polygon_new_given_first_edge(first_edge);
	return res;
}
示例#18
0
static inline void addLine(const char* line, Vector* lines, Panel* panel, const char* incFilter) {
     Vector_add(lines, (Object*) ListItem_new(line, 0));
     if (!incFilter || String_contains_i(line, incFilter))
          Panel_add(panel, (Object*)Vector_get(lines, Vector_size(lines)-1));
}
示例#19
0
文件: stack.c 项目: smaeul/algorithms
int
Stack_peek(Stack s, void *value)
{
	return Vector_get(&s->vector, Vector_length(&s->vector) - 1, value);
}