ivp_friction_gaps.cxx

hl2 source code. Do not use it illegal.

    translation.set(&core0->speed);
    translation.add(&core0->speed_change);
    core0->get_surface_speed_on_test(&obj_points_dist[counter*2],&translation,&rotation,&speed_world0);

    rotation.set(&core1->rot_speed);
    rotation.add(&core1->rot_speed_change);
    translation.set(&core1->speed);
    translation.add(&core1->speed_change);
    core1->get_surface_speed_on_test(&obj_points_dist[counter*2+1],&translation,&rotation,&speed_world1);

    speed_world1.subtract(&speed_world0);
    IVP_DOUBLE new_dist_speed=speed_world1.dot_product(&dist_vector_world[counter]);
    IVP_IF(environment->debug_information->debug_impact)
    {
	printf("dist_speed_d: distance %.5f  speed:  before %.5f  after %.5f dis_change %.5f\n",dist_len[counter],dist_velocity[counter],new_dist_speed,new_dist_speed-dist_velocity[counter]);
    }
#endif    
}

void IVP_Friction_Solver::complex_failed(IVP_Friction_System *fs) {
    IVP_Contact_Point *my_dist;
    int i;
    my_dist=fs->get_first_friction_dist();
    for(i=0;i<fs->friction_dist_number;i++)
    {
#if !defined(IVP_DISTANCE_KEEPERS) && !defined(IVP_PANELTY_SOLVER)
	my_dist->now_friction_pressure=0.0f;
#endif	
	my_dist=fs->get_next_friction_dist(my_dist);
    }
}

// returns 1 if a friction distance has to be removed (not used any more)
int IVP_Friction_Solver::do_resulting_pushes(IVP_Friction_System *fs)
{

    int m_index = 0;
    int active_count = 0;
    for ( IVP_Contact_Point *my_dist=fs->get_first_friction_dist();
	  my_dist;
	  m_index++, my_dist=fs->get_next_friction_dist(my_dist)){
	IVP_DOUBLE impulse_now;
	
	if((m_index < fs->complex_not_necessary_number)||0) {
	    impulse_now=0.0f;
	    goto no_complex_done;
	}
	
	impulse_now = dist_change_mat.result_vector[m_index];
	    
	if(impulse_now>0.0f){
	    if(my_dist->has_negative_pull_since>=0){
		my_dist->has_negative_pull_since=-1; //first positive push
	    } else {
		my_dist->has_negative_pull_since--;
	    }
	} else {
	    if(impulse_now==0.0f){
		//inactive
		my_dist->has_negative_pull_since=0;
		impulse_now = my_dist->now_friction_pressure * 0.0f * this->es->delta_time;
		goto cont_for_next;
	    } else {
		// negative push
		if(my_dist->has_negative_pull_since<0) {
		    my_dist->has_negative_pull_since=0;
		}
		my_dist->has_negative_pull_since++;
		if(my_dist->has_negative_pull_since > 9)  {//allow only 9 negative pushes in following PSIs
			my_dist->has_negative_pull_since=0; //next time inactiv
		}
	    }
	}
	{
	    IVP_Impact_Solver_Long_Term *info = my_dist->get_lt();	    
	    this->async_apply_impulse(info, impulse_now);
	}
	active_count ++;
    
	  cont_for_next:	    
	if(impulse_now<0.0f) {
	    impulse_now=0.0f;
	}
	
	  no_complex_done:
	
#if !defined(IVP_DISTANCE_KEEPERS) && !defined(IVP_PANELTY_SOLVER)
	my_dist->now_friction_pressure = impulse_now * this->es->i_delta_time;
#else	    
	my_dist->now_friction_pressure += impulse_now * this->es->i_delta_time; //keine Glaettung der Reibung
#endif	
	
    }
    IVP_IF(l_environment->debug_information->debug_friction){
	for(int j=0;j<fs->friction_dist_number;j++) {
	    IVP_IF(j>=fs->complex_not_necessary_number) {
		debug_distance_after_push(j-fs->complex_not_necessary_number);
	    }
	}
    }
    //printf("n active lines %i\n", active_count);
    
#ifdef DEBUG
    IVP_IF(l_environment->debug_information->debug_friction)
    {
	printf("pushes_ok. resulting_distance_velos:\n");
	this->print_dist_velocity(fs);
    }
#endif	/* DEBUG */
    
    return 0;
}

