ivp_friction_gaps.cxx

hl2 source code. Do not use it illegal.

	}	
	int which_obj = 1; //a distance is between two objects, which one is mine ? - 0 first, 1 second
	IVP_DOUBLE factor;
	
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(line_num);

	if(info->contact_core[0]==core){
	    which_obj=0;
	    factor = -1.0f;
	} else {
	    factor = +1.0f;
	    which_obj=1;
	}
	if ( info->contact_core[which_obj]){
	    IVP_DOUBLE speed_change_scalar = factor * get_closing_speed_core_i( info, which_obj, rotation_vec, translation_vec);
	    dist_change_mat.matrix_values[line_num * dist_change_mat.aligned_row_len + current_contact_point_index] += speed_change_scalar;
	    //if ( line_num != current_contact_point_index){
	    //dist_change_mat.matrix_values[current_contact_point_index * dist_change_mat.columns + line_num] += speed_change_scalar * ( ivp_rand() * 0.001f + 1.0f);
	    //}
	}
    }
}


// returns number of active mindists (that had in last PSI push_values different to 0.0f)
// array 'active_is_at_pos' is filled up to position 'active_is_at_pos'-1
// main task is to fill 'dist_change_mat'
int IVP_Friction_Solver::calc_solver_PSI(IVP_Friction_System *fs,int *active_is_at_pos){

    IVP_IF((l_environment->debug_information->debug_friction)||0)    {
	global_friction_counter++;
	printf("\ndoing_friction_calc %d\n",global_friction_counter);
    }
    {
	//delete distance change matrix
	int mat_width=(dist_change_mat.aligned_row_len * dist_change_mat.columns);
	for(int d = mat_width-1;d >= 0;d --)    {
	    dist_change_mat.matrix_values[d] = 0.0f; 
	}
    }

    int active_dists_so_far=0;
    // new super fast version
    for (int i = 0; i< contact_info_vector.len(); i++){
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	// get closing speed
	IVP_DOUBLE dist_velocity = info->get_closing_speed();
	    
	IVP_DOUBLE distance_off = ivp_mindist_settings.friction_dist - info->friction.dist_len;  // distance to my optimal position
	dist_change_mat.desired_vector[i] = calc_desired_gap_speed(dist_velocity,distance_off,1.0f);

	// mark contact point as active if positive pressure
	{
	    //printf("has_neg_since %d\n",my_dist->has_negative_pull_since);
	    if(info->friction.has_negative_pull_since != 0) {
		*active_is_at_pos = i;
		active_is_at_pos++;
		active_dists_so_far++;
	    }
	}
	    

	// calc matrix
	if ( info->contact_core[0]){
	    IVP_U_Float_Point rotation_vec,translation_vec;
	    this->test_push_core_i( info, 0, &rotation_vec, &translation_vec, -1.0f);
	    calc_distance_matrix_column(i,info->contact_core[0],info->friction.friction_infos[0],&rotation_vec,&translation_vec);
	}
	    
	if ( info->contact_core[1]){
	    IVP_U_Float_Point rotation_vec,translation_vec;
	    this->test_push_core_i( info, 1, &rotation_vec, &translation_vec, +1.0f);
	    calc_distance_matrix_column(i,info->contact_core[1],info->friction.friction_infos[1],	&rotation_vec,&translation_vec);
	}

    }
    int k = 0;
    for ( IVP_Contact_Point *my_dist = fs->get_first_friction_dist();
	  my_dist;
	  k++, my_dist = fs->get_next_friction_dist(my_dist)){
	if (my_dist->get_lt()->index_in_fs <0) continue;
	if (my_dist->get_lt()->index_in_fs != k){
	    CORE;
	}
    }
    
    return active_dists_so_far;
}


