ivp_mindist.cxx

hl2 source code. Do not use it illegal.

			old_cp->now_friction_pressure = 0.0f;
			new_cp->last_gap_len = gap_len;
			new_cp->last_time_of_recalc_friction_s_vals = old_time;
			old_cp->last_time_of_recalc_friction_s_vals = old_time;

			IVP_U_Float_Point world_friction_vec;
			world_friction_vec.set_multiple(&old_cp->tmp_contact_info->span_friction_v[0],old_cp->span_friction_s[0]);
			world_friction_vec.add_multiple(&old_cp->tmp_contact_info->span_friction_v[1],old_cp->span_friction_s[1]);

			float s1,s2;
			s1=new_cp->tmp_contact_info->span_friction_v[0].dot_product(&world_friction_vec);
			s2=new_cp->tmp_contact_info->span_friction_v[1].dot_product(&world_friction_vec);
			new_cp->span_friction_s[0]= s1;
			new_cp->span_friction_s[1]= s2;
	    }

        robject->environment->sim_unit_mem->end_memory_transaction();
	}
}

void IVP_Mindist::simulate_time_event(IVP_Environment * env)
{
#ifdef IVP_ENABLE_PERFORMANCE_COUNTER
	env->get_performancecounter()->pcount(IVP_PE_AT_INIT);
#endif

    IVP_ASSERT( index == IVP_U_MINLIST_UNUSED );
    IVP_Mindist *l_mindist = this;
    
    // update and check wether announced situation change really has occurred
     IVP_IF(ivp_check_debug_mindist(l_mindist)){
	 IVP_DOUBLE md_len = l_mindist->get_length();
	 ivp_message("%32s COLL_TYPE %X: old_len=%g\n", "Time_Event_Mindist_Coll", l_mindist->coll_type, md_len);
     }

     l_mindist->recalc_mindist();	// calculate the current situation, no phantoms
     IVP_ASSERT( l_mindist->mindist_function != IVP_MF_PHANTOM);

    // what situation is announced ?
     IVP_IF(ivp_check_debug_mindist(l_mindist)){
	 IVP_DOUBLE md_len = l_mindist->get_length();
	 ivp_message("%32s COLL_TYPE %X: current_len=%g\n", "Time_Event_Mindist_Coll", l_mindist->coll_type, md_len);
     }
    
     if (l_mindist->recalc_result == IVP_MDRR_OK){ 
       if ( (l_mindist->coll_type & 0x0f) == 0){		// real collision might have happened
	 IVP_ASSERT(l_mindist->mindist_status == IVP_MD_EXACT);
       
	 IVP_FLOAT md_len = l_mindist->get_length();
	 IVP_FLOAT sec_dist = this->get_coll_dist() + ivp_mindist_settings.mindist_change_force_dist;
	 if(md_len < sec_dist){
	   // mindist shorter than this -> impact
       //ivp_message("do impact %x at %f\n", this, this->get_synapse(0)->get_object()->get_environment()->get_current_time().get_seconds());

	   l_mindist->do_impact();
	 }else{
    	    l_mindist->update_exact_mindist_events(IVP_FALSE, IVP_EH_BIG_DELAY);	// recalculate mindist, no hull conversion
	 }
       }else{
	     l_mindist->update_exact_mindist_events(IVP_FALSE, IVP_EH_SMALL_DELAY);	// recalculate mindist, no hull conversion
       }
     }

#ifdef IVP_ENABLE_PERFORMANCE_COUNTER
	env->get_performancecounter()->pcount(IVP_PE_AT_END);
#endif
    /*
     IVP_FLOAT md_len = l_mindist->get_length();
     
     if( md_len < ivp_mindist_settings.max_distance_for_friction ) {
	    IVP_Synapse *syn1=&l_mindist->synapse[0];
	    IVP_Synapse *syn2=&l_mindist->synapse[1];
	    if(syn1->get_status()!=IVP_ST_BALL) {
		    IVP_Synapse *temp=syn1;    syn1=syn2;    syn2=temp;
	    }	    
//check ball / triangle
	    if( (syn2->get_status()==IVP_ST_TRIANGLE)&&(syn1->get_status()==IVP_ST_BALL) ) {
	    if( syn1->l_obj->get_core()->car_wheel ) {
		    create_cp_in_advance_pretension(syn1->l_obj,md_len);
		    }
	    }
     }
   */
}