#ifdef DEBUG
void IVP_Friction_Solver::print_dist_velocity(IVP_Friction_System *fs)
{
    //int dist_counter=0;
    int dist_counter_two=0;
    int total_dist_counter=-1;
    IVP_Contact_Point *my_dist,*next_dist=NULL;
    my_dist=fs->get_first_friction_dist();
    while(my_dist)
    {
	total_dist_counter++;
	next_dist=fs->get_next_friction_dist(my_dist);

	if(total_dist_counter<fs->complex_not_necessary_number) {
	    my_dist=next_dist;
	    continue;
	}
		
	IVP_Synapse_Friction *syn0 = my_dist->get_synapse(0);
	IVP_Synapse_Friction *syn1 = my_dist->get_synapse(1);
	IVP_Core *core0,*core1;
	core0 = syn0->get_object()->get_core();
	core1 = syn1->get_object()->get_core();

	IVP_U_Float_Point w_speed0,w_speed1;
	IVP_U_Float_Point speed_var_trans,speed_var_rot;
	speed_var_trans.add(&core0->speed,&core0->speed_change);
	speed_var_rot.add(&core0->rot_speed,&core0->rot_speed_change);
	core0->get_surface_speed_on_test(&my_dist->tmp_contact_info->contact_point_cs[0],&speed_var_trans,&speed_var_rot,&w_speed0);
	speed_var_trans.add(&core1->speed,&core1->speed_change);
	speed_var_rot.add(&core1->rot_speed,&core1->rot_speed_change);
	core1->get_surface_speed_on_test(&my_dist->tmp_contact_info->contact_point_cs[1],&speed_var_trans,&speed_var_rot,&w_speed1);

	IVP_U_Float_Point rel_world_speed;
	rel_world_speed.subtract(&w_speed1,&w_speed0);

	IVP_IF(1) {
	    printf("dvelo_ %.4f   ",rel_world_speed.dot_product(&my_dist->get_lt()->surf_normal));
	}
       
	dist_counter_two++;
	my_dist=next_dist;
    }
    IVP_IF(1) {
	printf("\n");
    }
}
#endif


void IVP_Friction_System::test_hole_fr_system_data()
{
    IVP_IF(1) {
    //printf("hole_fr_test %lx \n",(long)this);
    IVP_Friction_System *fs=this;    
    IVP_Contact_Point *mindist=fs->get_first_friction_dist();
    if (!mindist) return;
    
#if 0	
    int startnumber;
    {
	startnumber=mindist->number_in_friction+1;
	if(startnumber!=fs->friction_dist_number)
	{
	    printf("test_fr dist_number wrong\n");
	    CORE;
	}
    }
#endif	
    int two_count=0;
    while(mindist)
    {
#if 0	
	startnumber--;
	if(startnumber<0)
	{
	    printf("test_fr number <0");
	    CORE;
	}

	if(startnumber!=mindist->number_in_friction)
	{
	    printf("test_fr wrong number");
	    CORE;
	}
#endif
	for(int i=0;i<2;i++){
	    IVP_Core *r_obj= mindist->get_synapse(i)->l_obj->friction_core;	    
	    IVP_Real_Object *obj_obj=r_obj->objects.element_at(0);
	    IVP_Time time=r_obj->environment->get_current_time();
	    
	    IVP_Friction_Info_For_Core *fr_info = r_obj->get_friction_info(fs);

	    if(fr_info->l_friction_system!=this){
		printf("test_fr l_friction_system %f %lx\n",time.get_time(),(long)obj_obj); CORE;
	    }
	    
	    int found_mine=0;
	    for (int j = fr_info->friction_springs.len()-1; j>=0;j--){
		IVP_Contact_Point *all_dists = fr_info->friction_springs.element_at(j);
		if(all_dists==mindist){
		    found_mine=1;
		} else {
		    int is_in_collection=0;
		    IVP_Contact_Point *temp_dist = fs->get_first_friction_dist();
		    while(temp_dist){
			if(temp_dist==all_dists){
			    is_in_collection=1;
			}
			temp_dist=fs->get_next_friction_dist(temp_dist);
		    }
		    if(!is_in_collection){
			printf("test_fr there was mindist %lx in obj which is not in system\n",(long)all_dists&0x0000ffff); CORE; 
		    }
		}
	    }
	    if(!found_mine)
	    {
		printf("test_fr mindist %lx missing in obj\n",(long)mindist); CORE;
	    }
	}

	{
	    //test if mindist is found in pairs
	    int found_mine=0;
	    for (int k = fr_pairs_of_objs.len()-1; k>=0;k--){
		IVP_Friction_Core_Pair *fr_pair = fr_pairs_of_objs.element_at(k);
		for (int n = fr_pair->fr_dists.len()-1; n>=0; n--){
		    IVP_Contact_Point *fr_dist=fr_pair->fr_dists.element_at(n);
		    if(fr_dist==mindist) {
			found_mine=1;
		    }
		}
	    }
	    if(!found_mine) {
		printf("test_fr dist %lx not found in pairs\n",(long)mindist);
		CORE;
	    }
	}
	
	two_count+=2;
	mindist=fs->get_next_friction_dist(mindist);
    }

    {
	//test if all dists in pairs are in system
	for (int n = fr_pairs_of_objs.len()-1; n>=0; n--){
	    IVP_Friction_Core_Pair *fr_pair = fr_pairs_of_objs.element_at(n);
	    for (int m = fr_pair->fr_dists.len()-1; m>=0; m--){
		IVP_Contact_Point *fr_dist = fr_pair->fr_dists.element_at(m);
		int found_mine=0;
		for(IVP_Contact_Point *mindist2=fs->get_first_friction_dist();mindist2;mindist2=fs->get_next_friction_dist(mindist2)){
		    if(mindist2==fr_dist) {
			found_mine=1;
		    }
		}
		if(!found_mine)	{
		    printf("test_fr pair %lx containing dist %lx not in system\n",(long)fr_pair,(long)fr_dist);
		    CORE;
		}
	    }
	}
    }
    }
}

