ivp_friction.cxx

hl2 source code. Do not use it illegal.

IVP_FLOAT IVP_Contact_Point_API::get_vert_force(IVP_Contact_Point *friction_handle){
    return friction_handle->now_friction_pressure;
};

void IVP_Friction_Info_For_Core::friction_info_insert_friction_dist(IVP_Contact_Point *dist)
{
    friction_springs.add(dist);
}

void IVP_Friction_Info_For_Core::friction_info_delete_friction_dist(IVP_Contact_Point *dist)     
{
    friction_springs.remove(dist);
}

int IVP_Friction_Info_For_Core::dist_number()
{
    return friction_springs.len();
}


IVP_Friction_Solver::IVP_Friction_Solver(IVP_Friction_System *fri_sys, const IVP_Event_Sim *es_in)
{
    l_environment=fri_sys->l_environment;
    es = es_in;
#ifdef DEBUG
                gauss_succed=0;
#endif

    IVP_U_Memory *my_mem = fri_sys->l_environment->get_memory_manager();
    dist_change_mat.columns = fri_sys->friction_dist_number - fri_sys->complex_not_necessary_number;    
    dist_change_mat.calc_aligned_row_len();

    dist_change_mat.matrix_values =(IVP_DOUBLE*)my_mem->get_mem((size_t)(dist_change_mat.aligned_row_len*dist_change_mat.columns+IVP_VECFPU_SIZE-1)*sizeof(IVP_DOUBLE));
    dist_change_mat.desired_vector=(IVP_DOUBLE*)my_mem->get_mem((size_t)dist_change_mat.aligned_row_len*sizeof(IVP_DOUBLE));
    dist_change_mat.result_vector =(IVP_DOUBLE*)my_mem->get_mem((size_t)dist_change_mat.aligned_row_len*sizeof(IVP_DOUBLE));
    dist_change_mat.align_matrix_values();
}



void IVP_Friction_System::delete_friction_distance(IVP_Contact_Point *old_dist) {
    //ensure objects have time to move a little bit (e.g. fall down) after contact lost (do not freeze them)
    IVP_Core *core0,*core1;
    core0=old_dist->synapse[0].l_obj->get_core();
    core1=old_dist->synapse[1].l_obj->get_core();
    
    core0->reset_freeze_check_values();
    core1->reset_freeze_check_values();

    this->remove_dist_from_system(old_dist);
    if(this->dist_removed_update_pair_info(old_dist)==IVP_TRUE)
    {
	this->union_find_necessary=IVP_TRUE;
    }
    IVP_Friction_Info_For_Core *fr_info0,*fr_info1;
    fr_info0=core0->get_friction_info(this);
    fr_info1=core1->get_friction_info(this);

    fr_info0->friction_info_delete_friction_dist(old_dist);
    if(fr_info0->dist_number()==0)
    {
	core0->delete_friction_info(fr_info0);
	this->remove_core_from_system(core0);
	core0->sim_unit_of_core->union_find_needed_for_sim_unit=IVP_TRUE;
    }
    fr_info1->friction_info_delete_friction_dist(old_dist);
    if(fr_info1->dist_number()==0)
    {
	core1->delete_friction_info(fr_info1);
	this->remove_core_from_system(core1);
	core1->sim_unit_of_core->union_find_needed_for_sim_unit=IVP_TRUE;
    }
    
    for(int i=0;i<0;i++) {
	IVP_Synapse_Friction *my_syn=old_dist->get_synapse(i);
	if(my_syn->prev) {
	    my_syn->prev->next=my_syn->next;
	} else {
	    my_syn->l_obj->friction_synapses = my_syn->get_next();
	}
	if(my_syn->next) {
	    my_syn->next->prev=my_syn->prev;
	}
    }

    //printf("deleting_frdist %lx o %lx c %lx  o %lx c %lx\n",(long)old_dist,(long)old_dist->synapse[0]->l_obj,(long)old_dist->synapse[0]->l_obj->to_real()->physical_core,(long)old_dist->synapse[1]->l_obj,(long)old_dist->synapse[1]->l_obj->to_real()->physical_core);
    P_DELETE(old_dist);
}



