ivp_mindist.cxx

hl2 source code. Do not use it illegal.

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

// IVP_EXPORT_PRIVATE

#include <ivp_physics.hxx>
#include <ivp_performancecounter.hxx>

#include <ivp_debug_manager.hxx>
#include <ivp_debug.hxx>

#ifndef WIN32
#pragma implementation "ivp_mindist.hxx"
#pragma implementation "ivp_mindist_intern.hxx"
#endif

#include <ivu_memory.hxx>
#include <ivp_hull_manager.hxx>

#include <ivp_mindist_intern.hxx>
#include <ivp_mindist_event.hxx>
#include <ivp_mindist_macros.hxx>

#include <ivp_compact_ledge.hxx>
#include <ivp_cache_object.hxx>
#include <ivp_cache_ledge_point.hxx>
#include <ivp_compact_ledge_solver.hxx>
#include <ivp_compact_surface.hxx>

#include <ivp_core_macros.hxx>
#include <ivu_hash.hxx>
#include <ivu_diff_hash.hxx>
#include <ivp_time.hxx>

#include <ivp_phantom.hxx>
#include <ivp_collision_filter.hxx>
#include <ivp_clustering_longrange.hxx>
#include <ivp_clustering_visualizer.hxx>
#include <ivp_range_manager.hxx>
#include <ivp_universe_manager.hxx>
#include <ivp_surman_polygon.hxx>

//#include <../IVP_INTERN/ivp_friction.hxx> //for debugging
#include <ivp_friction.hxx> //for debugging @@CB

// #define MIN_DEBUG 1

// MINIMIZE distance from 2 convex bowls beginning with
// start position of 2 synapses.


IVP_Mindist_Settings ivp_mindist_settings;

void IVP_Mindist_Settings::set_collision_tolerance(IVP_DOUBLE t){
    real_coll_dist =                         0.1f * t;
    
    min_coll_dists = real_coll_dist        + 0.9f * t;
    minimum_friction_dist = min_coll_dists + 0.0f * t;

    for (int i = IVP_MAX_STEPS_FOR_COLLDIST_DECREASE-1; i >= 0 ; i-- ){
	coll_dists[i] = min_coll_dists + ( i / IVP_DOUBLE(IVP_MAX_STEPS_FOR_COLLDIST_DECREASE)) * ( minimum_friction_dist - min_coll_dists );
    }
    friction_dist = minimum_friction_dist + 1.0f *t;

    keeper_dist = friction_dist + 0.3f * t;

    speed_after_keeper_dist = IVP_Inline_Math::ivp_sqrtf( 2.0f * (keeper_dist - min_coll_dists) * 9.81f ); //speed when falling down from keeper dist to coll dist
    distance_keepers_safety = 0.01f * t; 			//safety gap, when surpassed, mindist doesnt appear in complex
    max_distance_for_friction = friction_dist + 2.5f * t;
    max_distance_for_impact_system = friction_dist + 20.0f * t;
    
    min_vertical_speed_at_collision = 2.0f * t;	// meter /second

    mindist_change_force_dist = min_coll_dists * 0.1f;
}

void IVP_Mindist_Settings::set_event_queue_min_delta_time_base(IVP_DOUBLE base){
	event_queue_min_delta_time_base = base; //@@CB
}

IVP_Mindist_Settings::IVP_Mindist_Settings(){
    set_collision_tolerance( 0.01f );
	set_event_queue_min_delta_time_base(1.0f);
	max_spawned_mindist_count = 1000;
}



IVP_Mindist_Base::IVP_Mindist_Base(IVP_Collision_Delegator *del): IVP_Collision(del){
     coll_type = IVP_COLL_NONE;
     synapse_sort_flag = 0;
     recalc_result = IVP_MDRR_OK;
     is_in_phantom_set = IVP_FALSE;   // mindist is in phantom set_of_mindists already
     mindist_status = IVP_MD_UNINITIALIZED;
     mindist_function = IVP_MF_COLLISION;
     coll_dist_selector = IVP_MAX_STEPS_FOR_COLLDIST_DECREASE-1;
#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED
     disable_halfspace_optimization = IVP_FALSE;
     sum_angular_hull_time = 0.0f;
#endif
     index = IVP_U_MINLIST_UNUSED;
}

