ivp_clustering_longrange.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.

#include <ivp_physics.hxx>

#define MY_DEBUG
//#define DEBUG_BASIC_OUTPUT
#define DEBUG_ERROR_OUTPUT
//#define DEBUG_CONNECTBOXES
//#define DEBUG_CALCOPTIMALBOX
//#define DEBUG_EXPANDTREE
//#define DEBUG_VECTORLISTS

#define DEBUG_IF if ( ctr > 0 )

#include <ivu_float.hxx>

#include <ivu_vhash.hxx>
#include <ivp_mindist.hxx>
#include <ivp_mindist_intern.hxx>
#include <ivp_clustering_longrange.hxx>
#include <ivp_clustering_lrange_hash.hxx>
#include <ivp_hull_manager.hxx>
#include <ivp_clustering_visualizer.hxx>

int ivp_debug_indent=0;

void ivp_indent_output()
{
    for (int indent_x=0; indent_x<ivp_debug_indent; indent_x++) {
        printf(" ");
    }
    return;
}



IVP_OV_Element::IVP_OV_Element(IVP_Real_Object *obj): collision_fvector(16)
{
    this->node = NULL;
    this->center.set_to_zero();
    this->radius = -1.0f;
    this->real_object = obj;
    this->hull_manager = NULL;
    return;
}

IVP_OV_Element::~IVP_OV_Element(){
    if (hull_manager){
	hull_manager->remove_synapse(this);
	hull_manager = NULL;
    }
    this->real_object->get_environment()->fire_object_is_removed_from_collision_detection(real_object);
    IVP_ASSERT( collision_fvector.len() == 0);
    this->real_object->get_environment()->get_ov_tree_manager()->remove_ov_element(this);
}

void IVP_OV_Element::add_to_hull_manager(IVP_Hull_Manager *hm, IVP_DOUBLE hull_time){
    const IVP_Time &current_time = real_object->get_environment()->get_current_time();
    if (hull_manager){
	IVP_ASSERT(hull_manager == hm);
	hull_manager->update_synapse(this,current_time,hull_time);
	return;
    }
    hull_manager = hm;
    hm->insert_synapse(this,current_time, hull_time);
}

IVP_HULL_ELEM_TYPE IVP_OV_Element::get_type(){ return IVP_HULL_ELEM_OO_CONNECTOR; };

void IVP_OV_Element::hull_manager_is_going_to_be_deleted_event(IVP_Hull_Manager *hm){
    IVP_ASSERT(hm == hull_manager );
    delete this;
}

void IVP_OV_Element::hull_limit_exceeded_event(IVP_Hull_Manager *hm, IVP_HTIME){
    IVP_ASSERT(hm == hull_manager);
    real_object->get_environment()->get_mindist_manager()->recheck_ov_element( real_object );
}

void IVP_OV_Element::add_oo_collision(IVP_Collision *connector){
    collision_fvector.add(connector);
}

void IVP_OV_Element::remove_oo_collision(IVP_Collision *connector){
    collision_fvector.remove_allow_resort(connector);
}
    


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


IVP_OV_Node::IVP_OV_Node()
{
    this->parent = 0;
    return;
}


IVP_OV_Node::~IVP_OV_Node()
{
    if (parent){
	parent->children.remove(this);
    }
    while (children.len()){
	delete children.element_at(0);
    }
    // do not delete the elements
    return;
}


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


IVP_OV_Tree_Manager::IVP_OV_Tree_Manager()
{
    
    this->environment = NULL;
    this->root = NULL;
    this->hash_table = new IVP_ov_tree_hash(256);

    IVP_DOUBLE y = 1.0f;
    IVP_DOUBLE iy = 1.0f;
    
    for (int x=0 ; x<=40; x++) {
	this->powerlist[40+x] = y;
	this->powerlist[40-x] = iy;
	y *= 2.0f;
	iy *= 0.5f;
    }
    
    return;
}


IVP_OV_Tree_Manager::~IVP_OV_Tree_Manager()
{
    P_DELETE(this->hash_table);
    return;
}


int IVP_OV_Tree_Manager::log_base2(IVP_DOUBLE x) const
{
    return PFM_LD(x);
}



