ivp_clustering_longrange.cxx

hl2 source code. Do not use it illegal.

    }
    // if none of the above is true, use highest row
    else {
	new_subnode->data.y = node->data.y*2;
    }

    // if new node lower than center, use lowest row
    if ( new_node->data.z >= ((node->data.z+1)<<rasterlevel_diff) ) {
	new_subnode->data.z = (node->data.z+1)*2;
    }
    // if new node lower than quarter, use middle row
    else if ( new_node->data.z >= (((node->data.z<<1)+1)<<(rasterlevel_diff-1)) ) {
	new_subnode->data.z = (node->data.z*2)+1;
    }
    // if none of the above is true, use highest row
    else {
	new_subnode->data.z = node->data.z*2;
    }

    new_subnode->data.rasterlevel = node->data.rasterlevel - 1;
    new_subnode->data.sizelevel   = node->data.sizelevel - 1;
    new_subnode->parent           = node;

    node->children.add(new_subnode);
    this->hash_table->add_node(new_subnode);

#ifdef MY_DEBUG
    if ( node->children.n_elems > 27 ) printf("*** ERROR *** Excessive amount of children: %d\n", node->children.n_elems);
#endif

#ifdef DEBUG_CONNECTBOXES
    DEBUG_IF {
	ivp_indent_output();
	printf("Created new subbox (%+.4f/%+.4f/%+.4f [%d: %+d/%+d/%+d] +++ %d: %f)\n", \
	       this->power2(new_subnode->data.rasterlevel)*new_subnode->data.x, \
	       this->power2(new_subnode->data.rasterlevel)*new_subnode->data.y, \
	       this->power2(new_subnode->data.rasterlevel)*new_subnode->data.z, \
	       new_subnode->data.rasterlevel, \
	       new_subnode->data.x, \
	       new_subnode->data.y, \
	       new_subnode->data.z, \
	       new_subnode->data.sizelevel, \
	       this->power2(new_subnode->data.sizelevel));
    }	
#endif
		    
    connect_boxes(new_subnode, new_node);

    return;
}