IVP_Mindist::IVP_Mindist(IVP_Environment *my_env, IVP_Collision_Delegator *del): IVP_Mindist_Base(del){
  IVP_ASSERT(del);
    recalc_time_stamp = 0;
    last_visited_triangle=NULL;
    my_env->get_statistic_manager()->sum_of_mindists++;
    my_env->get_statistic_manager()->mindists_generated++;
}

IVP_Mindist *g_pCurrentMindist = NULL;
bool g_fDeferDeleteMindist = false;

/********************************************************************************
 *	Name:	    	~IVP_Mindist  	
 *	Description:	deletes mindist
 ********************************************************************************/
IVP_Mindist::~IVP_Mindist(){
#if defined(DEBUG)
	if ( g_pCurrentMindist==this)
	{
		IVP_Collision_Filter *coll_filter = get_synapse(0)->l_obj->get_environment()->get_collision_filter();
		coll_filter->check_objects_for_collision_detection( get_synapse(0)->l_obj, get_synapse(1)->l_obj );
	}
#endif
	IVP_ASSERT(g_pCurrentMindist!=this);
    IVP_Environment *l_environment = get_mindist_synapse(0)->get_object()->get_environment();
    l_environment->get_statistic_manager()->sum_of_mindists--;
    l_environment->get_statistic_manager()->mindists_deleted++;

    IVP_Mindist_Manager *mm = l_environment->get_mindist_manager();
    IVP_Mindist *md = (IVP_Mindist *)this;
    if (is_in_phantom_set){
      mm->mindist_left_phantom(md);
    }
    
    switch(mindist_status){
    case IVP_MD_EXACT: 
	mm->remove_exact_mindist(md);
	break;
    case IVP_MD_HULL:
    case IVP_MD_HULL_RECURSIVE:
	mm->remove_hull_mindist(md);      
	break;
    case IVP_MD_INVALID:
	mm->remove_invalid_mindist(md);
	break;
      
    case IVP_MD_UNINITIALIZED:
    break;
    }

    IVP_ASSERT (index == IVP_U_MINLIST_UNUSED);
    
    md->get_synapse(0)->get_object()->get_surface_manager()->remove_reference_to_ledge(md->get_synapse(0)->get_ledge());
    md->get_synapse(1)->get_object()->get_surface_manager()->remove_reference_to_ledge(md->get_synapse(1)->get_ledge());
    
    delegator->collision_is_going_to_be_deleted_event(this);
}


void IVP_Mindist::print(const char *)
{
    ivp_message("syn0: ");
    this->get_synapse(0)->print();
    ivp_message("\nsyn1: ");
    this->get_synapse(0)->print();
    ivp_message("\n");
}


void IVP_Mindist_Base::get_objects( IVP_Real_Object *objects_out[2] ){
    objects_out[0] = get_mindist_synapse(0)->get_object();
    objects_out[1] = get_mindist_synapse(1)->get_object();
}

void IVP_Mindist_Base::get_ledges( const IVP_Compact_Ledge *ledges_out[2] ){
  ledges_out[0] = get_mindist_synapse(0)->edge->get_compact_ledge();
  ledges_out[1] = get_mindist_synapse(1)->edge->get_compact_ledge();
}


void IVP_Synapse_Real::check_consistency_of_ledge(const IVP_Compact_Edge * /*test_edge*/ )const{
    return;
#if 0
    const IVP_Compact_Ledge *ledge = test_edge->get_compact_ledge();
    const IVP_Compact_Surface *sur = ((IVP_SurfaceManager_Polygon *)l_obj->get_surface_manager())->get_compact_surface();
    const IVP_Compact_Ledgetree_Node *root = sur->get_compact_ledge_tree_root();
    IVP_IF (root ->is_terminal()){
	const IVP_Compact_Ledge *ref_ledge = root->get_compact_ledge();
	IVP_ASSERT (  ref_ledge == ledge );
    }
#endif    
}


const IVP_Compact_Ledge *IVP_Synapse::get_ledge() const
{
    return edge->get_compact_ledge();
}


#if defined(DEBUG) || 1
void IVP_Synapse_Real::print()
{
    char *stat_text = NULL;
    int n_points = 0;
    printf("(syn) ");
    switch(this->status){
    case IVP_ST_POINT:
     	stat_text = (char *)"Point";
	n_points = 1;
	break;
    case IVP_ST_EDGE:
	stat_text = (char *)"Edge";
	n_points = 2;
	break;
    case IVP_ST_TRIANGLE:
	stat_text = (char *)"Triangle";
	n_points = 3;
	break;
    default:
	CORE;
    }
}
#endif