void debug_out_friction_system(IVP_Friction_System *fr_sys)
{
    IVP_Contact_Point *my_dist;
    my_dist=fr_sys->get_first_friction_dist();
    IVP_U_Point world_p;
    IVP_U_Float_Point obj_p;
    int cou=0;
    while(my_dist)
    {
	IVP_Synapse_Friction *syn0 = my_dist->get_synapse(0);
	IVP_Synapse_Friction *syn1 = my_dist->get_synapse(1);
	if(syn0->get_status() !=IVP_ST_POINT){
	    syn0=syn1;
	}
	CORE; // no calc_object_coords should be used
	//syn0->calc_object_coords(&obj_p);
	IVP_U_Matrix *mat;
	mat=&syn0->get_object()->physical_core->m_world_f_core_last_psi;
	mat->vmult4(&obj_p,&world_p);

	IVP_U_Float_Point world_push_debug;
	world_push_debug.set(0.0f,-1.0f,0.0f);
	world_push_debug.normize();
	world_push_debug.mult(10.0f);

	IVP_IF(0) {
	    char *out_text=p_make_string("%ddistdd%lx",cou,my_dist);
	    syn0->get_object()->get_environment()->add_draw_vector(&world_p,&world_push_debug,out_text,3);
	    delete (out_text);
	}
	
	cou++;
	my_dist=fr_sys->get_next_friction_dist(my_dist);
    }
}

void IVP_Friction_System::reorder_mindists_for_complex() {
    int not_in_complex=0;
    IVP_Contact_Point *my_frdist,*next_frdist;
    my_frdist=this->get_first_friction_dist();
    while(my_frdist) {
	next_frdist=this->get_next_friction_dist(my_frdist);

	if( 1 && ((my_frdist->get_gap_length() >= ivp_mindist_settings.max_distance_for_friction) ||
		  (my_frdist->get_lt()->friction_is_broken == IVP_TRUE) )){
    	    
	    this->delete_friction_distance(my_frdist);
	    my_frdist=next_frdist;
	    continue;
	}
	
	if(my_frdist->get_gap_length() > ivp_mindist_settings.friction_dist + ivp_mindist_settings.distance_keepers_safety) {
	    // move to the start of the list

	    IVP_Core *core0,*core1;
	    core0=my_frdist->get_synapse(0)->get_object()->friction_core;
	    core1=my_frdist->get_synapse(1)->get_object()->friction_core;
	
	    IVP_BOOL allow0=core0->fast_piling_allowed();
	    IVP_BOOL allow1=core1->fast_piling_allowed();
	    if(( (allow0) & (allow1))|0 ) {
	        //not_in_complex++;
		//printf("want_to_remove %lx\n",(long)my_frdist);
		this->remove_dist_from_system( my_frdist );
		this->add_dist_to_system( my_frdist );		
	    }
	}
	
	my_frdist=next_frdist;
    }
    this->complex_not_necessary_number=not_in_complex;
    //printf("not_necessary_nr %ld\n",not_in_complex);
}


// first_frd and second_frd are neighbours in queue, first is before second, exchange them
void IVP_Friction_System::exchange_friction_dists(IVP_Contact_Point *first_frd,IVP_Contact_Point *second_frd)
{
    if(this->first_friction_dist==first_frd) {
	this->first_friction_dist=second_frd;
    }
    
    {
	if(first_frd->prev_dist_in_friction) {
	    first_frd->prev_dist_in_friction->next_dist_in_friction=second_frd;
	}
	if(second_frd->next_dist_in_friction) {
	    second_frd->next_dist_in_friction->prev_dist_in_friction=first_frd;
	}   
    }
    {
	first_frd->next_dist_in_friction=second_frd->next_dist_in_friction;
	second_frd->prev_dist_in_friction=first_frd->prev_dist_in_friction;
	first_frd->prev_dist_in_friction=second_frd;
	second_frd->next_dist_in_friction=first_frd;
    }
}

void IVP_Friction_System::ivp_debug_fs_pointers()
{
    IVP_IF(1) {
    printf("%lx  ",(long)first_friction_dist&0x0000ffff);
    for(IVP_Contact_Point *fr_d=get_first_friction_dist();fr_d;fr_d=get_next_friction_dist(fr_d))
    {
	printf("%lx %lx %d %lx  ",(long)fr_d->prev_dist_in_friction&0x0000ffff,(long)fr_d&0x0000ffff,fr_d->has_negative_pull_since,(long)fr_d->next_dist_in_friction&0x0000ffff);
    }
    printf("\n");
    }
}

