void FullSpiralStc::scan(CellPtr current) { std::cout << "\033[1;34mcurrent-\033[0m\033[1;32mBEGIN:\033[0m " << current->get_center()->x << "," << current->get_center()->y << "\n"; VectorPtr direction = (current->get_parent()->get_center() - current->get_center()) / 2 / tool_size; VectorPtr initial_direction = direction++; // Check current cell has diagonally opposite obstacles or not PartiallyOccupiableCellPtr c = boost::static_pointer_cast<PartiallyOccupiableCell>(current); Quadrant q = c->get_current_quadrant(); Orientation orientation; if (q == I) orientation = AT_LEFT_SIDE; else if (q == II) orientation = IN_BACK; else if (q == III) orientation = AT_RIGHT_SIDE; else if (q == IV) orientation = IN_FRONT; c->set_quadrants_state(+q, see_obstacle(~orientation, tool_size) ? OBSTACLE : NEW); c->set_quadrants_state(-q, see_obstacle(~++orientation, tool_size) ? OBSTACLE : NEW); // While current cell has a new obstacle-free neighboring cell bool is_starting_cell = current == starting_cell; do { // Scan for new neighbor of current cell in counterclockwise order PartiallyOccupiableCellPtr neighbor = PartiallyOccupiableCellPtr( new PartiallyOccupiableCell(current->get_center() + direction * 2 * tool_size, 2 * tool_size)); std::cout << " \033[1;33mneighbor:\033[0m " << neighbor->get_center()->x << "," << neighbor->get_center()->y; // go_from(current, DONT_PASS, neighbor); // Full Scan-STC preparing if (should_go_to(neighbor, direction)) { // Go to free subcell of neighbor bool successful = go_from(current, PASS, neighbor); if (!successful) { // Obstacle } else { // New free neighbor // Construct a spanning-tree edge neighbor->set_parent(current); old_cells.insert(neighbor); scan(neighbor); } } else { // Go to next subcell go_from(current, DONT_PASS, neighbor); continue; } } while (direction++ % initial_direction != (is_starting_cell ? IN_FRONT : AT_RIGHT_SIDE)); // Back to subcell of parent if (!is_starting_cell) { go_from(current, PASS, current->get_parent()); } std::cout << "\033[1;34mcurrent-\033[0m\033[1;31mEND:\033[0m " << current->get_center()->x << "," << current->get_center()->y << "\n"; }
void MstcOnline::scan(CellPtr current) { std::string status; communicator->set_current_cell(current); // FIXME status = communicator->create_status_message(boost::static_pointer_cast<IdentifiableCell>(current)); communicator->write_status_message(status); communicator->read_message_then_update_old_cells(); std::cout << "\033[1;34mcurrent-\033[0m\033[1;32mBEGIN:\033[0m " << current->get_center()->x << "," << current->get_center()->y << "\n"; VectorPtr direction = (current->get_parent()->get_center() - current->get_center()) / 2 / tool_size; VectorPtr initial_direction = direction++; // While current cell has a new obstacle-free neighboring cell bool is_starting_cell = current == starting_cell; do { // Scan for new neighbor of current cell in counterclockwise order IdentifiableCellPtr neighbor = IdentifiableCellPtr(new IdentifiableCell( current->get_center() + direction * 2 * tool_size, 2 * tool_size, communicator->get_robot_name())); std::cout << " \033[1;33mneighbor:\033[0m " << neighbor->get_center()->x << "," << neighbor->get_center()->y; communicator->read_message_then_update_old_cells(); if (state_of(neighbor) == OLD) { // Check neighbor with current old cells // Go to next sub-cell if (communicator->ask_other_robot_still_alive(communicator->find_robot_name(neighbor))) { // Still alive communicator->set_current_cell(current); go_with(++direction, tool_size); continue; } else { // Dead if (state_of(neighbor) == OLD) { // Check again neighbor with new old cells communicator->set_current_cell(current); go_with(++direction, tool_size); continue; } else { std::cout << "\n"; neighbor->set_parent(current); communicator->read_message_then_update_old_cells(); communicator->insert_old_cell(neighbor); std::string message = communicator->create_old_cells_message(); communicator->write_old_cells_message(message); communicator->set_current_cell(current); go_with(direction++, tool_size); scan(neighbor); continue; } } } if (see_obstacle(direction, tool_size / 2)) { // Obstacle // Go to next sub-cell communicator->set_current_cell(current); go_with(++direction, tool_size); } else { // New free neighbor std::cout << "\n"; // Construct a spanning-tree edge neighbor->set_parent(current); communicator->read_message_then_update_old_cells(); communicator->insert_old_cell(neighbor); std::string message = communicator->create_old_cells_message(); communicator->write_old_cells_message(message); communicator->set_current_cell(current); go_with(direction++, tool_size); scan(neighbor); } } while (direction % initial_direction != (is_starting_cell ? AT_LEFT_SIDE : IN_FRONT)); // Back to sub-cell of parent if (!is_starting_cell) { communicator->set_current_cell(current); go_with(direction, tool_size); } std::cout << "\033[1;34mcurrent-\033[0m\033[1;31mEND:\033[0m " << current->get_center()->x << "," << current->get_center()->y << "\n"; }