ivp_surbuild_ledge_soup.cxx

hl2 source code. Do not use it illegal.

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

#include <ivp_physics.hxx>
#include <string.h>

//#define STATS
//#define IVP_CLUSTER_SHORTRANGE_BOTTOMUP
#define IVP_CLUSTER_SHORTRANGE_TOPDOWN
#define IVP_CLUSTER_SHORTRANGE_BALANCED
//#define IVP_CLUSTER_SHORTRANGE_BALANCE_VOLUME

#include <ivp_betterdebugmanager.hxx>
#include <ivp_template_surbuild.hxx>
#include <ivp_surbuild_ledge_soup.hxx>

#include <ivp_i_fpoint_vhash.hxx>

#include <ivp_compact_ledge.hxx>
#include <ivp_compact_surface.hxx>

#include <ivp_compact_ledge_solver.hxx>
#include <ivp_rot_inertia_solver.hxx>
#include <ivv_cluster_min_hash.hxx>
#include <ivp_surbuild_pointsoup.hxx>
#include <ivp_compact_recursive.hxx>


class IVV_Sphere {
public:
    int		number;
    IVP_U_Point	center;
    IVP_DOUBLE	radius;
    uchar	box_sizes[IVP_CLT_N_DIRECTIONS];
  
    IVP_Compact_Ledge *compact_ledge;
    IVV_Sphere	*child_1, *child_2;
    IVV_Sphere(){ P_MEM_CLEAR(this); };
};

struct IVV_Sphere_Cluster {
    ushort previous, next;
    IVV_Sphere *sphere;
};


int ivp_debug_sf_indent=0;
int ivp_debug_sf_treedepth=0;
int ivp_debug_sf_max_treedepth=0;
int ivp_debug_sf_n_nodes_with_one_terminal=0;
IVP_BOOL ivp_debug_sf_last_node_was_terminal = IVP_FALSE;

void debug_sphere_output(IVV_Sphere *sphere)
{
    ivp_debug_sf_treedepth++;
    if ( ivp_debug_sf_max_treedepth < ivp_debug_sf_treedepth ) ivp_debug_sf_max_treedepth = ivp_debug_sf_treedepth;
    
    //for (int x=0; x<ivp_debug_sf_indent; x++) {
    //	printf("  ");
    //}
    //printf("center: %.6f/%.6f/%.6f --- Radius: %.6f\n", sphere->center.k[0], sphere->center.k[1], sphere->center.k[2], sphere->radius);
    //ivp_debug_sf_indent++;
    if ( sphere->child_1 ) {
	IVP_ASSERT(sphere->child_1); // prevent nodes with one single child
	IVP_ASSERT(sphere->child_2);

	int n_terminal_children = 0;
	
	debug_sphere_output(sphere->child_1);
	if ( ivp_debug_sf_last_node_was_terminal ) n_terminal_children++;
	
	debug_sphere_output(sphere->child_2);
	if ( ivp_debug_sf_last_node_was_terminal ) n_terminal_children++;

	if ( n_terminal_children == 1 ) ivp_debug_sf_n_nodes_with_one_terminal++;
	
	ivp_debug_sf_last_node_was_terminal = IVP_FALSE;
    }
    else {
	IVP_ASSERT(!sphere->child_1); // prevent nodes with one single child
	IVP_ASSERT(!sphere->child_2);
	ivp_debug_sf_last_node_was_terminal = IVP_TRUE;
    }
    //ivp_debug_sf_indent--;
    ivp_debug_sf_treedepth--;
}



IVP_SurfaceBuilder_Ledge_Soup::IVP_SurfaceBuilder_Ledge_Soup()
{
    P_MEM_CLEAR(this);
    this->smallest_radius = 0;
    this->compact_surface = NULL;

    this->extents_min.set( 1000000.0f,  1000000.0f,  1000000.0f);
    this->extents_max.set(-1000000.0f, -1000000.0f, -1000000.0f);
    
    this->interval_minhash = NULL;

    return;
}

IVP_SurfaceBuilder_Ledge_Soup::~IVP_SurfaceBuilder_Ledge_Soup(){
}


