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"; }
bool FullSpiralStc::go_from(CellPtr current, bool need_to_pass, CellPtr next) { VectorPtr direction = (next->get_center() - current->get_center()) / (2 * tool_size); PartiallyOccupiableCellPtr c = boost::static_pointer_cast<PartiallyOccupiableCell>(current); PartiallyOccupiableCellPtr n = boost::static_pointer_cast<PartiallyOccupiableCell>(next); Quadrant quadrant = c->get_current_quadrant(); Quadrant q; Quadrant q1; Quadrant q2; Quadrant q3; Quadrant q4; if (~direction == AT_RIGHT_SIDE) q = IV; else if (~direction == IN_FRONT) q = I; else if (~direction == AT_LEFT_SIDE) q = II; else if (~direction == IN_BACK) q = III; q1 = q; q2 = ++q; q3 = ++q; q4 = ++q; if (quadrant == q1 || quadrant == q2) { if (!see_obstacle(direction, tool_size)) { if (need_to_pass == DONT_PASS) return true; bool successful = visit(next, quadrant == q1 ? q4 : q3); std::cout << "\n"; return successful; } else { n->set_quadrants_state(q1 ? q4 : q3, OBSTACLE); if (!see_obstacle(quadrant == q1 ? ++direction : --direction, tool_size)) { visit(current, quadrant == q1 ? q2 : q1); if (!see_obstacle(quadrant == q1 ? --direction : ++direction, tool_size)) { if (need_to_pass == DONT_PASS) return true; bool successful = visit(next, quadrant == q1 ? q3 : q4); std::cout << "\n"; return successful; } else { n->set_quadrants_state(q1 ? q3 : q4, OBSTACLE); return false; } } else { c->set_quadrants_state(q1 ? q2 : q1, OBSTACLE); return false; } } } else if (quadrant == q4 || quadrant == q3) { if (!see_obstacle(direction, tool_size)) { visit(c, quadrant == q4 ? q1 : q2); return go_from(c, need_to_pass, next); } else { c->set_quadrants_state(quadrant == q4 ? q1 : q2, OBSTACLE); if (!see_obstacle(quadrant == q4 ? ++direction : --direction, tool_size)) { visit(c, quadrant == q4 ? q3 : q4); if (!see_obstacle(quadrant == q4 ? --direction : ++direction, tool_size)) { visit(c, quadrant == q4 ? q2 : q1); return go_from(c, need_to_pass, next); } else { c->set_quadrants_state(quadrant == q4 ? q2 : q1, OBSTACLE); return false; } } else { c->set_quadrants_state(quadrant == q4 ? q3 : q4, OBSTACLE); return false; } } } else return false; }