void IVP_Mindist_Manager::print_mindists()
{
    IVP_Mindist *mdist;
    int i;
    printf("\n--------------------------\n");
    for(i=0, mdist=this->exact_mindists; mdist; mdist=mdist->next, i++){
	mdist->print("exact:");
    }
    for(i=0, mdist=this->invalid_mindists; mdist; mdist=mdist->next, i++){
	mdist->print("invalid:");
    }
}

void IVP_Mindist::init_mindist(IVP_Real_Object *pop0, IVP_Real_Object *pop1,const IVP_Compact_Edge *e0,const  IVP_Compact_Edge *e1){
  IVP_Synapse_Real *syn0 = this->get_synapse(0);
  IVP_Synapse_Real *syn1 = this->get_synapse(1);

  switch (pop1->get_type()){
  case IVP_POLYGON:
    syn1->init_synapse(this, pop1,  e1, IVP_ST_POINT);
    pop1->get_surface_manager()->add_reference_to_ledge(e1->get_compact_ledge());
    break;
  case IVP_BALL:
	  if ( pop0->get_type() == IVP_BALL ){
		  if ( pop0->client_data < pop1->client_data ) {
			IVP_Synapse_Real *synh; synh = syn0; syn0 = syn1; syn1 = synh;
		  }
	  }else{
		IVP_Synapse_Real *synh; synh = syn0; syn0 = syn1; syn1 = synh;
	  }
    syn1->init_synapse(this, pop1,  e1,IVP_ST_BALL);
    break;
  default:
    CORE;
  }
  switch (pop0->get_type()){
  case IVP_POLYGON:
    syn0->init_synapse(this, pop0,  e0, IVP_ST_POINT);
    pop0->get_surface_manager()->add_reference_to_ledge(e0->get_compact_ledge());
    break;
  case IVP_BALL:
    syn0->init_synapse(this, pop0,  e0, IVP_ST_BALL);
    break;
  default:
    CORE;
  }

  syn0->set_synapse_mindist(this);
  syn1->set_synapse_mindist(this);

  this->sum_extra_radius = pop0->get_extra_radius() + pop1->get_extra_radius();

  IVP_Mindist_Manager *mm = pop0->get_environment()->get_mindist_manager();
  	    
  if (pop0->get_controller_phantom() || pop1->get_controller_phantom() ){
    this->mindist_function = IVP_MF_PHANTOM;
    mm->insert_and_recalc_phantom_mindist(this);   // #+# insert only
  }else{
    mm->insert_and_recalc_exact_mindist(this);  // #+# insert only (data cache !!)
  }

}

class IVP_MM_CMP_Key {
public:
    const IVP_Real_Object *object[2];
    const IVP_Compact_Ledge *ledge[2];
};

class IVP_MM_CMP {
public:
    static inline int calc_hash_index( IVP_MM_CMP_Key * o){
	int x = (int)o->ledge[0] ^ ( int(o->ledge[1])* 75 );
	return x + 1023 * (x>>8);
    }

    // calc hash index of c, con is a reference
    static inline int calc_hash_index( IVP_Collision *c, IVP_MM_CMP_Key * /*ref_key*/){
	const IVP_Compact_Ledge *ledge[2];
	c->get_ledges(ledge);
	int x = (int)ledge[0] ^ ( int(ledge[1])* 75 );
	return x + 1023 * (x>>8);
    }

    static inline IVP_BOOL are_equal( IVP_Collision *c, IVP_MM_CMP_Key *search ){
	const IVP_Compact_Ledge *ledge[2];
	c->get_ledges(ledge);
	if (ledge[0] == search->ledge[0] && ledge[1] == search->ledge[1]){
#ifdef DEBUG
	    IVP_Real_Object *object[2];
	    c->get_objects( object );
	    IVP_ASSERT( object[0] == search->object[0]);
	    IVP_ASSERT( object[1] == search->object[1]);
#endif	    
	    return IVP_TRUE;
	}

	return IVP_FALSE;
    }
};

class IVP_Vector_of_Collisions_128 : public IVP_U_Vector<class IVP_Collision> {
    void *elem_buffer[128];
public:
    IVP_Vector_of_Collisions_128(): IVP_U_Vector<class IVP_Collision>(&elem_buffer[0],128){;};
};


