ivp_mindist.cxx

hl2 source code. Do not use it illegal.

		if ( syn1->get_edge() == mdist->last_visited_triangle ) 
			continue;
		
		if( (syn1->get_status()==IVP_ST_TRIANGLE) && (syn0->get_status()==IVP_ST_BALL) ) 
		{
			IVP_FLOAT md_len = mdist->get_length();
			if( md_len < ivp_mindist_settings.max_distance_for_friction ) 
			{
				if ( syn1->get_ledge()->is_terminal() )
				{  
					mdist->create_cp_in_advance_pretension(syn0->l_obj,md_len);
					//ivp_message("wheel check mindist %x at %f\n", mdist,    environment->get_current_time().get_seconds());
					
					//wheel_look_ahead_mindists.remove_at_and_allow_resort( i );
					mdist->last_visited_triangle = syn1->get_edge();
				}
			}
		}
	}
}


/** recalculation of all exact mindist at the end of PSI */
void IVP_Mindist_Manager::recalc_all_exact_mindists(){
    IVP_Mindist *mdist, *mdist_next;
    for(mdist=this->exact_mindists; mdist; mdist=mdist_next){
	mdist_next = mdist->next;
	IVP_ASSERT(mdist->mindist_status == IVP_MD_EXACT);

	IVP_IF_PREFETCH_ENABLED(mdist_next){
	    IVP_PREFETCH_BLOCK(mdist_next,sizeof(*mdist_next));
	}
	this->recalc_exact_mindist(mdist);
    }
}