/*****************************************************************/
/*****************************************************************/
/*****************************************************************/
/*****************************************************************/
/*****************************************************************/
/*****************************************************************/

void IVP_Mindist_Manager::remove_exact_mindist(IVP_Mindist *del_mindist)
{
	if (del_mindist->index != IVP_U_MINLIST_UNUSED)
	{
		IVP_Time_Manager *time_manager = environment->get_time_manager();
		time_manager->remove_event(del_mindist);
		del_mindist->index = IVP_U_MINLIST_UNUSED;
	}
	
	IVP_ASSERT(del_mindist->mindist_status == IVP_MD_EXACT); // requires friction mode
	IVP_IF(1)
	{
		del_mindist->mindist_status = IVP_MD_UNINITIALIZED;
	}
	// de-link mindist from manager
	if(del_mindist->next)
	{
		del_mindist->next->prev = del_mindist->prev;
	}
	if(del_mindist->prev)
	{
		del_mindist->prev->next = del_mindist->next;
	}
	else
	{
		this->exact_mindists = del_mindist->next;
	}
	
	del_mindist->get_synapse(0)->remove_exact_synapse_from_object();
	del_mindist->get_synapse(1)->remove_exact_synapse_from_object();
	
	int i = wheel_look_ahead_mindists.index_of( del_mindist );
	//ivp_message("exact mindist removed %x at %f index %i\n", del_mindist, environment->get_current_time().get_seconds(), i);
	if (i>=0)
	{
		wheel_look_ahead_mindists.remove_at_and_allow_resort(i);
	}
}

void IVP_Mindist_Manager::remove_invalid_mindist(IVP_Mindist *del_mindist)
{
	IVP_ASSERT(del_mindist->mindist_status == IVP_MD_INVALID); // requires friction mode
	
	IVP_IF(1)
	{
		del_mindist->mindist_status = IVP_MD_UNINITIALIZED;
	}
	// de-link mindist from manager
	if(del_mindist->next)
	{
		del_mindist->next->prev = del_mindist->prev;
	}
	if(del_mindist->prev)
	{
		del_mindist->prev->next = del_mindist->next;
	}
	else
	{
		this->invalid_mindists = del_mindist->next;
	}
	del_mindist->get_synapse(0)->remove_invalid_synapse_from_object();
	del_mindist->get_synapse(1)->remove_invalid_synapse_from_object();
}

void IVP_Mindist_Manager::remove_hull_mindist(IVP_Mindist *del_mindist)
{
	IVP_ASSERT(del_mindist->mindist_status == IVP_MD_HULL || del_mindist->mindist_status == IVP_MD_HULL_RECURSIVE);
	
	IVP_IF(1)
	{
		del_mindist->mindist_status = IVP_MD_UNINITIALIZED;
	}
	IVP_Synapse_Real *syn0 = del_mindist->get_synapse(0);
	IVP_Synapse_Real *syn1 = del_mindist->get_synapse(1);
	syn0->get_hull_manager()->remove_synapse(syn0);
	syn1->get_hull_manager()->remove_synapse(syn1);
}

void IVP_Mindist_Manager::insert_hull_mindist(IVP_Mindist *md, IVP_HTIME hull_time0, IVP_HTIME hull_time1)
{
	IVP_ASSERT(md->mindist_status == IVP_MD_UNINITIALIZED);
	md->mindist_status = IVP_MD_HULL;
	
	IVP_Synapse_Real *syn0 = md->get_synapse(0);
	IVP_Synapse_Real *syn1 = md->get_synapse(1);
	
#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED	
	IVP_DOUBLE center0 = syn0->insert_in_hull_manager(hull_time0);
	IVP_DOUBLE center1 = syn1->insert_in_hull_manager(hull_time1);
	md->sum_angular_hull_time = center0 + center1;
#else
	syn0->insert_in_hull_manager(hull_time0);
	syn1->insert_in_hull_manager(hull_time1);
#endif
}

