ivp_core.cxx

hl2 source code. Do not use it illegal.

		new_m_object_f_core.orthonormize();
		obj->set_new_m_object_f_core(&new_m_object_f_core);
		ccore->objects.add(obj);
		obj->physical_core = ccore;
	    }
	}
    }


    // set pointers 
    this->merged_core_which_replace_this_core = ccore;
    other_core->merged_core_which_replace_this_core = ccore;  
}



//////// split core and activate friction core !!!!!!!
void IVP_Core_Collision::split_collision_merged_core_next_PSI(){
    if (this->merged_core_which_replace_this_core != NULL) return;	// already touched
    
    return; //@@@@@
    // set original friction core speeds and position
#if 0
    {
	for(int c = objects.len()-1;c>=0;c--){
		IVP_Real_Object *obj=this->objects.element_at(c);
		IVP_USE(obj);
	}
    }
    //this is old unused code
    
    IVP_Core_Sim_Manager *sim_man = environment->get_core_sim_manager();
    sim_man->remove_sim_core(this);
    CORE
    IVP_Core *old_core = NULL;
    for (obj_search = this->first_object; obj_search; obj_search = next_obj){
	next_obj = obj_search->next_in_core;
	if (obj_search->friction_core == old_core) continue;		// search new core
	old_core = obj_search->friction_core;

	//// loop over all original cores
	IVP_U_Matrix m_CORE_f_core;
	old_core->set_matrizes_and_speed(this, &m_CORE_f_core);		// copy values from this
	old_core->merged_core_which_replace_this_core = NULL;
	
	IVP_Real_Object *obj;
	// search end of list
	for ( obj = obj_search; obj->next_in_core && obj->next_in_core->friction_core == old_core; obj = obj->next_in_core);

	// split linked lists
	if (obj->next_in_core) obj->next_in_core->prev_in_core = NULL;
	obj->next_in_core = NULL;

	// reset core coordinates of objects
	{
	    for (obj = old_core->first_object; obj; obj=obj->next_in_core){
		
		IVP_U_Matrix m_object_f_CORE;
		obj->m_object_f_core.mmult4( &m_CORE_f_core, &m_object_f_CORE);
		m_object_f_CORE.orthonormize();
		obj->set_new_m_object_f_core(&m_object_f_CORE);
		obj->physical_core = old_core;
	    }
	}

	//
	sim_man->add_sim_core(old_core);
    }
#endif    
}

void IVP_Core::set_matrizes_and_speed(IVP_Core_Merged *template_core, IVP_U_Matrix *m_CORE_f_core_out){

    /************** for all old cores do: ***********/
    // Note: Convention: all matrixnames for current time are in capitel letters
    
    IVP_ASSERT( template_core->merged_core_which_replace_this_core == NULL);
    
    ///// sum up m_world_now_f_world_when_merged
    IVP_U_Matrix m_WORLD_f_world;
	    // check for hierarchy and optimize when no hierarchy is given
    if (template_core == this->merged_core_which_replace_this_core){
	template_core->m_world_f_core_last_psi.mi2mult4( &template_core->m_world_f_core_when_created,
							 &m_WORLD_f_world);
    }else{
	m_WORLD_f_world.init();
	IVP_Core_Merged *core;
	for (core = this->merged_core_which_replace_this_core;
	     core;
	     core = this->merged_core_which_replace_this_core){
	    IVP_U_Matrix m_WORLD_f_world_one_core;
	    core->m_world_f_core_last_psi.mi2mult4( &core->m_world_f_core_when_created,
						    &m_WORLD_f_world_one_core);
	    m_WORLD_f_world_one_core.mmult4( & m_WORLD_f_world,
					     & m_WORLD_f_world);
	}
    }
    m_WORLD_f_world.orthonormize();
    
    /// calc m_CORE_f_core
    IVP_U_Matrix m_CORE_f_core;
    {
	IVP_U_Matrix m_WORLD_f_core;
	m_WORLD_f_world.mmult4( &this->m_world_f_core_last_psi,
				&m_WORLD_f_core);
	m_world_f_core_last_psi.mimult4( &m_WORLD_f_core,
					 &m_CORE_f_core);
    }
    *m_CORE_f_core_out = m_CORE_f_core;    
    
    // set matrizes
    m_WORLD_f_world.mmult4( &m_world_f_core_last_psi, &m_world_f_core_last_psi);

    /// set speed + rot_speed
    this->speed.set(&template_core->speed);
    IVP_U_Float_Point rot_speed_world;
    template_core->m_world_f_core_last_psi.vmult3(&template_core->speed, &rot_speed_world);
    this->m_world_f_core_last_psi.vimult3(&rot_speed_world, & this->rot_speed);
}