/* check length, check for hull, check for coll events*/
void IVP_Mindist::update_exact_mindist_events(IVP_BOOL allow_hull_conversion, IVP_MINDIST_EVENT_HINT event_hint)
{
	IVP_IF(1) 
	{
		IVP_ASSERT( mindist_status == IVP_MD_EXACT);
		IVP_Debug_Manager *dm=get_environment()->get_debug_manager();
		if(dm->file_out_impacts) 
		{
			fprintf(dm->out_deb_file,"doing_mindist_events %lx at %f\n",0x0000ffff&(long)this,get_environment()->get_current_time().get_time());
		}
	}
	
	IVP_Synapse_Real *syn0 = get_sorted_synapse(0);
	IVP_Synapse_Real *syn1 = get_sorted_synapse(1);
	IVP_Core *core0 = syn0->get_core();
	IVP_Core *core1 = syn1->get_core();
	IVP_Environment *env = get_environment();
	
	// speed is speed toward each other
	IVP_Mindist_Event_Solver mim;
	{
		mim.sum_max_surface_rot_speed = core0->max_surface_rot_speed + core1->max_surface_rot_speed;
		mim.worst_case_speed = mim.sum_max_surface_rot_speed + core0->current_speed + core1->current_speed;
	}
	
	if (this->index != IVP_U_MINLIST_UNUSED)
	{
		IVP_Time_Manager *time_manager = env->get_time_manager();
		time_manager->remove_event(this);
		this->index = IVP_U_MINLIST_UNUSED;
	}
	
	{  
		// check for hull
		IVP_DOUBLE coll_dist = this->get_coll_dist();
		IVP_DOUBLE hull_check_len = coll_dist + env->get_delta_PSI_time() * mim.worst_case_speed * 2.1f;
		IVP_Mindist_Manager *my_manager = env->get_mindist_manager();
		
		if( get_length() > hull_check_len)
		{
			if (allow_hull_conversion)
			{
				// check if hull survives at least some psi's
				my_manager->remove_exact_mindist(this);	
				my_manager->insert_hull_mindist( this, get_length() - coll_dist ); // secdist is subtracted, because hull manager is already checked till next PSI
			}
			return;
		}
	}
	
	{
		mim.projected_center_speed =  core1->speed.dot_product(&contact_plane) - core0->speed.dot_product(&contact_plane);	// inverse to direction
		
		IVP_DOUBLE rot0_projected = contact_plane.dot_product( &core0->rotation_axis_world_space);
		rot0_projected *= rot0_projected;
		rot0_projected = IVP_Inline_Math::ivp_sqrtf(1.001f - rot0_projected) * core0->max_surface_rot_speed;	// make sure sqrt does not crash -> increase result
		
		IVP_DOUBLE rot1_projected = contact_plane.dot_product( &core1->rotation_axis_world_space);
		rot1_projected *= rot1_projected;
		rot1_projected = IVP_Inline_Math::ivp_sqrtf(1.001f - rot1_projected) * core1->max_surface_rot_speed;
		mim.max_coll_speed = rot0_projected + rot1_projected + mim.projected_center_speed;
		mim.t_now =  env->get_current_time();
		mim.t_max =  env->get_next_PSI_time();
	}
	
	IVP_DOUBLE len = get_length();
	
	if( mim.max_coll_speed < ivp_mindist_settings.speed_after_keeper_dist ) 
	{	
		if( len < ivp_mindist_settings.keeper_dist ) 
		{
			//mim.flag_near_slow_impact = IVP_TRUE;
		}
		if (mim.max_coll_speed < P_DOUBLE_EPS)
		{
			return; // no movement towards each other!!
		}
	}
	
	IVP_DOUBLE d_time = mim.t_max - mim.t_now;
	IVP_DOUBLE d_len = d_time * mim.max_coll_speed;	// max length that the system can move till next PSI
	IVP_DOUBLE sec_dist = this->get_coll_dist() + d_len;
	
	IVP_IF(ivp_check_debug_mindist(this))
	{
		ivp_message("%32s  event  len_now %g, sec_dist:%g\n","update_exact_mindist_events",  get_length(), sec_dist);
	}
	
	if (len >= sec_dist)
	{  
		// no collision possible
		return;
	}
	
	// real check next collision time
	if (this->mindist_function == IVP_MF_PHANTOM) 
	{
		// no deep checking for phantoms //@@CB
		return;
	} 
	
	mim.mindist = this;
	mim.environment = env;
	
	/* ************** calc the time of the next event ******************* */
	/* ************** calc the time of the next event ******************* */
	/* ************** calc the time of the next event ******************* */
	if (coll_dist_selector > 0)
	{ 
		// decrease collision distance
		static int count = 0;
		if (count ++ > 2)
		{
			coll_dist_selector-=1;
			count = 0;
		}
	}
	mim.calc_time_of_next_event();
	
	IVP_Time &next_event = mim.event_time_out;
	
	IVP_IF(ivp_check_debug_mindist(this))
	{
		ivp_message("%32s     calc_time_of_next_event called: ","''");
		if (mim.event_type_out != IVP_COLL_NONE)		
			ivp_message("event at %f\n", next_event.get_time());
		else		
			ivp_message("no\n");
	}
	
	if (mim.event_type_out != IVP_COLL_NONE)
	{
		if ( next_event - mim.t_now < P_FLOAT_RES)
		{	// maybe an error
			if (event_hint == IVP_EH_NOW)
			{
				next_event = mim.t_now;
			}
			else
			{
				// check if we can do recalc  right now
				// only if allowed and event is a coll event
				IVP_DOUBLE mdist = len - ivp_mindist_settings.real_coll_dist;
				if ( (event_hint == IVP_EH_BIG_DELAY) || (mim.event_type_out & 0x0f)) 
				{	// 
					if (mdist >= P_DOUBLE_RES)
					{
						next_event = env->get_current_time() + mdist/mim.worst_case_speed + 
							(ivp_mindist_settings.event_queue_min_delta_time_base * 1e-4f) * env->get_delta_PSI_time(); //@@CB
					}
					else
					{
						next_event = env->get_current_time() + (ivp_mindist_settings.event_queue_min_delta_time_base * 1e-3f) 
							* env->get_delta_PSI_time(); //@@CB
					}
				}
				else
				{
					if (mdist >= P_DOUBLE_RES)
					{
						next_event = env->get_current_time() + 0.1f * mdist/mim.worst_case_speed 
							+ (ivp_mindist_settings.event_queue_min_delta_time_base * 1e-7f) * env->get_delta_PSI_time(); //@@CB
					}
					else
					{
						next_event = env->get_current_time() + (ivp_mindist_settings.event_queue_min_delta_time_base * 1e-5f) 
							* env->get_delta_PSI_time(); //@@CB
					}
				}

				if (next_event - env->get_next_PSI_time() >= 0.0f) 
					return;
			}
		}
		
		// insert real event in task manager
		IVP_Time_Manager *tm = env->get_time_manager();
		
		if (0)
		{	
			tm->insert_event(this,next_event);
		}
		else
		{
			IVP_FLOAT event_time = next_event - tm->base_time;
			this->index = tm->min_hash->add((void *)this, event_time);
		}
		
		IVP_ASSERT(this->index !=IVP_U_MINLIST_UNUSED);
		
		this->coll_type = mim.event_type_out;
	}
}

void IVP_Synapse::hull_manager_is_reset(IVP_FLOAT dt, IVP_FLOAT center_dt){  // hull_manager is reset
    IVP_Mindist *md = (IVP_Mindist *) this->get_synapse_mindist();
    md->hull_manager_is_reset(dt,center_dt);
}