void IVP_Mindist_Manager::insert_hull_mindist(IVP_Mindist *md, IVP_HTIME hull_time)
{
	IVP_Real_Object *obj0 = md->get_synapse(0)->l_obj;
	IVP_Real_Object *obj1 = md->get_synapse(1)->l_obj;
	
	if ( !IVP_MTIS_IS_MOVING(obj0->get_movement_state()))
	{
		insert_hull_mindist( md, 0.0f, hull_time );
	}
	else if ( !IVP_MTIS_IS_MOVING(obj1->get_movement_state()))
	{
		insert_hull_mindist( md, hull_time, 0.0f );
	}
	else
	{
		IVP_Core *core0 = obj0->get_core();
		IVP_Core *core1 = obj1->get_core();
		
		IVP_FLOAT speed0 = core0->max_surface_rot_speed + core0->current_speed + P_FLOAT_EPS;
		IVP_FLOAT speed1 = core1->max_surface_rot_speed + core1->current_speed + P_FLOAT_EPS;
		IVP_FLOAT speed_corr_0 = speed0 + 0.1f * speed1;
		IVP_FLOAT speed_corr_1 = speed1 + 0.1f * speed0;
		IVP_FLOAT sum_speed = speed_corr_0 + speed_corr_1;
		IVP_FLOAT factor = hull_time / sum_speed;
		insert_hull_mindist( md, factor * speed_corr_0, factor * speed_corr_1 );
	}
}

void IVP_Mindist_Manager::insert_lazy_hull_mindist(IVP_Mindist *md, IVP_HTIME hull_time0, IVP_HTIME hull_time1)
{
	IVP_ASSERT(md->mindist_status == IVP_MD_UNINITIALIZED);
	md->mindist_status = IVP_MD_HULL;
	
	md->get_synapse(0)->insert_lazy_in_hull_manager(hull_time0);
	md->get_synapse(1)->insert_lazy_in_hull_manager(hull_time1);
}


void IVP_Mindist_Manager::insert_lazy_hull_mindist(IVP_Mindist *md, IVP_HTIME hull_time)
{
	IVP_Real_Object *obj0 = md->get_synapse(0)->l_obj;
	IVP_Real_Object *obj1 = md->get_synapse(1)->l_obj;
	
	if ( !IVP_MTIS_IS_MOVING(obj0->get_movement_state()))
	{
		insert_lazy_hull_mindist( md, P_FLOAT_EPS, hull_time );
		IVP_IF(ivp_check_debug_mindist(md))
		{
			ivp_message("%32s into hull, htimes %f: %f\n","insert_lazy_hull_mindist", P_DOUBLE_EPS, hull_time);
		}
	}
	else if ( !IVP_MTIS_IS_MOVING(obj1->get_movement_state()))
	{
		insert_lazy_hull_mindist( md, hull_time, P_FLOAT_EPS );
		IVP_IF(ivp_check_debug_mindist(md))
		{
			ivp_message("%32s into hull, htimes %f: %f\n","insert_lazy_hull_mindist", hull_time, P_DOUBLE_EPS);
		}
	}
	else
	{
		IVP_Core *core0 = obj0->get_core();
		IVP_Core *core1 = obj1->get_core();
		
		IVP_FLOAT speed0 = core0->max_surface_rot_speed + core0->current_speed + P_FLOAT_EPS;
		IVP_FLOAT speed1 = core1->max_surface_rot_speed + core1->current_speed + P_FLOAT_EPS;
		IVP_FLOAT speed_corr_0 = speed0 + 0.1f * speed1;
		IVP_FLOAT speed_corr_1 = speed1 + 0.1f * speed0;
		IVP_FLOAT sum_speed = speed_corr_0 + speed_corr_1;
		IVP_FLOAT factor = hull_time / sum_speed;
		insert_lazy_hull_mindist( md, factor * speed_corr_0, factor * speed_corr_1 );
		
		IVP_IF(ivp_check_debug_mindist(md))
		{
			ivp_message("%32s into hull, htimes %f: %f\n","insert_lazy_hull_mindist", factor * speed_corr_0, factor * speed_corr_1);
		}
	}
}