//sort dists, dists beeing active for longest nr of PSIs are at start of queue
void IVP_Friction_System::bubble_sort_dists_importance()
{
    IVP_Contact_Point *first_dist=this->get_first_friction_dist();
    IVP_Contact_Point *second_dist;
    if( !first_dist ) {
	return;
    }
    while(1) {
	second_dist=get_next_friction_dist(first_dist);
	if( !second_dist ) {
	    break;
	}
	if( first_dist->has_negative_pull_since > second_dist->has_negative_pull_since ) {
	    exchange_friction_dists(first_dist,second_dist);
	    IVP_Contact_Point *test_dist;
	    while( (test_dist=get_prev_friction_dist(second_dist)) && ( test_dist->has_negative_pull_since > second_dist->has_negative_pull_since ) ) {
		exchange_friction_dists(test_dist,second_dist);
	    }
	    second_dist=first_dist;
	}
	first_dist=second_dist;
    }
    IVP_IF(1) {
	for( first_dist=get_first_friction_dist(); first_dist; first_dist=get_next_friction_dist(first_dist) ) {
	    second_dist=get_next_friction_dist(first_dist);
	    if(second_dist) {
		IVP_ASSERT( !(first_dist->has_negative_pull_since > second_dist->has_negative_pull_since ) );
	    }
	}
    }
}

#if 0
//sort dists, dists beeing active for longest nr of PSIs are at end of queue
void IVP_Friction_System::bubble_sort_dists_importance()
{
    int did_exchange=1;
    //printf("start_bubble\n");
    while(did_exchange) {
	did_exchange=0;
	IVP_Contact_Point *moving_dist=this->get_first_friction_dist();
	if(!moving_dist) {
	    return; //degenerated system (without any mindist)
	}
	IVP_Contact_Point *test_dist;
	//printf("tlp_for %lx  ",(long)moving_dist&0x0000ffff);
	//ivp_debug_fs_pointers();
	while((test_dist=this->get_next_friction_dist(moving_dist))!=NULL) {
	    if(moving_dist->has_negative_pull_since<test_dist->has_negative_pull_since) {
		this->exchange_friction_dists(moving_dist,test_dist);
		did_exchange=1;
		//printf("exchgg %lx %lx  ",(long)moving_dist&0x0000ffff,(long)test_dist&0x0000ffff);
		//ivp_debug_fs_pointers();
	    } else {
		moving_dist=test_dist;
	    }
	}
    }
    IVP_IF(1) {
	for(IVP_Contact_Point *fr_d=get_first_friction_dist();fr_d;fr_d=get_next_friction_dist(fr_d))
	{
	    IVP_Contact_Point *test_d=get_next_friction_dist(fr_d);
	    if(test_d) {
		if(fr_d->has_negative_pull_since<test_d->has_negative_pull_since) {
		    printf("bubble_sort_error\n");
		    CORE;
		}
	    }
	}
    }
    //printf("after_bs  ");
    //ivp_debug_fs_pointers();
    IVP_IF(0)
    {
        printf("sort_dists ");
	for(IVP_Contact_Point *fr_d=get_first_friction_dist();fr_d;fr_d=get_next_friction_dist(fr_d))
	{
	    printf("%d ",fr_d->has_negative_pull_since);
	}
	printf("\n");
    }
}
#endif

IVP_BOOL IVP_Friction_System::core_is_terminal_in_fs(IVP_Core *test_core) {
    int total_number_of_partners=0;
    IVP_ASSERT(test_core->physical_unmoveable==IVP_FALSE);
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
	IVP_Friction_Core_Pair *fcp = fr_pairs_of_objs.element_at(i);
	if( (fcp->objs[0]==test_core) || (fcp->objs[1]==test_core) ) {
	    int unmov=fcp->objs[0]->physical_unmoveable | fcp->objs[1]->physical_unmoveable;
	    if(!unmov) {
		total_number_of_partners++;
	    }
	}
    }
    if(total_number_of_partners>1) return IVP_FALSE;
    //printf("found_terminal %lx\n",(long)test_core&0x0000ffff);
    return IVP_TRUE;
}

//fallback for oversized matrizes:
//for terminal objects: turn on fast piling technique and hope they will be pushed away
//for nonterminal objects: turn on fast piling but delete whole energy (with delayed pushes the energy would be accumulated)
void IVP_Friction_System::static_fr_oversized_matrix_panic() {
    IVP_Core *my_core;
    int i;
    for(i=moveable_cores_of_friction_system.len()-1;i>=0;i--) {
	my_core=moveable_cores_of_friction_system.element_at(i);
	my_core->fast_piling_allowed_flag=IVP_TRUE;
	if(this->core_is_terminal_in_fs(my_core)) {
	    //hope the object will be pushed away some time
	} else {
	    //remove all energy from object
	    my_core->speed.set_to_zero();
	    my_core->rot_speed.set_to_zero();
	}
    }
}