IVP_Core_Collision::IVP_Core_Collision(IVP_Core *core0, IVP_Core *core1)
    : IVP_Core_Merged(core0, core1){
	next_collision_core = NULL;
}


    
// update all mindist between objects of this core and all other objects with
// invalid mindist_event_already_done
// + sets this mindist_event_already_done
void IVP_Core::update_exact_mindist_events_of_core(){
    this->mindist_event_already_done = environment->mindist_event_timestamp_reference; //flag in core to avoid IVP_DOUBLE calculating
    for(int c = objects.len()-1;c>=0;c--){
	IVP_Real_Object *obj=this->objects.element_at(c);
	obj->update_exact_mindist_events_of_object();
    }
}

void IVP_Core::set_mass(IVP_FLOAT new_mass){
  IVP_DOUBLE factor = new_mass/ this->get_mass();
  IVP_U_Float_Hesse *ri = (IVP_U_Float_Hesse *)get_rot_inertia();
  ri->mult( factor );
  ri->hesse_val = new_mass;
  this->calc_calc();
}

void IVP_Core::core_plausible_check() {
    IVP_DOUBLE rot_change_len,trans_change_len;
    rot_change_len=rot_speed_change.real_length();
    trans_change_len=speed_change.real_length();
    IVP_ASSERT(rot_change_len<MAX_PLAUSIBLE_LEN);
    IVP_ASSERT(trans_change_len<MAX_PLAUSIBLE_LEN);
    IVP_ASSERT(rot_change_len>-MAX_PLAUSIBLE_LEN);
    IVP_ASSERT(trans_change_len>-MAX_PLAUSIBLE_LEN);

    IVP_DOUBLE rot_len,trans_len;
    rot_len=rot_speed.real_length();
    trans_len=speed.real_length();
    IVP_ASSERT(rot_len<MAX_PLAUSIBLE_LEN);
    IVP_ASSERT(trans_len<MAX_PLAUSIBLE_LEN);    
    IVP_ASSERT(rot_len>-MAX_PLAUSIBLE_LEN);
    IVP_ASSERT(trans_len>-MAX_PLAUSIBLE_LEN);    
}

void IVP_Core::rot_speed_plausible_check(const IVP_U_Float_Point *rot_speed_test){
    IVP_DOUBLE rot_len;
    rot_len = rot_speed_test->real_length();
    IVP_ASSERT(rot_len<MAX_PLAUSIBLE_LEN);
    IVP_ASSERT(rot_len>-MAX_PLAUSIBLE_LEN);
}


//return NULL when no friction_info is available for Friction_System
IVP_Friction_Info_For_Core *IVP_Core::get_friction_info(IVP_Friction_System *my_fr_system)
{
    if( this->physical_unmoveable == IVP_TRUE ) {
	IVP_Friction_Hash *my_hash=this->core_friction_info.for_unmoveables.l_friction_info_hash;
	if(my_hash) {
	    IVP_Friction_Info_For_Core *fr_info;
	    fr_info=my_hash->find_friction_info( my_fr_system );
#ifdef DEBUG_FRICTION_CONSISTENCY
	    IVP_IF( my_fr_system->l_environment->get_debug_manager()->check_fs ) {
	    IVP_Friction_Info_For_Core *test_info;
		test_info=NULL;
		int i;
		for(i=0;i<list_debug_hash.len();i++) {
		    IVP_Friction_Info_For_Core *search;
		    search=list_debug_hash.element_at(i);
		    if(search->l_friction_system == my_fr_system) {
			test_info=search;
			break;
		    }
		}
		IVP_ASSERT( test_info == fr_info );
	    }
#endif
	    return fr_info;
	} else {
	    return NULL;
	}
    } else {
	IVP_Friction_Info_For_Core *my_info=this->core_friction_info.for_moveables.moveable_core_friction_info;
	if(my_info) {
	    if(my_info->l_friction_system == my_fr_system) {
		return my_info;
	    }
	}
	return NULL;
    }
}


IVP_Friction_Info_For_Core *IVP_Core::moveable_core_has_friction_info() {
    IVP_ASSERT( this->physical_unmoveable == IVP_FALSE );
    return this->core_friction_info.for_moveables.moveable_core_friction_info;
}

