struct copy_graph_node * permute_copies(
struct copy_graph_node **copy_graph_head, struct add_list * add_list)
{
int count;
struct copy_graph_node * topo_list_head;
struct copy_graph_node ** node_array;
if(do_stats) {
timing(TIMING_START);
}
node_array = build_edges(*copy_graph_head,&count);
if(do_stats) {
rprintf(FINFO, "Build graph: %s\n", timing(TIMING_END));
}
if(do_stats) {
timing(TIMING_START);
}
topo_list_head = topo_sort(node_array,count, add_list);
if(do_stats) {
rprintf(FINFO, "Topological sort: %s\n", timing(TIMING_END));
}
update_extra_memory(-(count * sizeof(struct copy_graph_node * )));
if(do_stats) {
rprintf(FINFO, "Extra memory: %d Extra bytes sent: %d\n",
max_extra_memory,extra_bytes);
rprintf(FINFO, "Broken copies without delta comp: %d\n",
broken_copies);
rprintf(FINFO, "Cycles broken: %d Bytes trimmed: %d\n",
cycles_broken, bytes_trimmed);
}
free(node_array);
return topo_list_head;
}
void algorithms::KDDecomposer<IROBOT>::AdaptiveDecompose(double larg_radius, double min_radius)
{
this->ShallowDecompose(larg_radius);
algorithms::Analyzer<IROBOT> analyzer( *this );
//importance = analyzer.build_simple_weighted_centrality_matrix(M_PI/8, M_PI/64);
std::vector<double> importance = analyzer.build_path_importance_matrix(larg_radius * 7, larg_radius);
// node_index --> importance
std::unordered_map<int, double> impt_map;
std::stack<int> stack;
for( int i = 0; i < this->cells.size(); i++ )
{
double cell_radius = get_cell(i).radius();
if(cell_radius > larg_radius*2)
continue;
stack.push(get_cell(i).node_id);
impt_map[get_cell(i).node_id] = importance[i];
}
this->cells.clear();
this->DecomposeSubspaces( stack, larg_radius, min_radius, impt_map );
this->cells.reserve(nodes.size());
nodes.shrink_to_fit();
cells.shrink_to_fit();
clean_tree();
build_edges();
}
void algorithms::KDDecomposer<IROBOT>::DecomposeSpace() {
std::clock_t t0 = std::clock();
const std::array<robotics::Range, DIM>& ranges = robot_.get_config_ranges();
ND::vec<DIM> center;
for (int i = 0; i < DIM; i++) {
center[i] = (std::get<0>(ranges[i]) + std::get<1>(ranges[i])) * 0.5;
}
radius_array[0] = (std::get<1>(ranges[0]) - std::get<0>(ranges[0])) * 0.5;
for (int i = 1; i < sizeof(radius_array) / sizeof(radius_array[0]); ++i)
radius_array[i] = radius_array[i-1] * 0.5;
// NOTE: Assumes hypercube config space!! (Commented code more general)
nodes.reserve(360000000);
cells.reserve(3000000);
root_index = get_new_node(center, 0);
std::stack<int> stack;
stack.emplace(root_index);
while (!stack.empty())
{
int node_index = stack.top();
stack.pop();
robot_.set_config(get_node(node_index).center());
double dist_to_obsts = obstacle_manager_.dist_to_obsts(robot_);
if (dist_to_obsts > 0)
{
// Create free space ball
double initial_guess = CalcFreeCellRadius(dist_to_obsts);
double radius = robot_.free_space_oracle(obstacle_manager_, initial_guess /* lower bound */, 2 * initial_guess /* upper bound */);
if (robot_.subconvex_radius(obstacle_manager_, radius, radius_array[get_node(node_index).depth()]) )
{
get_node(node_index).set_covered();
get_node(node_index).set_free();
if (!MERGE_CELLS)
get_node(node_index).set_cell(get_new_cell(get_node(node_index).center(), radius_array[get_node(node_index).depth()], node_index));
}
else if (dist_to_obsts >= epsilon_ * 0.5 || radius_array[get_node(node_index).depth()] >= robot_.get_s_small(epsilon_ * 0.5) )
{
SplitCell(node_index);
for (int i = 0; i < NUM_SPLITS; ++i)
stack.emplace(get_node(node_index).get_children() + i);
}
}
else if (obstacle_manager_.penetration(robot_) / robot_.get_max_speed() >= (PENETRATION_CONSTANT / min_param_speed_) * radius_array[get_node(node_index).depth()] )
{
continue;
}
else if (radius_array[get_node(node_index).depth()] >= robot_.get_s_small(epsilon_ * 0.5))
{
SplitCell(node_index);
for (int i = 0; i < NUM_SPLITS; ++i)
stack.emplace(get_node(node_index).get_children() + i);
}
}
nodes.shrink_to_fit();
cells.shrink_to_fit();
clean_tree();
build_edges();
}
void load_graph(const char *filename, graph *graph) {
// open the file
std::ifstream graph_file;
graph_file.open(filename);
get_meta_data(graph_file, graph);
int* scratch = (int*) malloc(sizeof(int) * (graph->num_nodes + graph->num_edges));
read_graph_file(graph_file, scratch);
build_start(graph, scratch);
build_edges(graph, scratch);
free(scratch);
}
void algorithms::KDDecomposer<IROBOT>::ShallowDecompose( double min_radius )
{
const std::array<robotics::Range, DIM>& ranges = robot_.get_config_ranges();
ND::vec<DIM> center;
for (int i = 0; i < DIM; i++) {
center[i] = (std::get<0>(ranges[i]) + std::get<1>(ranges[i])) * 0.5;
}
radius_array[0] = (std::get<1>(ranges[0]) - std::get<0>(ranges[0])) * 0.5;
for (int i = 1; i < sizeof(radius_array) / sizeof(radius_array[0]); ++i)
radius_array[i] = radius_array[i-1] * 0.5;
// NOTE: Assumes hypercube config space!! (Commented code more general)
nodes.reserve(360000000);
cells.reserve(3000000);
root_index = get_new_node(center, 0);
std::stack<int> stack;
stack.emplace(root_index);
while (!stack.empty())
{
int node_index = stack.top();
stack.pop();
double cell_radius = radius_array[get_node(node_index).depth()];
robot_.set_config(get_node(node_index).center());
double dist_to_obsts = obstacle_manager_.dist_to_obsts(robot_);
if (dist_to_obsts > 0)
{
// Create free space ball
double initial_guess = CalcFreeCellRadius(dist_to_obsts);
double radius = robot_.free_space_oracle(obstacle_manager_, initial_guess /* lower bound */, 2 * initial_guess /* upper bound */);
if (robot_.subconvex_radius(obstacle_manager_, radius, cell_radius) )
{
get_node(node_index).set_covered();
get_node(node_index).set_free();
if (!MERGE_CELLS)
get_node(node_index).set_cell(get_new_cell(get_node(node_index).center(), cell_radius, node_index));
}
else if ( cell_radius > min_radius )
{
SplitCell(node_index);
for (int i = 0; i < NUM_SPLITS; ++i)
stack.emplace(get_node(node_index).get_children() + i);
}
else
{
get_node(node_index).set_covered();
//get_node(node_index).set_mix();
get_node(node_index).set_free();
}
}
else
{
const double penetration = obstacle_manager_.penetration(robot_);
if ( penetration >= cell_radius )
{
continue;
}
/*
else if (cell_radius > penetration )
{
get_node(node_index).set_covered();
get_node(node_index).set_free();
}*/
else if (cell_radius > min_radius )
{
SplitCell(node_index);
for (int i = 0; i < NUM_SPLITS; ++i)
stack.emplace(get_node(node_index).get_children() + i);
}
}
}
nodes.shrink_to_fit();
cells.shrink_to_fit();
clean_tree();
build_edges(false);
}