void IVP_Friction_System::apply_real_friction(const IVP_Event_Sim *es)
{
    //fr_solver.mem_friction.init_mem(); // is freed in do_friction_system
    //printf("whole_mindists %ld\n",this->friction_dist_number);
    
    this->calc_friction_forces(es);
    this->ease_friction_forces();

}


// throws second sys in first one
// Optimize: leave pair_info valid, don't process with with dist_added_update_pair_info
void IVP_Friction_System::fusion_friction_systems(IVP_Friction_System *second_sys)
{
    IVP_Friction_System *first_sys=this;
    
    IVP_Simulation_Unit *s1,*s2;
    s1=first_sys->cores_of_friction_system.element_at(0)->sim_unit_of_core;
    s2=second_sys->cores_of_friction_system.element_at(0)->sim_unit_of_core;
  
    IVP_IF(1) {
        IVP_Environment *env=first_sys->l_environment;
	IVP_IF(env->get_debug_manager()->check_fs) {
	    printf("fusion_fs %f %lx %lx  cores ",env->get_current_time().get_time(),(long)first_sys,(long)second_sys);
	    for (int k = first_sys->cores_of_friction_system.len()-1; k>=0; k--){
		IVP_Core *my_core = first_sys->cores_of_friction_system.element_at(k);
	        printf("%lx ",(long)my_core);
	    }
	    printf(" ");
	    for (int l = second_sys->cores_of_friction_system.len()-1; l>=0; l--){
		IVP_Core *my_core = second_sys->cores_of_friction_system.element_at(l);
	        printf("%lx ",(long)my_core);
	    }
	    printf("\n");	    
	}
    }
  
    IVP_Contact_Point *my_dist,*next_dist;
    my_dist=second_sys->get_first_friction_dist();
    //int counter=0;

    IVP_Core *a_core_from_second;
    
    while(my_dist)  {
        next_dist=second_sys->get_next_friction_dist(my_dist);
	second_sys->remove_dist_from_system(my_dist);
	second_sys->dist_removed_update_pair_info(my_dist); //is a little bit slow. remove all core pairs directly. But this way it is saver
        first_sys->add_dist_to_system(my_dist);
	first_sys->dist_added_update_pair_info(my_dist);
	//first_sys->fr_solver.calc_calc_solver(first_sys);	
      	my_dist=next_dist;
	//counter+=2;
    }
    //second_sys->first_friction_dist=NULL; //DIRTY, as this should be done with a loop of deletes, but this way faster

    // now transfer objects ({unmovable!} if necessary) from one system to another

    a_core_from_second=second_sys->moveable_cores_of_friction_system.element_at(0);

    for (int n = second_sys->cores_of_friction_system.len()-1; n>=0;n--){
	IVP_Core *obj1 = second_sys->cores_of_friction_system.element_at(n);
	IVP_Friction_Info_For_Core *fr_info0,*fr_info1;
	fr_info1 = obj1->get_friction_info(second_sys); // this info block must exist
	if( (fr_info0 = obj1->get_friction_info(first_sys)) )	{
	    // this (unmovable) object is part of both systems; transferr mindists and delete second Info-Blocks
	    fr_info1 = obj1->get_friction_info(second_sys);
	    for (int m = fr_info1->friction_springs.len()-1; m>=0; m--){
		my_dist = fr_info1->friction_springs.element_at(m);
		fr_info0->friction_info_insert_friction_dist(my_dist);
		fr_info1->friction_info_delete_friction_dist(my_dist);
	    }
	    obj1->delete_friction_info(fr_info1);
	} else {
	    //this object is not member of first sys, leave info block but change link to system
	    obj1->unlink_friction_info(fr_info1);  // l_friction_system is used as key in hash
	    fr_info1->l_friction_system=first_sys; // now it is save to change l_friction_system
	    obj1->add_friction_info(fr_info1);     // re-add
	    second_sys->remove_core_from_system(obj1);
	    first_sys->add_core_to_system(obj1);
	}
    }

    //remove_friction_system(second_sys);
    IVP_USE(a_core_from_second);

    P_DELETE(second_sys);
    //first_sys->fr_solver.calc_calc_solver(first_sys);
    IVP_IF(1) {
        first_sys->test_hole_fr_system_data();
    }
    //printf("did_fusion\n");
}