void IVP_Synapse::hull_limit_exceeded_event(IVP_Hull_Manager *, IVP_HTIME hull_intrusion_value){
    IVP_Mindist *md = (IVP_Mindist *) this->get_synapse_mindist();
    md->mindist_hull_limit_exceeded_event(hull_intrusion_value);
}

void IVP_Synapse::hull_manager_is_going_to_be_deleted_event(IVP_Hull_Manager *){

    delete this->get_synapse_mindist(); //dont use P_DELETE, because mindist deletes synapse and P_DELETE sets pointer to NULL
}

void ivp_optimiztion_center_check_successfull(){
  ;
}

void IVP_Mindist::hull_manager_is_reset(IVP_FLOAT dt, IVP_FLOAT center_dt){  // hull_manager is reset
#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED
    sum_angular_hull_time += dt - center_dt;
#endif
}


void IVP_Mindist::mindist_hull_limit_exceeded_event( IVP_HTIME hull_intrusion_value){
    if (mindist_status == IVP_MD_HULL_RECURSIVE){
	IVP_Mindist_Recursive *mr = (IVP_Mindist_Recursive *)this;
	mr->rec_hull_limit_exceeded_event();
	return;
    }
    IVP_ASSERT(mindist_status == IVP_MD_HULL );

    IVP_Synapse_Real *syn0 = get_sorted_synapse(0);
    IVP_Synapse_Real *syn1 = get_sorted_synapse(1);


    IVP_Real_Object *obj0 = syn0->get_object();
    IVP_Real_Object *obj1 = syn1->get_object();
    IVP_Hull_Manager *m0 = obj0->get_hull_manager();
    IVP_Hull_Manager *m1 = obj1->get_hull_manager();

    IVP_Core *core0 = obj0->get_core();
    IVP_IF_PREFETCH_ENABLED(IVP_TRUE){
        m0->prefetch0_hull();
	IVP_PREFETCH(&core0, &((IVP_Core *)0)->current_speed);
	IVP_PREFETCH(core0->get_position_PSI(),0);
    }

    IVP_Core *core1 = obj1->get_core();
    IVP_IF_PREFETCH_ENABLED(IVP_TRUE){
        m1->prefetch0_hull();
	IVP_PREFETCH(&core1,&((IVP_Core *)0)->current_speed);
	IVP_PREFETCH(core1->get_position_PSI(),0);
    }

    IVP_Time current_time = obj0->get_environment()->get_current_time();
    IVP_U_Point center0; core0->inline_calc_at_position( current_time, &center0 );
    IVP_U_Point center1; core1->inline_calc_at_position( current_time, &center1 );

    IVP_U_Point diff; diff.subtract( &center0, &center1 );
    IVP_DOUBLE qdistance  = diff.quad_length();

    IVP_DOUBLE speed0 = core0->current_speed + core0->max_surface_rot_speed + P_DOUBLE_EPS; // 
    IVP_DOUBLE speed1 = core1->current_speed + core1->max_surface_rot_speed + P_DOUBLE_EPS;
    IVP_DOUBLE speed = speed0 + speed1;

    IVP_IF(ivp_check_debug_mindist(this)){
	ivp_message("%32s center_distance %f old_dist %f\n","hull_limit_exceeded_event",  IVP_Inline_Math::ivp_sqrtf(qdistance), get_length());
    }

    m0->prefetch1_hull();
    m1->prefetch1_hull();

#if defined(IVP_HALFSPACE_OPTIMIZATION_ENABLED)
    // fast check projected centers
    if (!disable_halfspace_optimization){
      // calc real center movement
      IVP_DOUBLE new_contact_dot_diff_center = diff.dot_product( &contact_plane );
      IVP_DOUBLE delta_center_distance = contact_dot_diff_center - new_contact_dot_diff_center;

      // calc estimated angular movement
      IVP_DOUBLE current_angular_hull_time_0 = m0->get_angular_hull_time(current_time);
      IVP_DOUBLE current_angular_hull_time_1 = m1->get_angular_hull_time(current_time);

      IVP_DOUBLE sum_angular = current_angular_hull_time_0 + current_angular_hull_time_1;
      IVP_DOUBLE angular_movement = sum_angular - this->sum_angular_hull_time;

      IVP_DOUBLE estimated_distance = this->get_length() - angular_movement - delta_center_distance + hull_intrusion_value;
      // this estimated_distance can now be used to throw mindist into hull again
      IVP_DOUBLE const look_ahead_factor = 6.0f;
	IVP_IF( 0 && this->contact_plane.k[1] == -1.0f && !this->mindist_function){
	    ivp_message("sum_angular %f   angular_movement %f    sum_angular_hull_time %f\n", 
		sum_angular, angular_movement, sum_angular_hull_time);
	}

      
      if ( estimated_distance > speed * obj0->get_environment()->get_delta_PSI_time() * look_ahead_factor){
	// successfull delay
#ifdef LINUX
	ivp_optimiztion_center_check_successfull();
#endif
	IVP_IF(ivp_check_debug_mindist(this)){
	  ivp_message("%32s estimated_distance into hull dist: %f error %f speed %f\n","hull_limit_exceeded_event",  IVP_Inline_Math::ivp_sqrtf(qdistance),estimated_distance,speed);
	}

        this->sum_angular_hull_time = sum_angular;
	this->contact_dot_diff_center = new_contact_dot_diff_center;
	this->len_numerator = estimated_distance - hull_intrusion_value;

	IVP_DOUBLE dist_factor = estimated_distance / speed;
	IVP_DOUBLE dist0 = dist_factor * speed0;
	IVP_DOUBLE dist1 = dist_factor * speed1;
	IVP_IF( 0 &&  this->contact_plane.k[1] == -1.0f && !this->mindist_function){
	    ivp_message("est_dist %f, center_dist %f   sum_angular %f\n", 
		len_numerator, delta_center_distance, sum_angular);
	}

	
	syn0->get_hull_manager()->update_synapse(syn0,current_time, dist0);
	syn1->get_hull_manager()->update_synapse(syn1,current_time, dist1);
	return;
      }
    }
#endif
    
    // fast check sphere first
    if (0){ // check for small objects
      IVP_FLOAT radius = core0->upper_limit_radius + core1->upper_limit_radius;
      IVP_FLOAT qradius = radius * radius;

      if ( qdistance > qradius){ // seems like two small objects

	const IVP_DOUBLE look_ahead_time = 0.5f;
	IVP_DOUBLE hull_radius = radius + speed * look_ahead_time;
	qradius = hull_radius * hull_radius;

	if ( qdistance > qradius){ // seems like two small objects
	  IVP_DOUBLE distance = IVP_Inline_Math::ivp_sqrtf( qdistance)  - radius;
	  IVP_IF(ivp_check_debug_mindist(this)){
	    ivp_message("%32s center_distance %f  speed %f\n","hull_limit_exceeded_event",  distance, speed);
	  }
	 
	  IVP_DOUBLE dist0 = distance * 0.5f;
	  IVP_DOUBLE dist1 = distance * 0.5f;
	  IVP_Time t_now = obj0->get_environment()->get_current_time();
	  syn0->get_hull_manager()->update_synapse(syn0,t_now, dist0);
	  syn1->get_hull_manager()->update_synapse(syn1,t_now, dist1);
	  return;
	}
      }
    }
    
    IVP_Mindist_Manager *mgr = obj0->get_environment()->get_mindist_manager();
    mgr->remove_hull_mindist(this);

    if ( this->mindist_function != IVP_MF_PHANTOM){  // #+# just insert in list and wait for delayed update
      mgr->insert_and_recalc_exact_mindist(this);
    }else{
      mgr->insert_and_recalc_phantom_mindist(this); //@@CB
    }
}

void IVP_Mindist::create_cp_in_advance_pretension(IVP_Real_Object *robject,float gap_len) {
    IVP_Synapse_Friction *old_fr_syn = robject->get_first_friction_synapse();
    if(old_fr_syn) {
        robject->environment->sim_unit_mem->start_memory_transaction();

	    IVP_Friction_System *associated_fs;
	    IVP_BOOL success;
	    IVP_Simulation_Unit *sim_unit_not_destroy=NULL;
	    IVP_BOOL call_recalc_svals=IVP_TRUE;

            IVP_Contact_Point *new_cp=try_to_generate_managed_friction(&associated_fs,&success,sim_unit_not_destroy,call_recalc_svals);


	    if(success==IVP_TRUE) {
			IVP_Contact_Point *old_cp=old_fr_syn->get_contact_point();
			IVP_Time old_time = old_cp->last_time_of_recalc_friction_s_vals;
			old_cp->last_time_of_recalc_friction_s_vals = robject->environment->get_current_time();
			old_cp->recalc_friction_s_vals();

			new_cp->now_friction_pressure = old_cp->now_friction_pressure;