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";
}