#if 0
// doesnt work when a object has more than one friction system
void check_fr_info_mindist(IVP_Friction_Solver *fsolv,IVP_Contact_Point *my_dist,int dist_counter_two)
{
    IVP_Real_Object *obj0=(IVP_Real_Object*)my_dist->synapse[0]->l_obj;
    IVP_Real_Object *obj1=(IVP_Real_Object*)my_dist->synapse[1]->l_obj;

    IVP_Friction_Info_For_Obj *fr_i;
    fr_i=obj0->get_first_friction_info_of_obj();
    if(fr_i!=fsolv->friction_infos[dist_counter_two])
    {
	printf("fr_info_damaged obj %lx\n",(long)obj0&0x0000ffff);
	CORE;
    }
    fr_i=obj1->get_first_friction_info_of_obj();
    if(fr_i!=fsolv->friction_infos[dist_counter_two+1])
    {
	printf("fr_info_damaged obj %lx\n",(long)obj1&0x0000ffff);
	CORE;
    }
}
#endif


int global_friction_counter=0;

void IVP_Friction_Solver::setup_coords_mindists(IVP_Friction_System *fs)
{
    int index_of_contact_point = 0;
    int total_dist_counter = 0;
    for (IVP_Contact_Point *my_dist = fs->get_first_friction_dist();
	 my_dist;
	 my_dist = fs->get_next_friction_dist(my_dist)){

	IVP_Impact_Solver_Long_Term *info = my_dist->tmp_contact_info;

	if(total_dist_counter < fs->complex_not_necessary_number) {
	    info->index_in_fs = -1; //not used in complex
	    continue;
	}
	total_dist_counter++;
	
	info->friction.friction_infos[0] = my_dist->get_synapse(0)->get_object()->get_core()->get_friction_info(fs); // #+# bad bad
	info->friction.friction_infos[1] = my_dist->get_synapse(1)->get_object()->get_core()->get_friction_info(fs);
	info->friction.dist_len = my_dist->get_gap_length();
	info->friction.has_negative_pull_since = my_dist->has_negative_pull_since; // copy to avoid later cache misses

	contact_info_vector.add(info);
	info->index_in_fs = index_of_contact_point++;
    } // for all contact_points
}