void IVP_Mindist_Manager::mindist_entered_phantom(IVP_Mindist *mdist){ //@@CB
  if (! mdist->is_in_phantom_set ){
    mdist->is_in_phantom_set = IVP_TRUE;
#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED
    mdist->disable_halfspace_optimization = IVP_TRUE;
#endif    
    {
      IVP_Real_Object* obj0 = mdist->get_synapse(0)->get_object();
      {
	  IVP_Controller_Phantom * cp0 = obj0->get_controller_phantom();
	  if (cp0)  cp0->mindist_entered_volume(mdist);
      }
    }
    {
      IVP_Real_Object* obj1 = mdist->get_synapse(1)->get_object();
      {
	  IVP_Controller_Phantom * cp1 = obj1->get_controller_phantom();
	  if (cp1)  cp1->mindist_entered_volume(mdist);
      }
    }
  }
}

void IVP_Mindist_Manager::mindist_left_phantom(IVP_Mindist *mdist){
  if (mdist->is_in_phantom_set ){
	    
#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED
      mdist->disable_halfspace_optimization = IVP_FALSE;
#endif      
    mdist->is_in_phantom_set = IVP_FALSE;
    {
      IVP_Synapse_Real *syn0 = mdist->get_synapse(0);
      IVP_Controller_Phantom * cp0 = syn0->get_object()->get_controller_phantom();
      if (cp0)  cp0->mindist_left_volume(mdist);
    }
    {
      IVP_Synapse_Real *syn1 = mdist->get_synapse(1);
      IVP_Controller_Phantom * cp1 = syn1->get_object()->get_controller_phantom();
      if (cp1)  cp1->mindist_left_volume(mdist);
    }
  }

}

IVP_Mindist_Manager::IVP_Mindist_Manager(IVP_Environment *i_env)
{
    P_MEM_CLEAR(this);
    this->environment = i_env;
}

IVP_Mindist_Manager::~IVP_Mindist_Manager() {
    IVP_Mindist *mdist, *mdist_next;
    
    for(mdist=this->exact_mindists; mdist; mdist=mdist_next){
	mdist_next = mdist->next;
	P_DELETE(mdist);
    }

    for(mdist=this->invalid_mindists; mdist; mdist=mdist_next){
	mdist_next = mdist->next;
	P_DELETE(mdist);
    }
}



void IVP_Mindist_Manager::recalc_exact_mindist( IVP_Mindist *mdist){
      mdist->recalc_mindist();	
      if (mdist->mindist_function == IVP_MF_COLLISION){	
	if (mdist->recalc_result == IVP_MDRR_OK ){
	  return;
	}
	mdist->exact_mindist_went_invalid(this);
	return;
      }

      // mindist function is IVP_MF_PHANTOM
      IVP_FLOAT uncertanty;

      if (mdist->recalc_result == IVP_MDRR_OK){
	if (mdist->get_length() > 0.0f){
	  if (mdist->is_in_phantom_set ){
	    this->mindist_left_phantom(mdist);
	  }
	  return;
	}
	uncertanty =   - mdist->get_length();
      }else{
	uncertanty =   mdist->sum_extra_radius;
      }

      // mindist has entered phantom
      IVP_Real_Object* obj0 = mdist->get_synapse(0)->get_object();
      IVP_Real_Object* obj1 = mdist->get_synapse(1)->get_object();

      // early exit for phantom / phantom
      if(obj0->get_controller_phantom() && obj1->get_controller_phantom())
      {
	return;
      }

      if(obj0->get_controller_phantom())
      {
        uncertanty += obj0->get_controller_phantom()->exit_policy_extra_radius;
      }
      else
      {
        uncertanty += obj1->get_controller_phantom()->exit_policy_extra_radius;
      }
      
      if(!mdist->is_recursive())
      {
	// early exit for phantom / convex test
	if (! mdist->is_in_phantom_set )
	{
	    this->mindist_entered_phantom(mdist);
	    this->remove_exact_mindist(mdist);
	    this->insert_lazy_hull_mindist( mdist, uncertanty);
	}
      } else {
#ifdef IVP_PHANTOM_FULL_COLLISION
        //obj1 has a root convex hull
	mdist->exact_mindist_went_invalid(this);
#else
	// early exit for phantom / convex hull test
	if (! mdist->is_in_phantom_set )
	{
	    this->mindist_entered_phantom(mdist);
	    this->remove_exact_mindist(mdist);
	    this->insert_lazy_hull_mindist( mdist, uncertanty);
	}		
#endif
      }
}