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