IVP_BOOL IVP_OV_Tree_Manager::box_contains_box(const struct IVP_OV_Node_Data *master_data,
					       const IVP_OV_Node *sub_node,
					       const int rasterlevel_diff) const
{
  // returns IVP_TRUE  if second box completely fits into first box
  // returns IVP_FALSE if second box is (partially) outside of first box
  
    int master_x1 =  master_data->x << rasterlevel_diff; // convert master rasterpoints to sub rasterpoints
    int master_y1 =  master_data->y << rasterlevel_diff;
    int master_z1 =  master_data->z << rasterlevel_diff;
    int offset = (2 << rasterlevel_diff) - 2;

    if ( sub_node->data.x < master_x1 ) return(IVP_FALSE);
    if ( sub_node->data.y < master_y1 ) return(IVP_FALSE);
    if ( sub_node->data.z < master_z1 ) return(IVP_FALSE);

    if ( sub_node->data.x > (master_x1+offset) ) return(IVP_FALSE);
    if ( sub_node->data.y > (master_y1+offset) ) return(IVP_FALSE);
    if ( sub_node->data.z > (master_z1+offset) ) return(IVP_FALSE);

    return(IVP_TRUE);
}
#if !defined(SUN) && !defined(LINUX) && !(__MWERKS__ && __POWERPC__) && !defined(GEKKO)
inline int ivp_int_floor(IVP_DOUBLE x){
    return (int)floorf(x);
}
inline int ivp_int_ceil(IVP_DOUBLE x){
    return (int)ceilf(x);
}
#else
inline int ivp_int_floor(IVP_DOUBLE x){
    return (int)floor(x);
}
inline int ivp_int_ceil(IVP_DOUBLE x){
    return (int)ceil(x);
}
#endif

IVP_DOUBLE IVP_OV_Tree_Manager::calc_optimal_box(const IVP_OV_Element *element, IVP_DOUBLE min_radius, IVP_DOUBLE max_radius)
{
    IVP_DOUBLE raster_size   = 2.0f * min_radius;
    int    raster_level  = this->log_base2(raster_size)+1;
    int    raster_points;

    // as our selfmade power/log algorithm only works for the range of -40 to 40 we have to manually
    // crop any possible underflow (caused by extremely small objects) to the minimum value, i.e. we
    // have a minimum boxsize of 2^-40 meters to be used by our tree
    if ( raster_level < -40 ) {
	raster_level = -40;
	IVP_IF(1) {
	    printf("IVP_OV_Tree_Manager - WARNING: Object too small! Adjusting raster size to 2^-40 meters.\n");
	}
    }
    
#ifdef DEBUG_CALCOPTIMALBOX
    DEBUG_IF {
	ivp_indent_output();
	printf("log2(%f) = %f\n", raster_size, this->log2(raster_size));
	ivp_indent_output();
	printf("initial rasterlevel: %d\n", raster_level);
    }
#endif    

    int   x_min, x_max;
    int   y_min, y_max;
    int   z_min, z_max;

    // first find the perfect box for our element using the minimal radius
    for (;; raster_level++) {

        raster_points = raster_level - 1;
        IVP_DOUBLE iraster_dist   = this->power2(-raster_points);
#ifdef DEBUG_CALCOPTIMALBOX
	DEBUG_IF {
	    ivp_indent_output();
	    printf("raster distance: %f\n", raster_dist);
	}
#endif    
        x_min = ivp_int_floor((element->center.k[0]-min_radius) * iraster_dist);
        x_max = ivp_int_ceil((element->center.k[0]+min_radius) * iraster_dist);

#ifdef DEBUG_CALCOPTIMALBOX
	DEBUG_IF {
	    ivp_indent_output();
	    printf("x: %d - %d\n", x_min, x_max);
	}
#endif    
        if ( x_max > x_min+2 ) continue;
        
        y_min = ivp_int_floor((element->center.k[1]-min_radius) * iraster_dist);
        y_max = ivp_int_ceil((element->center.k[1]+min_radius) *iraster_dist);

#ifdef DEBUG_CALCOPTIMALBOX
	DEBUG_IF {
	    ivp_indent_output();
	    printf("y: %d - %d\n", y_min, y_max);
	}
#endif    
        if ( y_max > y_min+2 )       continue;
        
        z_min = ivp_int_floor((element->center.k[2]-min_radius) * iraster_dist);
        z_max = ivp_int_ceil((element->center.k[2]+min_radius) * iraster_dist);

#ifdef DEBUG_CALCOPTIMALBOX
	DEBUG_IF {
	    ivp_indent_output();
	    printf("z: %d - %d\n", z_min, z_max);
	}
#endif    
        if ( z_max > z_min+2 )       continue;

        break;

    }

    this->search_node.data.x           = x_min;
    this->search_node.data.y           = y_min;
    this->search_node.data.z           = z_min;
    this->search_node.data.rasterlevel = raster_points;
    this->search_node.data.sizelevel   = raster_level;

    
    // check whether the element will fit into a box one level above using the
    // maximum radius; if this operation fails, we simply keep the values from
    // the first (smaller) box
    if ( min_radius >= max_radius ) {
        return(min_radius);
    }

    raster_level++;
    raster_points = raster_level - 1;
    IVP_DOUBLE iraster_dist   = this->power2(-raster_points);

    x_min = ivp_int_floor((element->center.k[0]-max_radius) * iraster_dist);
    x_max = ivp_int_ceil((element->center.k[0]+max_radius) *iraster_dist);

#ifdef DEBUG_CALCOPTIMALBOX
    DEBUG_IF {
	ivp_indent_output();
	printf("x: %d - %d\n", x_min, x_max);
    }
#endif    
    if ( x_max > x_min+2 ) {
        return(min_radius);
    }
        
    y_min = ivp_int_floor((element->center.k[1]-max_radius) * iraster_dist);
    y_max = ivp_int_ceil((element->center.k[1]+max_radius) * iraster_dist);

#ifdef DEBUG_CALCOPTIMALBOX
    DEBUG_IF {
	ivp_indent_output();
	printf("y: %d - %d\n", y_min, y_max);
    }
#endif    
    if ( y_max > y_min+2 ) {
        return(min_radius);
    }
        
    z_min = ivp_int_floor((element->center.k[2]-max_radius) * iraster_dist);
    z_max = ivp_int_ceil((element->center.k[2]+max_radius) * iraster_dist);

#ifdef DEBUG_CALCOPTIMALBOX
    DEBUG_IF {
	ivp_indent_output();
	printf("z: %d - %d\n", z_min, z_max);
    }
#endif    
    if ( z_max > z_min+2 ) {
        return(min_radius);
    }

    this->search_node.data.x           = x_min;
    this->search_node.data.y           = y_min;
    this->search_node.data.z           = z_min;
    this->search_node.data.rasterlevel = raster_points;
    this->search_node.data.sizelevel   = raster_level;

    return(max_radius);
}


