void algorithms::KDDecomposer<IROBOT>::build_edges(bool clear_nodes)
{
all_boundaries.clear();
std::vector<CONFIG> all_corners;
std::vector<const CONFIG*> corner_configs;
all_corners.reserve(1000000);
corner_configs.reserve(200000);
get_unique_crn_cfgs(corner_configs, all_corners);
std::clock_t t0 = std::clock();
all_boundaries.reserve(300000);
// Build all boundaries
std::vector<int> containing_cells;
for (const CONFIG* cfg : corner_configs) {
containing_cells.clear();
ContainedCells(cfg->vector(), containing_cells);
if(containing_cells.size() == 0)
continue;
for (int i = 0; i < containing_cells.size() - 1; ++i) {
for (int j = i + 1; j < containing_cells.size(); ++j) {
if (is_adjacent(containing_cells[i], containing_cells[j]))
continue;
int fix_dim = compute_fix_dim(containing_cells[i], containing_cells[j]);
// If the fixed dimension is not one, then it is degenerate case
if (fix_dim == -1)
continue;
int boundary_index = get_new_boundary(containing_cells[i], containing_cells[j]);
cells[containing_cells[i]].add_boundary(boundary_index);
cells[containing_cells[j]].add_boundary(boundary_index);
double weight = IROBOT::heuristic(robot_, CONFIG(cells[containing_cells[i]].center()), CONFIG(cells[containing_cells[j]].center()));
all_boundaries[boundary_index].set_weight(weight);
all_boundaries[boundary_index].set_fix_dim(fix_dim);
}
}
}
for (int i = 0; i < all_boundaries.size(); ++i) {
compute_bound(i);
}
std::clock_t t2 = std::clock();
all_boundaries.shrink_to_fit();
if(clear_nodes) nodes.clear();
nodes.shrink_to_fit();
for (Cell<IROBOT>& cell : cells)
cell.shrink_to_fit();
std::clock_t t3 = std::clock();
}
void canvas::get_bound(rect* bound)
{
compute_bound(bound);
}
