/*
* === FUNCTION ======================================================================
* Name: alloc_malloc
* Description: search for adequate free node, call malloc&nalloc
* =====================================================================================
*/
void *alloc_malloc(long nbytes, const char *file, int line)
{
assert(nbytes > 0);
alloc_n *node;
/*-----------------------------------------------------------------------------
* nbytes is the multiple of ALIGN_SIZE && nbytes_new >= nbytes_old
*-----------------------------------------------------------------------------*/
nbytes = (nbytes + ALIGN_SIZE - 1) / ALIGN_SIZE * ALIGN_SIZE;
for (node = freelist.free; node; node = node->free) {
/*--------------------------------------------------------------------
* 2 tests, 1 is size, 1 is addr. 'cause it's a circle
*
* First-Fit Algorithm
*
* size > nbytes, then divide the whole into 2 parts:
* one is dirty, start at the original addr;
* the other is free, start at original addr + nbytes
* apply a new node for the other one
*--------------------------------------------------------------------*/
if (node->size > nbytes) {
/*------------------------------------------------------------
* old node, free&size
*------------------------------------------------------------*/
node->free = NULL;
node->size = nbytes;
/*------------------------------------------------------------
* new node, free&link
*------------------------------------------------------------*/
get_new_node((char *)node + nbytes, \
node->size - nbytes, file, line);
return (void *) node->addr; /* only 1 exit */
}
/*-----------------------------------------------------------------------------
* no more space fitted, wholesale with a node
*
* will find the right in the next loop
*-----------------------------------------------------------------------------*/
if (node == &freelist) { /* back to the circle start? */
void *addr = malloc(nbytes + WHOLESALE);
if (!addr) {
FAILURE();
}
get_new_node(addr, nbytes + WHOLESALE, file, line);
}
}
assert(0); /* can't arrive here */
return NULL;
}
Node *search_trie(Node *root, const char *val) {
/*
* Search the Trie for the char array.
* Create new nodes if part of the array does not exist yet.
* Return the node corresponding to the end of the char array.
*/
if (root == NULL ) {
log_warning("NULL root found in Trie search_trie.");
return NULL ;
}
if (val == NULL ) {
log_warning("Empty target string found in search_trie.");
return NULL ;
}
if (strlen(val) + (root->level) > MAX_TRIE_LEVEL) {
log_critical("Reached MAX_TRIE_LEVEL. Root level %d, Node %s",
root->level, val);
return NULL ;
}
Node *cur = root;
Node *child = NULL;
int i;
int found;
int cur_level = root->level;
int len = strlen(val);
char c;
for (i = 0; i < len; i++, cur_level++) {
child = cur->children;
c = val[i];
found = 0;
while (child != NULL ) {
if (child->val != c) {
child = child->sibling;
} else {
found = 1;
break;
}
}
if (found == 0) {
Node *n = get_new_node(c, cur_level + 1);
if (n == NULL ) {
return NULL ;
}
if (cur->children == NULL ) {
cur->children = n;
} else {
child = cur->children;
while (child->sibling != NULL ) {
child = child->sibling;
}
child->sibling = n;
}
cur = n;
} else {
cur = child;
}
}
return cur;
}
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();
}
bool
insert(node *root, int key, void *value)
{
if (key >= root->key) {
if (root->right == NULL) {
root->right = get_new_node(root, key, value);
return root->right? true: false;
} else
return insert(root->right, key, value);
} else {
if (root->left == NULL) {
root->left = get_new_node(root, key, value);
return root->left? true: false;
} else
return insert(root->left, key, value);
}
}
ir_graph *create_irg_copy(ir_graph *irg)
{
ir_graph *res = alloc_graph();
res->irg_pinned_state = irg->irg_pinned_state;
/* clone the frame type here for safety */
irp_reserve_resources(irp, IRP_RESOURCE_ENTITY_LINK);
res->frame_type = clone_frame_type(irg->frame_type);
ir_reserve_resources(irg, IR_RESOURCE_IRN_LINK);
/* copy all nodes from the graph irg to the new graph res */
irg_walk_anchors(irg, copy_all_nodes, rewire, res);
/* copy the Anchor node */
res->anchor = get_new_node(irg->anchor);
/* -- The end block -- */
set_irg_end_block (res, get_new_node(get_irg_end_block(irg)));
set_irg_end (res, get_new_node(get_irg_end(irg)));
/* -- The start block -- */
set_irg_start_block(res, get_new_node(get_irg_start_block(irg)));
set_irg_no_mem (res, get_new_node(get_irg_no_mem(irg)));
set_irg_start (res, get_new_node(get_irg_start(irg)));
/* Proj results of start node */
set_irg_initial_mem(res, get_new_node(get_irg_initial_mem(irg)));
ir_free_resources(irg, IR_RESOURCE_IRN_LINK);
irp_free_resources(irp, IRP_RESOURCE_ENTITY_LINK);
return res;
}
struct node* insert_at_beginning(struct node* head, int data)
{
struct node* new_node = get_new_node();
new_node->data = data;
new_node->link = head;
head = new_node;
//printf("\n%x\n", head);
return head;
}
CFGINSTANCEMAPNODE *get_node(int id, CFGINSTANCEMAP *map)
{
int i;
for (i = 0; i < map->n_node; ++i) {
if (map->node[i]->id == id)
return (map->node[i]);
}
return get_new_node(map);
}
node_type* clone(node_type* n) const
{
if (n == n->left) { // cannot test against null_node...
return null_node;
} else {
node_type* n = get_new_node(n->key, n->value, clone(n->left), clone(n->right));
return n;
}
}
node* insert(node* root, int data)
{
if (!root)
root = get_new_node(data);
else if (data < root->data)
root->left = insert(root->left, data);
else
root->right = insert(root->right, data);
return root;
}
void AddQ(node **head,int n)
{
node* start = *head;
if(start == NULL)
{
node* new_node = get_new_node(n);
*head = new_node;
(*head) -> left = *head;
(*head) -> right = *head;
return;
}
node *end = start -> left;
node* new_node = get_new_node(n);
end -> right = new_node;
new_node -> left = end;
new_node -> right = start;
start -> left = new_node;
}
bool insert(const K& k, const T& t)
{
if (is_null(root)) {
root = get_new_node(k, t);
} else {
splay(k, root);
if (comp(k, root->key)) {
node_type* n = get_new_node(k, t, root->left, root);
root->left = null_node;
root = n;
} else if (comp(root->key, k)) {
node_type* n = get_new_node(k, t, root, root->right);
root->right = null_node;
root = n;
} else {
root->value = t;
return false;
}
}
return true;
}
node* insert(node* root, int data)
{
// case 1, when root is empty
if (!root)
root = get_new_node(data);
// case 2, when data is less
else if (root->data > data)
root->left = insert(root->left, data);
// case 3, when data is greater than parent
else if (root->data < data)
root->right = insert(root->right, data);
return root;
}
void add_garbage(int x) {
//head is head node
struct Node* temp = head_garbage;
//pointer to new nodea
struct Node* newNode = get_new_node(x);
if(head_garbage == NULL) {
//point to node
head_garbage = newNode;
return;
}
//temp is the pointer that moves around
while(temp->next != NULL)
temp = temp->next; // go to last Node
temp->next = newNode; //adds new node to end of list
newNode->prev = temp; //links node back to list
}
void run_bank(BankConfig config) {
Node *store = get_new_node('$', ROOT_TRIE_LEVEL);
pthread_t sender_thread = 0;
int i = 0;
int downstream_up[config.destiny_host_count];
for (i = 0; i < config.destiny_host_count; i++) {
downstream_up[i] = 1;
}
void *arg[3];
arg[0] = (void*) store;
arg[1] = (void*) &config;
arg[3] = (void*) downstream_up;
pthread_create(&sender_thread, NULL, &run_sender, (void *) arg);
run_receiver(store, config);
}
bool insert_bst(node_pointer_t *root, int data) {
node_pointer_t tmp_node = NULL;
node_pointer_t new_node = NULL;
tmp_node = search_bst(*root, data);
if(tmp_node) {
fprintf(stderr, " ERROR: same data already exist\n");
return false;
}
new_node = get_new_node(data);
if(!new_node) {
fprintf(stderr, " ERROR: memory is full\n");
return false;
}
if(!*root) {
*root = new_node;
return true;
}
tmp_node = search_parent(*root, data);
if(tmp_node->data < data) tmp_node->rchild = new_node;
else if(tmp_node->data > data) tmp_node->lchild = new_node;
return true;
}
node *
init(int key, void *value)
{
return get_new_node(NULL, key, value);
}
int main (int argc, char *argv[])
{
char ch;
int choice;
int ret_val = 0;
struct node *head = NULL;
int *data = NULL;
int value = 0;
int pos = -1;
while (1) {
printf("\nMENU\n\n");
printf("1. Insert at the front\n");
printf("2. Insert at a position\n");
printf("3. Delete from the front\n");
printf("4. Delete from a position\n");
printf("5. Display\n");
printf("6. Reverse the linked list\n");
printf("7. Pairwise Swap\n");
printf("\nEnter your choice\n");
scanf("%d", &choice);
switch (choice)
{
case 1:
printf("\nEnter a value\n");
scanf(" %d", &value);
data = get_new_node(&value);
ret_val = insert_node(&head, (void *)data);
if (ret_val)
printf("\nOperation failed\n");
break;
case 2:
printf("\nEnter the position at which to insert\n");
scanf(" %d", &pos);
printf("\nEnter a value\n");
scanf(" %d", &value);
data = get_new_node(&value);
ret_val = insert_node_pos(&head, pos, (void *)data);
if (ret_val)
printf("\nOperation failed\n");
break;
case 3:
ret_val = delete_node(&head);
if (ret_val)
printf("\nOperation failed\n");
break;
case 4:
printf("\nEnter the position at which to delete\n");
scanf(" %d", &pos);
ret_val = delete_node_pos(&head, pos);
if (ret_val)
printf("\nOperation failed\n");
break;
case 5:
print_list(head);
break;
case 6:
head = reverse(head);
break;
case 7:
pairwise_swap(&head);
break;
default:
printf("\nWrong choice. Doing nothing\n");
}
printf("Do you want to continue (y/n) ??");
scanf(" %c", &ch);
if (ch == 'n' || ch == 'N') {
printf("\nBreaking the driver loop\n");
break;
}
}
return 0;
}
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);
}
