void clean_tree(struct avl_node *node, bool free_mapping) {
struct avl_node *left = node->left;
struct avl_node *right = node->right;
// only free the mappings if told to to avoid redundant freeing
if(free_mapping)
free(node->mapping);
free(node);
// recurse on each subtrees if existent
if(left != NULL)
clean_tree(left, free_mapping);
if(right != NULL)
clean_tree(right, free_mapping);
}
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();
}
t_tree *get_tree(t_cmd *cmd_data)
{
t_tree *cmd_tree;
if ((cmd_tree = init_cmd_tree(TYPE_CMD_SEP)) == NULL
|| cut_cmd(cmd_tree, cmd_data, TYPE_CMD_SEP) == -1)
return (NULL);
clean_tree(cmd_tree);
return (cmd_tree);
}
void proceed_to_execution(t_var *var)
{
var->list = lexer(var, var->lex);
var->list = create_tokens(var->list);
var->root = parser(var->list);
execute_tree(var, var->root);
clean_tree(var->root);
var->root = NULL;
var->list = NULL;
clean_line_pointers(var);
}
void clean_tree(t_tree *tree)
{
t_param *param;
param = tree->param->next;
while (param != tree->param)
{
if (param->tree)
{
clean_tree(param->tree);
free(param->cmd);
param->cmd = NULL;
}
param = param->next;
}
}
Analysis::~Analysis()
{
clean_tree();
}
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);
}