int IVP_Friction_Core_Pair::number_of_pair_dists()
{
    return fr_dists.len();
}


void IVP_Friction_Core_Pair::add_fr_dist_obj_pairs(IVP_Contact_Point *dist)
{
#if 0 /* TL: this code is for initializing span_friction_s, but why at this point :( */   
    int numbers = fr_dists.len();
    if( numbers > 0 ) {
	IVP_U_Float_Point friction_vec;
	get_average_friction_vector(&friction_vec);
	IVP_Core *core0 = dist->get_synapse(0)->get_object()->friction_core;
	if( core0 != objs[0] ) {
	    friction_vec.mult(-1.0f);
	}
	IVP_Impact_Solver_Long_Term *info = dist->tmp_contact_info;
	dist->span_friction_s[0]=friction_vec.dot_product(&info->span_friction_v[0]);
	dist->span_friction_s[1]=friction_vec.dot_product(&info->span_friction_v[1]);
    }
#endif    
    fr_dists.add(dist);
}

void IVP_Friction_Core_Pair::del_fr_dist_obj_pairs(IVP_Contact_Point *dist)
{
    fr_dists.remove(dist);
}


void IVP_Friction_System::add_fr_pair(IVP_Friction_Core_Pair *pair)
{
    fr_pairs_of_objs.add(pair);
	pair->objs[0]->get_environment()->fire_event_friction_pair_created( pair );
}

void IVP_Friction_System::del_fr_pair(IVP_Friction_Core_Pair *pair)
{
	pair->objs[0]->get_environment()->fire_event_friction_pair_deleted( pair );
    fr_pairs_of_objs.remove(pair);
}

IVP_Friction_Core_Pair *IVP_Friction_System::get_pair_info_for_objs(IVP_Core *core0,IVP_Core *core1){
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
	IVP_Friction_Core_Pair *my_pair= fr_pairs_of_objs.element_at(i);
	if( (my_pair->objs[0]==core0) || (my_pair->objs[1]==core0) )	{
	    if( (my_pair->objs[0]==core1) || (my_pair->objs[1]==core1) )
	    return my_pair;
	}
    }
    return NULL;
}


// all cores of a simulated friction system have to be simulated
void IVP_Friction_System::debug_check_system_consistency() {
    for (int i = cores_of_friction_system.len()-1; i>=0; i--){
	IVP_Core *core = cores_of_friction_system.element_at(i);
	if(!core->physical_unmoveable)	{
	    IVP_ASSERT(IVP_MTIS_SIMULATED (core->movement_state) );
	}
    }    
}


// make inline
IVP_Core *IVP_Core::union_find_get_father()
{
    IVP_Core *obj=this;
    IVP_Core *on_top=obj;
    while(obj)
    {
	on_top=obj;
	obj=obj->tmp.union_find_father;
    }
    return on_top;
}