void IVP_Core::add_friction_info(IVP_Friction_Info_For_Core *my_fr_info)
{
    if( this->physical_unmoveable == IVP_TRUE ) {
	if( this->core_friction_info.for_unmoveables.l_friction_info_hash == NULL ) {
	    this->core_friction_info.for_unmoveables.l_friction_info_hash = new IVP_Friction_Hash(2);
	}
	IVP_IF(1) {
	    IVP_ASSERT( get_friction_info(my_fr_info->l_friction_system) == NULL );
	}
	this->core_friction_info.for_unmoveables.l_friction_info_hash->add_friction_info( my_fr_info );
#ifdef DEBUG_FRICTION_CONSISTENCY
	IVP_IF(environment->get_debug_manager()->check_fs) {
	    this->list_debug_hash.add( my_fr_info );
	}
#endif
    } else {
	IVP_ASSERT( this->core_friction_info.for_moveables.moveable_core_friction_info == NULL );
	this->core_friction_info.for_moveables.moveable_core_friction_info = my_fr_info;
    }    
}

void IVP_Core::unmovable_core_debug_friction_hash() {
  for(int c = objects.len()-1;c>=0;c--){
    IVP_Real_Object *obj=this->objects.element_at(c);

    IVP_Synapse_Friction *fr_synapse;
    for(fr_synapse=obj->get_first_friction_synapse();fr_synapse;fr_synapse=fr_synapse->get_next()) {
      IVP_Contact_Point *fr_mindist=fr_synapse->get_contact_point();
      IVP_Friction_System *fr_sys = fr_mindist->l_friction_system;
      fr_sys = fr_sys;

      IVP_ASSERT( get_friction_info(fr_sys)->l_friction_system == fr_sys ); //error after deleting hash entry
    }
  }
  //printf("debug_friction_hash_ok\n");
}

void IVP_Core::unlink_friction_info(IVP_Friction_Info_For_Core *my_fr_info)
{
    IVP_Friction_Info_For_Core *my_info;
    if( this->physical_unmoveable == IVP_TRUE ) {
	my_info = this->core_friction_info.for_unmoveables.l_friction_info_hash->remove_friction_info( my_fr_info );
	IVP_ASSERT( my_info );
#ifdef DEBUG_FRICTION_CONSISTENCY
	IVP_IF( environment->get_debug_manager()->check_fs ) {
	    int i=0;
	    IVP_Friction_Info_For_Core*my_test_info=NULL;
	    while(my_info!=NULL) {
		IVP_ASSERT(i<list_debug_hash.len());
		if(list_debug_hash.element_at(i)==my_fr_info) {
		    my_test_info=my_fr_info;
		    list_debug_hash.remove_at(i);
		    break;
		}
		i++;
	    }
	    IVP_ASSERT( my_info==my_test_info );
	}
#endif
    } else {
	IVP_ASSERT( this->core_friction_info.for_moveables.moveable_core_friction_info == my_fr_info );
	this->core_friction_info.for_moveables.moveable_core_friction_info = NULL;
    }    
}

void IVP_Core::delete_friction_info(IVP_Friction_Info_For_Core *my_fr_info) {
    unlink_friction_info(my_fr_info);
    P_DELETE(my_fr_info);
}


void IVP_Core::ensure_core_to_be_in_simulation() {
    if(this->physical_unmoveable) {
	return;
    }
    
    if((this->movement_state==IVP_MT_NOT_SIM))
    {
	IVP_Environment *env=this->environment;
	//IVP_ASSERT( env->state==IVP_ES_AT );
	this->sim_unit_of_core->sim_unit_revive_for_simulation(env);
    } else {
	this->sim_unit_of_core->sim_unit_ensure_cores_movement();
    }
}


// TL: is this needed any longer ???
void IVP_Core::ensure_all_core_objs_in_simulation()
{
    IVP_ASSERT( !this->physical_unmoveable );
    for(int c = objects.len()-1;c>=0;c--){
	IVP_Real_Object *obj=this->objects.element_at(c);
	obj->ensure_in_simulation();
    }
};

// TL: is this needed any longer ???
void IVP_Core::ensure_all_core_objs_in_simulation_now()
{
    IVP_ASSERT( !this->physical_unmoveable );
    for(int c = objects.len()-1;c>=0;c--){
	IVP_Real_Object *obj=this->objects.element_at(c);
	obj->ensure_in_simulation_now();
    }
};