#if 0
	IVP_Synapse_Friction *syn0 = my_dist->get_synapse(0);
	IVP_Synapse_Friction *syn1 = my_dist->get_synapse(1);

	IVP_Friction_Solver_Data *frisodat=&frimindist_data[dist_counter_two];

	frisodat->dist_vector_obj[0]=my_dist->tmp_contact_info->surface_normal_cs[0];
	frisodat->dist_vector_obj[1]=my_dist->tmp_contact_info->surface_normal_cs[1];	
 	frisodat->cores_of_dist[0]= syn0->l_obj->physical_core;
	frisodat->cores_of_dist[1]= syn1->l_obj->physical_core;
	
	//vielleicht nicht die IVP_Friction_Info_For_Obj speichern sondern gleich den IVP_U_Vector der mindists
	frisodat->friction_infos[0]=syn0->l_obj->friction_core->get_friction_info(fs);
	frisodat->friction_infos[1]=syn1->l_obj->friction_core->get_friction_info(fs);

	IVP_IF(0) {
	    IVP_U_Float_Point space_pointer;
	    IVP_U_Point world_points_dist[2];
	    IVP_U_Matrix *mat;
	    mat=&frisodat->cores_of_dist[0]->m_world_f_core_last_psi;
	    mat->vmult4(&frisodat->contact_point_cs[0],&world_points_dist[0]);
	    mat=&frisodat->cores_of_dist[1]->m_world_f_core_last_psi;
	    mat->vmult4(&frisodat->contact_point_cs[1],&world_points_dist[1]);

	    space_pointer.subtract(&world_points_dist[1],&world_points_dist[0]);
	    char *out_text=p_make_string("d%d_%.1f",my_dist->has_negative_pull_since,my_dist->now_friction_pressure);
	    syn0->l_obj->get_environment()->add_draw_vector(&world_points_dist[0],&space_pointer,out_text,1);
	    P_FREE (out_text);
	}

	//IVP_U_Float_Point diff_vec_world; //vector of position difference. points from first synapse to second
	//diff_vec_world.subtract(&world_points_dist[1],&world_points_dist[0]);
	//frisodat->dist_len=diff_vec_world.fast_real_length();
	frisodat->dist_len = my_dist->get_gap_length();
	//diff_vec_world.normize();
	//diff_vec_world.set(0.0f,1.0f,0.0f);
	//printf("difff %f %f %f\n",diff_vec_world.k[0],diff_vec_world.k[1],diff_vec_world.k[2]);
	//frisodat->dist_vector_world=diff_vec_world;

	frisodat->dist_vector_world.set_multiple(&my_dist->get_lt()->surf_normal,+1.0f);
	
	IVP_IF(l_environment->debug_information->debug_impact)
	{
	    //printf("dist_vec_world %.3f %.3f %.3f\n",dist_vector_world[dist_counter].k[0],dist_vector_world[dist_counter].k[1],dist_vector_world[dist_counter].k[2]);
	}
#endif


void out_friction_info_obj(IVP_Core *obj)
{
    return; //absturz
    IVP_IF(1) {
	IVP_Friction_Info_For_Core *fr_info=obj->moveable_core_has_friction_info();
	while(fr_info)    {
	    printf("obj %lx fs %lx obj_ma %d",(long)obj&0x0000ffff,(long)fr_info->l_friction_system&0x0000ffff,obj->physical_unmoveable);
	    
	    for (int i = fr_info->friction_springs.len()-1; i>=0; i--){
		IVP_Contact_Point *mindist = fr_info->friction_springs.element_at(i);
		printf("  md %lx",(long)mindist&0x0000ffff);
	    }
	    fr_info=NULL;
	    printf("\n");
	}
    }
}

/* friction distance with number 'current_contact_point_index' is tested. Therefore both objects of distance get a test push
 * Velocity changes are represented with 'rotation_vec' (obj coords) and 'translation_vec' (world coords)
 * then this funcion is called for both objects
 * for one object is looked if other distances are inflicted (speed of distance changes)
 * if yes, speed change is represented in a line of the matrix, with line number = number of the distance
 * values are added because a column of the matrix is built with testing of two objects (example: diagonal elements, but not only)
 */
void IVP_Friction_Solver::calc_distance_matrix_column(int current_contact_point_index,
						      IVP_Core *core,IVP_Friction_Info_For_Core *fr_info,
						      IVP_U_Float_Point *rotation_vec,IVP_U_Float_Point *translation_vec)
{

    //dist_counter is number of distance that is tested. It is represented with a column of the matrix.
    
    IVP_ASSERT(!core->physical_unmoveable);
        
    for (int col = 0;col < fr_info->friction_springs.n_elems; col++){
	IVP_Contact_Point *distance = fr_info->friction_springs.element_at(col);
	int line_num = distance->tmp_contact_info->index_in_fs;
	if (line_num < 0) continue;
	
	if(line_num < current_contact_point_index ) {
	    //continue; //my distance is not used in complex or symmetric