// a connection between two objects in fr sys no longer exists, test if fr sys can be splitted in two parts
// fixed objs dont block the splitting
IVP_Core *IVP_Friction_System::union_find_fr_sys()
{
    IVP_Friction_System *fr_sys=this;

    for (int k = fr_sys->cores_of_friction_system.len()-1; k>=0; k--){
	IVP_Core *obje = fr_sys->cores_of_friction_system.element_at(k);
	obje->tmp.union_find_father=NULL;
    }

    for (int l = fr_sys->fr_pairs_of_objs.len()-1; l>=0; l--){
	IVP_Friction_Core_Pair *fr_pair = fr_sys->fr_pairs_of_objs.element_at(l);
	IVP_Core *obj0,*obj1;
	obj0=fr_pair->objs[0];
	obj1=fr_pair->objs[1];
	if(obj0->physical_unmoveable || obj1->physical_unmoveable) {
	    continue; //ignore fixed objs. they cannot connect objects (fixed objs can have more than one friction system)
	}
	//printf("doinguf %lx %lx   ",(long)obj0&0x0000ffff,(long)obj1&0x0000ffff); UFTEST
	obj0=obj0->union_find_get_father();
	obj1=obj1->union_find_get_father();
	//printf("fathers are %lx %lx   ",(long)obj0&0x0000ffff,(long)obj1&0x0000ffff); UFTEST
	if(obj0!=obj1) {
	    obj1->tmp.union_find_father=obj0;
	    //printf("newfather %lx : %lx\n",(long)obj1&0x0000ffff,(long)obj0&0x0000ffff); UFTEST
	}
    }

    IVP_IF(0)	{
	for (int k = fr_sys->cores_of_friction_system.len()-1; k>=0; k--){
	    IVP_Core *objj = fr_sys->cores_of_friction_system.element_at(k);
	    IVP_Core *of=objj->union_find_get_father();
	    printf("uff of %lx : %lx\n",(long)objj&0x0000ffff,(long)of&0x0000ffff);
	}
    }

    
    IVP_Core *first_father=NULL,*second_father=NULL;

    // find representative obj for second system (must not be a fixed obj)
    for (int i1 = fr_sys->cores_of_friction_system.len()-1; i1>=0; i1--){
	IVP_Core *obj = fr_sys->cores_of_friction_system.element_at(i1);
	if(!obj->physical_unmoveable)	{
	    first_father=obj->union_find_get_father();
	}
    }
    IVP_ASSERT(first_father);
    
    for (int i2 = fr_sys->cores_of_friction_system.len()-1; i2>=0; i2--){
	IVP_Core *obj = fr_sys->cores_of_friction_system.element_at(i2);
	if(obj->physical_unmoveable) {
	    continue; //ignore fixed objs
	}
	IVP_Core *test_father=obj->union_find_get_father();
	if(test_father!=first_father)	{
	    second_father=test_father;
	}
    }
    
    return second_father;
}

// split in two parts; every object, that has 'split_father' as union-find father is put into a new system 
void IVP_Friction_System::split_friction_system(IVP_Core *split_father)
{
    IVP_Friction_System *fr_sys=this;
    
    IVP_Friction_System *new_fr_sys=new IVP_Friction_System(l_environment);

     IVP_IF( fr_sys->l_environment->get_debug_manager()->check_fs ) {
        printf("split_fs %f %lx %lx  cores ",fr_sys->l_environment->get_current_time().get_time(),(long)fr_sys,(long)new_fr_sys);
	for (int i = fr_sys->cores_of_friction_system.len()-1; i>=0;i--){
	    IVP_Core *my_core = fr_sys->cores_of_friction_system.element_at(i);
	    printf("%lx ",(long)my_core);
	}
	printf("\n");
     }

    //printf("splitting_now father %lx\n",(long)split_father&0x0000ffff);
    //fr_sys->debug_fs_out_ascii();
    {
	//first transfer objs
	for (int j = fr_sys->cores_of_friction_system.len()-1; j>=0; j--){
	    IVP_Core *obj=fr_sys->cores_of_friction_system.element_at(j);
	  //printf("   %lx hasfather %lx ",(long)obj&0x0000ffff,(long)union_find_get_father(obj)&0x0000ffff);