/********************************************************************************
 *	Name:	  	create_exact_mindists    	
 *	Description:	Insert mindists, between all pairs of ledges of two objects
 *	scan_radius:	the radius, the surface_manager is searched
 *	mindists:		an optional hash of mindists, which is checked and updated
 * 	returns:	the last collision created
 ********************************************************************************/
void IVP_Mindist_Manager::create_exact_mindists(IVP_Real_Object *pop0, IVP_Real_Object *pop1,
						IVP_DOUBLE scan_radius, IVP_U_FVector<IVP_Collision> *mindists,
						const IVP_Compact_Ledge *single_ledge0, const IVP_Compact_Ledge *single_ledge1,
						const IVP_Compact_Ledge *root_ledge0, const IVP_Compact_Ledge *root_ledge1,
						IVP_Collision_Delegator *oo_watcher){
    // takes first point of each pop and adds a mindist
    IVP_Environment *env = pop0->get_environment();
    IVP_Vector_of_Ledges_256 ledges0;
    IVP_Vector_of_Ledges_256 ledges1;

    if (!single_ledge0){
	IVP_DOUBLE search_radius_0;
	IVP_U_Point visitor_position_os0;
	IVP_Core *core1 = pop1->get_core();
	IVP_U_Point pos1; core1->inline_calc_at_position( env->get_current_time(), &pos1 );
	search_radius_0 = core1->upper_limit_radius + pop0->get_extra_radius() + scan_radius;
	IVP_Cache_Object *cache0 = pop0->get_cache_object_no_lock();
	cache0->m_world_f_object.vimult4( &pos1, &visitor_position_os0 );
	pop0->get_surface_manager()->get_all_ledges_within_radius( &visitor_position_os0, search_radius_0 , root_ledge0, NULL, single_ledge1, &ledges0);
    }else{
	ledges0.add( & (IVP_Compact_Ledge &)*single_ledge0);
    }

    if (!single_ledge1) {
	IVP_DOUBLE search_radius_1;
	IVP_U_Point visitor_position_os1;
	IVP_Core *core0 = pop0->get_core();
	IVP_U_Point pos0; core0->inline_calc_at_position( env->get_current_time(), &pos0 );
	search_radius_1 = core0->upper_limit_radius + pop1->get_extra_radius() + scan_radius;
	IVP_Cache_Object *cache1 = pop1->get_cache_object_no_lock();
	cache1->m_world_f_object.vimult4( &pos0, &visitor_position_os1 );
	pop1->get_surface_manager()->get_all_ledges_within_radius( &visitor_position_os1, search_radius_1 , root_ledge1, NULL, single_ledge0, &ledges1);	
    }else{
	ledges1.add( & (IVP_Compact_Ledge &)*single_ledge1);
    }
    	

#ifdef IVP_HOME_BUILD
    // IVP_SUPREME_SUPPORT
    ivp_global_clustering_visualizer.shortrange.analyze_collision(pop0, search_radius_1, pop1, search_radius_0);
#endif

    short a_buffer[1024];
    int buffer_size = 1024;
    
    { // optimize buffersize in order to reduce memory fills at diff_hash
	int ref_size = ledges0.len() * ledges1.len() + mindists->len()+ 1;
	ref_size <<= 1;
	while ( buffer_size > ref_size) buffer_size >>= 1;
    }

    // convert old list into hash
    IVP_Diff_Hash<IVP_MM_CMP, IVP_U_FVector<IVP_Collision>,IVP_Collision, IVP_MM_CMP_Key> diff_hash( mindists, a_buffer, buffer_size, NULL);
    IVP_Vector_of_Collisions_128 new_collisions;
    
    for (int i= ledges0.len()-1; i>=0; i--){
	const IVP_Compact_Ledge *l0 = ledges0.element_at(i);
	const IVP_Compact_Triangle *tri0 = l0->get_first_triangle();
	IVP_MM_CMP_Key key; key.ledge[0] = l0; key.object[0] = pop0;
	
	for (int j = ledges1.len()-1;j>=0;j--){
	    const IVP_Compact_Ledge *l1 = ledges1.element_at(j);
	    key.ledge[1] = l1; key.object[1] = pop1;
	    
	    if ( diff_hash.check_element( &key, NULL )){
		continue;
	    }
	    
	    IVP_Mindist *new_mindist;
	    if ( !l0->is_terminal() || !l1->is_terminal()){
	      new_mindist = new IVP_Mindist_Recursive(env, oo_watcher);
	    }else{
	      new_mindist = new IVP_Mindist(env, oo_watcher);