void IVP_SurfaceBuilder_Ledge_Soup::insert_ledge(IVP_Compact_Ledge *c_ledge)
{
  if (!c_ledge){
    IVP_IFDEBUG(IVP_DM_SURBUILD_LEDGESOUP) {
      ivp_debugmanager.dprint(IVP_DM_SURBUILD_LEDGESOUP, "warning: tried to add NULL ledge in IVP_SurfaceBuilder_Ledge_Soup::insert_ledge()\n");
    }
    return;

  }
  this->c_ledge_vec.add(c_ledge);
}


IVP_Compact_Surface *IVP_SurfaceBuilder_Ledge_Soup::compile(IVP_Template_Surbuild_LedgeSoup *templ){
    IVP_Template_Surbuild_LedgeSoup t2;
    if (!templ) templ = &t2;
    this->parameters = templ;

    if ( this->c_ledge_vec.len() == 0 ) return NULL; // invalid (empty) ledge list. aborting...

    // To be called after all ledges are inserted.
    // Builds tree.
#if 0
    this->ledges_to_spheres();
#else
    this->ledges_to_boxes_and_spheres();
#endif

#ifdef IVP_CLUSTER_SHORTRANGE_BOTTOMUP    
    this->cluster_spheres_bottomup(1.1f);
#endif
#ifdef IVP_CLUSTER_SHORTRANGE_TOPDOWN    
    this->cluster_spheres_topdown_mediancut(1.0f);
#endif	

#if defined(STATS)
    if(number_of_terminal_spheres > 1){
	ivp_debug_sf_treedepth = 0;
	ivp_debug_sf_max_treedepth = 0;
	ivp_debug_sf_n_nodes_with_one_terminal = 0;
	ivp_debug_sf_last_node_was_terminal = IVP_FALSE;
	debug_sphere_output(this->spheres_cluster[this->spheres_cluster[0].next].sphere);
	printf("Tree depth: %d\n", ivp_debug_sf_max_treedepth);
	printf("# of terminals: %d\n", this->number_of_terminal_spheres);
    }
    //printf("# of nodes with single terminal child: %d\n", ivp_debug_sf_n_nodes_with_one_terminal);
#endif
    
    IVP_ASSERT(compact_surface == 0);

    if (templ->build_root_convex_hull && c_ledge_vec.len()>1){
      this->build_root_convex_hull();
    }
    
    this->allocate_compact_surface();
    this->create_compact_ledgetree();
    this->insert_radius_in_compact_surface();
    
    this->cleanup();

    IVP_Compact_Surface *res = compact_surface;

    if ( this->number_of_terminal_spheres > 1 && !parameters->link_to_input_compact_ledges ) {
	if ( parameters->merge_points == IVP_SLMP_MERGE_AND_REALLOCATE ){
		res = (IVP_Compact_Surface *)ivp_malloc_aligned( compact_surface->byte_size, 16);
		memcpy( (void *)res, this->compact_surface, compact_surface->byte_size);
		ivp_free_aligned( compact_surface );
        }
    }
    compact_surface = NULL;
    
    return res;
}

void IVP_SurfaceBuilder_Ledge_Soup::add_ledge_tree_to_convex_hull(class IVP_Compact_Recursive &cr, IVV_Sphere *node){
  if (!node) return;
  if (node->compact_ledge){
    cr.add_compact_ledge(node->compact_ledge);
    return;
  }
  if (node->child_1) add_ledge_tree_to_convex_hull(cr, node->child_1);
  if (node->child_2) add_ledge_tree_to_convex_hull(cr, node->child_2);
}


void IVP_SurfaceBuilder_Ledge_Soup::build_root_convex_hull(){
    IVV_Sphere *node = this->spheres_cluster[this->spheres_cluster[0].next].sphere;

    IVP_Compact_Recursive cr;
    add_ledge_tree_to_convex_hull(cr, node);
    
    IVP_Compact_Ledge *hull = cr.compile(); 
    c_ledge_vec.add(hull);
    node->compact_ledge = hull;
    rec_spheres.add(node);
}


void IVP_SurfaceBuilder_Ledge_Soup::cleanup()
{
    this->terminal_spheres.clear();
    int i;
    for (i=0; i<this->all_spheres.len(); i++) {
	IVV_Sphere *sp = this->all_spheres.element_at(i);
	P_DELETE(sp);
    }
    this->all_spheres.clear();
    P_FREE(this->spheres_cluster);
    return;

}