// returns 1 if friction_system is no longer used
void IVP_Friction_System::do_friction_system(const IVP_Event_Sim *es_in)
{    
    IVP_U_Memory *my_mem = l_environment->short_term_mem;
    my_mem->start_memory_transaction();
    
    friction_global_counter++;
    IVP_IF(l_environment->debug_information->debug_friction)    {
	printf("doing_friction_nr %d\n",friction_global_counter);
	if(friction_global_counter==14)	{
	    printf("losgehts\n");
        }
	printf("friction_system_simulating %d distances\n",this->friction_dist_number);
    }    

    this->bubble_sort_dists_importance();   // to to put heavy pushed mindists ontop of the list
    this->reorder_mindists_for_complex();   // get rid of mindist already heaving a big gap @+@ ->check speed after do_pushes_distance_keepers also

#if defined(IVP_DISTANCE_KEEPERS) && !defined(IVP_PANELTY_SOLVER)
    this->do_pushes_distance_keepers(es_in);  // do local constraint pushes
#endif

    if((this->friction_dist_number - this->complex_not_necessary_number) > IVP_MAX_FRICTION_MATRIX) {
	this->static_fr_oversized_matrix_panic();
	my_mem->end_memory_transaction();
	return;    
    }
    
    IVP_Friction_Solver fr_solver(this,es_in);

    fr_solver.setup_coords_mindists(this);
    int *original_pos_of_active=(int*)my_mem->get_mem(friction_dist_number*sizeof(int));

    while(IVP_TRUE)   {
	int num_actives = fr_solver.calc_solver_PSI(this,original_pos_of_active);

	IVP_IF(l_environment->debug_information->disable_friction) {
	    break;
	}

#ifdef IVP_PANELTY_SOLVER
       	fr_solver.do_penalty_method(this);
#endif
	
	IVP_IF(1==0)   {
	  for(int j=0;j<num_actives;j++)		{
	    IVP_ASSERT(original_pos_of_active[j]>=0);
	    IVP_ASSERT(original_pos_of_active[j]<friction_dist_number);
	  }
	}
	
	fr_solver.solve_linear_equation_and_push(this,original_pos_of_active,num_actives,my_mem);
	break;
    }
    my_mem->end_memory_transaction();
}


IVP_DOUBLE IVP_Friction_System::kinetic_energy_of_hole_frs()
{
    IVP_DOUBLE sum=0.0f;
    for (int i = cores_of_friction_system.len()-1; i>=0; i--){
	IVP_Core *my_obj = cores_of_friction_system.element_at(i);
	IVP_U_Float_Point speed_vec,rot_vec;
	speed_vec.add(&my_obj->speed,&my_obj->speed_change);
	rot_vec.add(&my_obj->rot_speed,&my_obj->rot_speed_change);
	sum+=my_obj->get_energy_on_test(&speed_vec,&rot_vec);
    }
    return sum;
}

void IVP_Friction_System::confirm_complex_pushes() {
    for (int k = moveable_cores_of_friction_system.len()-1; k>=0; k--){
	IVP_Core *my_core = moveable_cores_of_friction_system.element_at(k);
	my_core->commit_all_async_pushes(); //not too nice: no clear_all_async_pushes done before
    }
}

void IVP_Friction_System::undo_complex_pushes() {
    for (int k = moveable_cores_of_friction_system.len()-1; k>=0; k--){
	IVP_Core *my_core = moveable_cores_of_friction_system.element_at(k);
	my_core->abort_all_async_pushes();
    }
}

IVP_DOUBLE IVP_Friction_System::get_max_energy_gain() {
    IVP_DOUBLE sum_gain=0.0f;
    for (int k = cores_of_friction_system.len()-1; k>=0; k--){
	IVP_Core *my_core = cores_of_friction_system.element_at(k);
	if(my_core->physical_unmoveable==IVP_FALSE) {
	    IVP_DOUBLE force_gravity=my_core->get_mass()*9.81f; //@@TL #+# oh oh 
	    // allow lift of 10cm
	    sum_gain+=force_gravity*0.1f;
	}
    }
    return sum_gain;
}