void IVP_OV_Tree_Manager::expand_tree(const IVP_OV_Node *new_node)
{
#ifdef DEBUG_EXPANDTREE
    DEBUG_IF {
	ivp_indent_output();
	printf("Expanding tree...\n");
    }
#endif
#ifdef DEBUG_ERROR_OUTPUT
    if ( this->root->data.sizelevel > 40 ) printf("*** ERROR *** Excessive sizelevel (%d) for element\n", this->root->data.sizelevel);
#endif

    IVP_ASSERT(this->root->data.sizelevel<41);
    
    IVP_OV_Node *old_root = this->root;

    IVP_OV_Node *new_root = new IVP_OV_Node();

    int rest_x = old_root->data.x%2;
    int rest_y = old_root->data.y%2;
    int rest_z = old_root->data.z%2;
#ifdef DEBUG_EXPANDTREE
    DEBUG_IF {
	ivp_indent_output();
	printf("Reste: %d %d %d\n", rest_x, rest_y, rest_z);
    }
#endif    
    new_root->data.x           = IVP_Inline_Math::int_div_2(old_root->data.x);
    new_root->data.y           = IVP_Inline_Math::int_div_2(old_root->data.y);
    new_root->data.z           = IVP_Inline_Math::int_div_2(old_root->data.z);
    new_root->data.rasterlevel = old_root->data.rasterlevel+1;
    new_root->data.sizelevel   = old_root->data.sizelevel+1;

#ifdef DEBUG_EXPANDTREE
    DEBUG_IF {
	ivp_indent_output();
	printf("Created new (larger) box...\n");
    }
#endif
    
    IVP_DOUBLE rasterpoints         = this->power2(new_root->data.rasterlevel);
    IVP_DOUBLE rasterpoints_newnode = this->power2(new_node->data.rasterlevel);
    
    if ( rest_x == -1 ) {
	new_root->data.x--;
    }
    else if ( rest_x == 0 ) {
	if ( (new_node->data.x*rasterpoints_newnode) < (rasterpoints * new_root->data.x) ) {
#ifdef DEBUG_EXPANDTREE
	    DEBUG_IF {
		ivp_indent_output();
		printf("Shifting box one rasterpoint to the left (x)...\n");
	    }
#endif
	    new_root->data.x--;
	}
    }
    if ( rest_y == -1 ) {
	new_root->data.y--;
    }
    else if ( rest_y == 0 ) {
	if ( (new_node->data.y*rasterpoints_newnode) < (rasterpoints * new_root->data.y) ) {
#ifdef DEBUG_EXPANDTREE
	    DEBUG_IF {
		ivp_indent_output();
		printf("Shifting box one rasterpoint back (y)...\n");
	    }
#endif
	    new_root->data.y--;
	}
    }
    if ( rest_z == -1 ) {
	new_root->data.z--;
    }
    else if ( rest_z == 0 ) {
	if ( (new_node->data.z*rasterpoints_newnode) < (rasterpoints * new_root->data.z) ) {
#ifdef DEBUG_EXPANDTREE
	    DEBUG_IF {
		ivp_indent_output();
		printf("Shifting box one rasterpoint down (z)...\n");
	    }
#endif
	    new_root->data.z--;
	}
    }

#ifdef DEBUG_EXPANDTREE
    DEBUG_IF {
	ivp_indent_output();
	printf("Connecting new (larger) box to existing tree...\n");
    }
#endif
    
#ifdef MY_DEBUG
    if ( new_root->children.n_elems >= 27 ) printf("*** ERROR *** : Mehr als 27 Kinder *********************************\n");
#endif			
    new_root->children.add(old_root);
    old_root->parent = new_root;
    this->hash_table->add_node(new_root);

    this->root = new_root;
    
#ifdef DEBUG_EXPANDTREE    
    DEBUG_IF {
	ivp_indent_output();
	printf("New root (%+.4f/%+.4f/%+.4f [%d: %+d/%+d/%+d] +++ %d: %f)\n", \
	       this->power2(this->root->data.rasterlevel)*this->root->data.x, \
	       this->power2(this->root->data.rasterlevel)*this->root->data.y, \
	       this->power2(this->root->data.rasterlevel)*this->root->data.z, \
	       this->root->data.rasterlevel, \
	       this->root->data.x, \
	       this->root->data.y, \
	       this->root->data.z, \
	       this->root->data.sizelevel, \
	       this->power2(this->root->data.sizelevel);
    }
#endif    
    return;
}


void IVP_OV_Tree_Manager::collect_subbox_collision_partners(const IVP_OV_Element *elem, const IVP_OV_Node *node)
{
    {
	for (int i=node->elements.len()-1;i>=0; i--) {		// check real distance of spheres
	    IVP_OV_Element *el = node->elements.element_at(i);
	    IVP_DOUBLE qdist = el->center.quad_distance_to(&elem->center);

	    IVP_DOUBLE minimal_dist = elem->radius + el->radius;
	    if ( qdist > minimal_dist * minimal_dist) continue;
	    this->collision_partners->add(el);
	}
    }
    {
	for (int i=node->children.len()-1; i>=0; i--) {
	    collect_subbox_collision_partners(elem, node->children.element_at(i));
	}
    }
    return;
}


void IVP_OV_Tree_Manager::collect_collision_partners(const IVP_OV_Element *elem, const IVP_OV_Node *masternode, const IVP_OV_Node *new_node)
{
    { // insert all element of higher levels 
	for (int i=masternode->elements.len()-1;i>=0; i--) {
		IVP_OV_Element *el = masternode->elements.element_at(i);
		IVP_DOUBLE qdist = el->center.quad_distance_to(&elem->center);

		IVP_DOUBLE minimal_dist = elem->radius + el->radius;
		if ( qdist > minimal_dist * minimal_dist) continue;
		this->collision_partners->add(el);
	}
    }
    // we have to sort the two cubes depending on their size
    if ( (masternode->data.sizelevel-1) > new_node->data.sizelevel ) {
	int rasterlevel_diff = (masternode->data.rasterlevel-1) - new_node->data.rasterlevel;
	for (int i=0; i<masternode->children.len(); i++) {
	    IVP_OV_Node *child = masternode->children.element_at(i);
	    if ( child == new_node ) {
		collect_subbox_collision_partners(elem, child);
	    } else {
		if ( box_overlaps_with_box(child, new_node, rasterlevel_diff) == IVP_TRUE ) {
		    collect_collision_partners(elem,child, new_node);
		}
	    }
	}
    }else {
	int rasterlevel_diff =  new_node->data.rasterlevel - (masternode->data.rasterlevel-1);
	for (int i=0; i<masternode->children.len(); i++) {
	    IVP_OV_Node *child = masternode->children.element_at(i);
	    if ( child == new_node ) {
		collect_subbox_collision_partners(elem, child);
	    }else {
		if ( box_overlaps_with_box(new_node, child, rasterlevel_diff) == IVP_TRUE ) {
		    collect_collision_partners(elem,child, new_node);
		}
	    }
        }
    }
    
    return;
}


IVP_DOUBLE IVP_OV_Tree_Manager::insert_ov_element(IVP_OV_Element *element,
                         IVP_DOUBLE min_radius,
                         IVP_DOUBLE max_radius,
                         IVP_U_Vector<IVP_OV_Element> *colliding_balls)
{
    if ( element == NULL ) {
        return(0);
    }
    
    IVP_DOUBLE used_radius;
    
    used_radius = calc_optimal_box(element, min_radius, max_radius);
	element->radius = (IVP_FLOAT)used_radius;

#ifdef IVP_HOME_BUILD
    // IVP_SUPREME_SUPPORT
    ivp_global_clustering_visualizer.longrange.add_object(element->real_object, &element->center, used_radius);
#endif

    // check if node (in terms of coordinates and size) is already present
    IVP_OV_Node *new_node=NULL;
    new_node = this->hash_table->find_node(&this->search_node);
    
    if ( new_node != NULL ) {

	// ------------------------------------------------
	// node already present in tree. Lets re-use it...
	// ------------------------------------------------
	
        new_node->elements.add(element);
        element->node = new_node;
    }else {
    
	// ------------------------------------------
	// desired node couldn't be found in tree...
	// ------------------------------------------
    

	// create new node...
	new_node = new IVP_OV_Node();
	new_node->data.x           = this->search_node.data.x;
	new_node->data.y           = this->search_node.data.y;
	new_node->data.z           = this->search_node.data.z;
	new_node->data.rasterlevel = this->search_node.data.rasterlevel;
	new_node->data.sizelevel   = this->search_node.data.sizelevel;

	new_node->elements.add(element);
	element->node = new_node;

	// the first object ever inserted defines our (preliminary) root box...
	if ( !this->root ) {
	    this->root = new_node;
	    this->hash_table->add_node(new_node);        
	    return(used_radius);
	}

	// check whether root box is already large enough for the element to completely fit in...
	for (;;) {
	    if ( this->root->data.rasterlevel >= new_node->data.rasterlevel ) {
		if ( !box_contains_box(&this->root->data, new_node, this->root->data.rasterlevel-new_node->data.rasterlevel) ) {
		    // new object won't fit into existing tree;
		    // expand tree until new object at least fits into root of tree...
		    expand_tree(new_node);
		    continue;
		}
	    }
	    else {
		// new object won't fit into existing tree;
		// expand tree until new object at least fits into root of tree...
		expand_tree(new_node);
		continue;
	    }
	    break;
	}

	// ---------------------------------------------------
	// new element will now fit (somewhere) into our tree
	// ---------------------------------------------------

	// new root cube is on same level as new node --> both are the same
	if ( this->root->data.rasterlevel == new_node->data.rasterlevel ) {
	    this->root->elements.add(element);
	    element->node = this->root;
	
	    P_DELETE(new_node);
	    new_node = this->root; // necessary for e.g. finding the collision partners (see below)

	} else {

	// insert new node into existing tree
	    this->hash_table->add_node(new_node);
	    
	    // search for the smallest box in one of the branches that completely embraces
	    // the new box
	    IVP_OV_Node *smallest_node;
	    smallest_node = find_smallest_box(this->root, new_node);

	    // now we can insert our new node (i.e. subbox) into our tree
	    connect_boxes(smallest_node, new_node);
	}
    }


    this->collision_partners = colliding_balls;
    if ( this->collision_partners ) {
    
	// search for colliding objects and insert them into the supplied list
	collect_collision_partners(element, this->root, new_node);

	// remove new element again from list (as we do not want our element to collide with itself :)
	//this->collision_partners->remove_allow_resort(element);  checks after are just faster than scanning this whole vector
    }
    
    // DONE...    
    return(used_radius);
}


IVP_OV_Node *IVP_OV_Tree_Manager::cleanup_node(IVP_OV_Node *node)
{
    if ( node->elements.n_elems != 0 ) return(NULL); // still elements left in node!
    if ( node->children.n_elems != 0 ) return(NULL); // still children left!

    if ( node->parent == NULL ) {
	this->root = NULL;
    }
    
    this->hash_table->remove_node(node);

    IVP_OV_Node *parent = node->parent;
    P_DELETE(node);

    return(parent);
}


void IVP_OV_Tree_Manager::remove_ov_element(IVP_OV_Element *element)
{
    
    IVP_OV_Node *node = element->node;
    if (!node) return;

#ifdef IVP_HOME_BUILD
    // IVP_SUPREME_SUPPORT
    ivp_global_clustering_visualizer.longrange.remove_object(element->real_object);
#endif

    element->node = NULL;
    node->elements.remove(element);

    while ( (node = cleanup_node(node)) != NULL ) { ; }

    return;
}


void IVP_OV_Tree_Manager::get_luf_coordinates_ws(const IVP_OV_Node *node, IVP_U_Float_Point *p, IVP_FLOAT *cubesize)
{
    p->k[0]   = (IVP_FLOAT)( this->power2(node->data.rasterlevel) * node->data.x );
    p->k[1]   = (IVP_FLOAT)( this->power2(node->data.rasterlevel) * node->data.y );
    p->k[2]   = (IVP_FLOAT)( this->power2(node->data.rasterlevel) * node->data.z );
    *cubesize = (IVP_FLOAT)( this->power2(node->data.sizelevel) );

    return;
}

void IVP_OV_Tree_Manager::get_center_coordinates_ws(const IVP_OV_Node *node, IVP_U_Float_Point *p, IVP_FLOAT *cubesize)
{
    p->k[0]   = (IVP_FLOAT)( this->power2(node->data.rasterlevel) * (node->data.x+1) );
    p->k[1]   = (IVP_FLOAT)( this->power2(node->data.rasterlevel) * (node->data.y+1) );
    p->k[2]   = (IVP_FLOAT)( this->power2(node->data.rasterlevel) * (node->data.z+1) );
    *cubesize = (IVP_FLOAT)( this->power2(node->data.sizelevel) );

    return;
}