void IVP_SurfaceBuilder_Ledge_Soup::ledges_to_spheres()
{
    // -----------------------------------------------------------------------------------
    // create a minimal sphere around each ledge and save it into a fix-sized array...
    // -----------------------------------------------------------------------------------    

    this->size_of_tree_in_bytes = 0; // debugging
    IVV_Sphere *sphere;
    
    // find number of spheres to generate
    int n_ledges = this->c_ledge_vec.len();
    this->number_of_terminal_spheres = n_ledges;

    // allocate array: element #0 (dummy) always indicates the first valid element through its 'next' pointer!
    this->spheres_cluster = (struct IVV_Sphere_Cluster *)p_calloc(this->number_of_terminal_spheres+1, sizeof(IVV_Sphere_Cluster));

    this->spheres_cluster[0].next = 1;
    int n = 1;
    int ledge_cnt;
    for (ledge_cnt=0; ledge_cnt<n_ledges; ledge_cnt++) {
	IVP_Compact_Ledge *c_ledge = this->c_ledge_vec.element_at(ledge_cnt);
	IVP_ASSERT (c_ledge->get_n_triangles() > 0);

	sphere = new IVV_Sphere();
	this->all_spheres.add(sphere);
	IVP_U_Point center;
	IVP_DOUBLE radius;
	IVP_CLS.calc_geom_center_and_radius( c_ledge, &center, &radius);
	IVP_CLS.calc_radius_to_given_center( c_ledge, &center, &radius);

	if ( center.k[0]-radius < this->extents_min.k[0] ) this->extents_min.k[0] = center.k[0]-radius;
	if ( center.k[0]+radius > this->extents_max.k[0] ) this->extents_max.k[0] = center.k[0]+radius;
	if ( center.k[1]-radius < this->extents_min.k[1] ) this->extents_min.k[1] = center.k[1]-radius;
	if ( center.k[1]+radius > this->extents_max.k[1] ) this->extents_max.k[1] = center.k[1]+radius;
	if ( center.k[2]-radius < this->extents_min.k[2] ) this->extents_min.k[2] = center.k[2]-radius;
	if ( center.k[2]+radius > this->extents_max.k[2] ) this->extents_max.k[2] = center.k[2]+radius;
	
	sphere->radius = radius;
	sphere->center.set(&center);

	this->size_of_tree_in_bytes += sizeof(IVV_Sphere); // debugging
	
	sphere->compact_ledge = c_ledge;
	//	sphere->parent = NULL;
	sphere->child_1 = NULL;
	sphere->child_2 = NULL;
	sphere->number = n;

	this->spheres_cluster[n].previous = n-1;
	this->spheres_cluster[n].next     = n+1;
	this->spheres_cluster[n].sphere   = sphere;

	if ( this->smallest_radius == 0 ) {
	    this->smallest_radius = sphere->radius;
	}
	else if ( sphere->radius < this->smallest_radius ) {
	    this->smallest_radius = sphere->radius;
	}

	this->terminal_spheres.add(sphere);

	n++;
    }

    this->spheres_cluster[n-1].next = 0;
    
    //printf("Radius of smallest sphere: %f\n", this->smallest_radius);

    // @@@SF: for speedup of single-convex objects, move this into sphere-clustering!
    if ( ledge_cnt > 1 ) {
	IVP_DOUBLE ext_x = this->extents_max.k[0] - this->extents_min.k[0];
	IVP_DOUBLE ext_y = this->extents_max.k[1] - this->extents_min.k[1];
	IVP_DOUBLE ext_z = this->extents_max.k[2] - this->extents_min.k[2];
	if ( ext_x < ext_y ) {
	    if ( ext_y < ext_z ) this->longest_axis = 2;
	    else                 this->longest_axis = 1;
	}
	else {
	    if ( ext_x < ext_z ) this->longest_axis = 2;
	    else                 this->longest_axis = 0;
	}
	//printf("Extension in x-direction: %f\n", this->extents_max_x - this->extents_min_x);
	//printf("Extension in y-direction: %f\n", this->extents_max_y - this->extents_min_y);
	//printf("Extension in z-direction: %f\n", this->extents_max_z - this->extents_min_z);
    }
    
    return;
}