IVP_BOOL IVP_OV_Tree_Manager::box_overlaps_with_box(const IVP_OV_Node *largenode,
						    const IVP_OV_Node *smallnode,
						    const int rasterlevel_diff) const
// returns IVP_TRUE  if the two boxes overlap
// returns IVP_FALSE if the two boxes are disjunkt
{
    if ( smallnode->data.x+2 <= (largenode->data.x << rasterlevel_diff) ) return(IVP_FALSE);
    if ( smallnode->data.y+2 <= (largenode->data.y << rasterlevel_diff) ) return(IVP_FALSE);
    if ( smallnode->data.z+2 <= (largenode->data.z << rasterlevel_diff) ) return(IVP_FALSE);

    if ( smallnode->data.x >= ((largenode->data.x + 2) << rasterlevel_diff) ) return(IVP_FALSE);
    if ( smallnode->data.y >= ((largenode->data.y + 2) << rasterlevel_diff) ) return(IVP_FALSE);
    if ( smallnode->data.z >= ((largenode->data.z + 2) << rasterlevel_diff) ) return(IVP_FALSE);

    return(IVP_TRUE);
}


IVP_OV_Node *IVP_OV_Tree_Manager::find_smallest_box(const IVP_OV_Node *master_node, const IVP_OV_Node *sub_node) const
{
    int i;
    int rasterlevel_diff = (master_node->data.rasterlevel-1) - sub_node->data.rasterlevel;
    for (i=0; i<master_node->children.len(); i++) {
	const IVP_OV_Node *child = master_node->children.element_at(i);
        if ( box_contains_box(&child->data, sub_node, rasterlevel_diff) == IVP_TRUE ) {
            return(find_smallest_box(child, sub_node));
        }
    }
    
    return((IVP_OV_Node *)master_node);
}


void IVP_OV_Tree_Manager::connect_boxes(IVP_OV_Node *node, IVP_OV_Node *new_node)
{
    int rasterlevel_diff = node->data.sizelevel - new_node->data.sizelevel;

    IVP_ASSERT(rasterlevel_diff>0);

    if ( rasterlevel_diff == 1 ) {
	// new node is exactly one level below -> simply insert it as one of our children
	new_node->parent = node;
	node->children.add(new_node);
#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("New box found one level below. Inserting it...\n");
	}
#endif
	return;
    }
    
    // create the new subbox,
    // add it to the current node, and continue the search using
    // the new subnode;
    IVP_OV_Node *new_subnode = new IVP_OV_Node();
	

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

    // if new node lower than center, use lowest row
    if ( new_node->data.y >= ((node->data.y+1)<<rasterlevel_diff) ) {
	new_subnode->data.y = (node->data.y+1)*2;
    }
    // if new node lower than quarter, use middle row
    else if ( new_node->data.y >= (((node->data.y<<1)+1)<<(rasterlevel_diff-1)) ) {
	new_subnode->data.y = (node->data.y*2)+1;