/******************************************************************************
 *  Method:	    ledges_to_boxes_and_spheres
 *  Description:    This method will ...
 *  Input:	    <>  ...
 *  Output:	    ...
 *****************************************************************************/
void IVP_SurfaceBuilder_Ledge_Soup::ledges_to_boxes_and_spheres()
{
    // -----------------------------------------------------------------------------------
    // create a minimal sphere around each ledge and save it into a fix-sized array...
    // -----------------------------------------------------------------------------------    

    IVP_DOUBLE dimension_steps = IVP_COMPACT_BOUNDINGBOX_STEP_SIZE;
    
    this->size_of_tree_in_bytes = 0; // debugging
    IVV_Sphere *sphere;
    
    // find number of spheres to generate
    int n_ledges = this->c_ledge_vec.len();
    this->number_of_terminal_spheres = n_ledges;

    // allocate array: element #0 (dummy) always indicates the first valid element through its 'next' pointer!
    this->spheres_cluster = (struct IVV_Sphere_Cluster *)p_calloc(this->number_of_terminal_spheres+1, sizeof(IVV_Sphere_Cluster));

    this->spheres_cluster[0].next = 1;
    int n = 1;
	int ledge_cnt;

    for (ledge_cnt = 0; ledge_cnt < n_ledges; ledge_cnt++) {
		IVP_Compact_Ledge* compact_ledge = this->c_ledge_vec.element_at(ledge_cnt);

		IVP_ASSERT(compact_ledge->get_n_triangles() > 0);

		sphere = new IVV_Sphere();
		this->all_spheres.add(sphere);

		IVP_U_Point min, max, rad;
		IVP_CLS.calc_bounding_box(compact_ledge, &min, &max);

		sphere->center.set_interpolate(&max, &min, 0.5f); // use center of bounding box as center of sphere
		rad.subtract(&max, &sphere->center);
		sphere->radius = rad.real_length(); // calculate radius of box-enclosing sphere

		if ( sphere->center.k[0]-sphere->radius < this->extents_min.k[0] ) this->extents_min.k[0] = sphere->center.k[0]-sphere->radius;
		if ( sphere->center.k[0]+sphere->radius > this->extents_max.k[0] ) this->extents_max.k[0] = sphere->center.k[0]+sphere->radius;
		if ( sphere->center.k[1]-sphere->radius < this->extents_min.k[1] ) this->extents_min.k[1] = sphere->center.k[1]-sphere->radius;
		if ( sphere->center.k[1]+sphere->radius > this->extents_max.k[1] ) this->extents_max.k[1] = sphere->center.k[1]+sphere->radius;
		if ( sphere->center.k[2]-sphere->radius < this->extents_min.k[2] ) this->extents_min.k[2] = sphere->center.k[2]-sphere->radius;
		if ( sphere->center.k[2]+sphere->radius > this->extents_max.k[2] ) this->extents_max.k[2] = sphere->center.k[2]+sphere->radius;
	
		// @@CB - scaling
		IVP_DOUBLE work = sphere->radius * dimension_steps; // dimension_steps is constanct, 1/250 @@CB
		sphere->box_sizes[0] = int((max.k[0] - sphere->center.k[0])/work)+1; // translate to origin, scale by 250, divide 
		sphere->box_sizes[1] = int((max.k[1] - sphere->center.k[1])/work)+1; // by radius, add 1
		sphere->box_sizes[2] = int((max.k[2] - sphere->center.k[2])/work)+1;
	
		this->size_of_tree_in_bytes += sizeof(IVV_Sphere); // debugging
	
		sphere->compact_ledge = compact_ledge;
		//	sphere->parent = NULL;
		sphere->child_1 = NULL;
		sphere->child_2 = NULL;
		sphere->number = n;

		this->spheres_cluster[n].previous = n-1;
		this->spheres_cluster[n].next     = n+1;
		this->spheres_cluster[n].sphere   = sphere;

		if ( this->smallest_radius == 0 ) {
		    this->smallest_radius = sphere->radius;
		} else if ( sphere->radius < this->smallest_radius ) {
		    this->smallest_radius = sphere->radius;