ivp_friction_gaps.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.


// IVP_EXPORT_PRIVATE

#include <ivp_physics.hxx>

#include <ivu_memory.hxx>
#include <ivp_great_matrix.hxx>
#include <ivp_core_macros.hxx>

#include <ivp_mindist_intern.hxx> // because of Mindist
#include <ivp_friction.hxx>
#include <ivp_friction_solver.hxx>
#include <ivp_debug_manager.hxx> // bvecause of debug psi_synchrone

#if !defined(WIN32) && !defined(PSXII) && !defined(GEKKO)
#	include <alloca.h>
#endif

inline IVP_DOUBLE ivp_frs_min(IVP_DOUBLE a,IVP_DOUBLE b){
    if(a<b) return a;
    return b;
}


IVP_RETURN_TYPE IVP_Friction_Solver::test_gauss_solution_suggestion(IVP_DOUBLE *push_results,int *active_is_at_pos,int total_actives,IVP_U_Memory *mem_friction){
    ivp_u_bool *was_already_tested=(int*)mem_friction->get_mem(dist_change_mat.columns*sizeof(ivp_u_bool));  
    memset((char*)was_already_tested,0,sizeof(int)*dist_change_mat.columns); // init with false = 0

    int gauss_failed=0;
    {
	for(int i=0;i<total_actives;i++){
	    int orig_index = active_is_at_pos[i];
	    IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(orig_index);

	    IVP_DOUBLE push_val = push_results[i];
	    //printf("gauss_value %f  ",push_val);
	
	    IVP_DOUBLE allowed_negative = IVP_MAX_ADHESION_GAUSS * l_environment->gravity_scalar;
	
	    //printf("allowed_negative %f\n",allowed_negative);
	    if(allowed_negative > push_val * correct_x_factor * get_inv_virtual_mass(info) ) {
		IVP_IF((l_environment->debug_information->debug_friction)||0){
		    printf("gauss_fault_pull %f %f\n",allowed_negative,push_val);
		}
		//return IVP_FAULT;
		gauss_failed=1;
	    }
	    was_already_tested[ orig_index ] = IVP_TRUE;
	    dist_change_mat.result_vector[ orig_index ] = push_val;
	}
    }
    {
	for(int i=0 ; i < dist_change_mat.columns ; i++)   {
	    if(was_already_tested[i]==IVP_FALSE){
		if(dist_change_mat.matrix_check_unequation_line(i) == IVP_FAULT)	    {
		    //printf("gauss_unequation_failed\n");
		    gauss_failed=1;
		    return IVP_FAULT;
		} 
	    }
	}}
    if(gauss_failed) {
	return IVP_FAULT;
    }
    return IVP_OK;
}

void IVP_Friction_Solver::factor_result_vec() {
    int i;
    for( i=dist_change_mat.columns-1;i>=0;i-- ) {
	dist_change_mat.result_vector[i] *= correct_x_factor;
    }
}

void IVP_Friction_Solver::normize_constraint_equ() {
    IVP_DOUBLE max_val=0.0f;
    int i;
    // find highest diagonal element
    for(i=dist_change_mat.columns-1;i>=0;i--) {
	IVP_DOUBLE test_val=dist_change_mat.matrix_values[ i * dist_change_mat.aligned_row_len + i ];
	if( test_val > max_val ) {
	    max_val = test_val;
	}
    }

    IVP_DOUBLE a_norm;
    if( max_val > P_DOUBLE_EPS ) {
	a_norm=1.0f/max_val;
    } else {
	a_norm=1.0f;
    }
    
    correct_x_factor = a_norm;
    
    max_val=0.0f;
    // find highest value in desired vector
    for(i=dist_change_mat.columns-1;i>=0;i--) {
	IVP_DOUBLE test_val=IVP_Inline_Math::fabsd( dist_change_mat.desired_vector[i] );
	if( test_val > max_val ) {
	    max_val = test_val;
	}
    }
    
    IVP_DOUBLE b_norm;
    if( max_val > P_DOUBLE_EPS ) {
	b_norm=1.0f/max_val;
    } else {
	b_norm=1.0f;
	max_val=1.0f;
    }

    correct_x_factor *= max_val;

    // #+# use fpu to do this
    for( i=0;i<dist_change_mat.columns;i++ ) {
	IVP_DOUBLE *base=&dist_change_mat.matrix_values[i*dist_change_mat.aligned_row_len];
	for( int j=0;j<dist_change_mat.aligned_row_len;j++ ) {
	    base[j] *= a_norm;
	}
    }

    for( i=dist_change_mat.columns-1;i>=0;i-- ) {
	dist_change_mat.desired_vector[i] *= b_norm;
    }
}

// absorb this function somewhere !
int IVP_Friction_System::get_num_supposed_active_frdists() {
    int sum=0;
    IVP_Contact_Point *fr_dist;
    for(fr_dist=this->get_first_friction_dist();fr_dist;fr_dist=this->get_next_friction_dist(fr_dist)) {
	if(fr_dist->has_negative_pull_since < 0) {
	    sum++;
	} else {
	    return sum;
	}
    }
    return sum;
}


// 'total_actives' is number of active mindists to try gauss with
// array 'active_is_at_pos' has 'total_actives' entries and gives original pos of an active
void IVP_Friction_Solver::solve_linear_equation_and_push(IVP_Friction_System *my_fs,int *active_is_at_pos,int total_actives,IVP_U_Memory *mem_friction)
{
    int supposed_actives;
    IVP_Great_Matrix_Many_Zero actives_matrix;
    actives_matrix.columns=total_actives;
    actives_matrix.calc_aligned_row_len();

    this->normize_constraint_equ();    
    actives_matrix.desired_vector = (IVP_DOUBLE*)mem_friction->get_mem(actives_matrix.aligned_row_len*sizeof(IVP_DOUBLE));
    actives_matrix.result_vector = (IVP_DOUBLE*)mem_friction->get_mem(actives_matrix.aligned_row_len*sizeof(IVP_DOUBLE));
    actives_matrix.matrix_values = (IVP_DOUBLE*)mem_friction->get_mem((total_actives*actives_matrix.aligned_row_len+IVP_VECFPU_SIZE-1)*sizeof(IVP_DOUBLE));
    actives_matrix.align_matrix_values();


    memset((char*)dist_change_mat.result_vector,0,dist_change_mat.aligned_row_len*sizeof(IVP_DOUBLE));

    actives_matrix.fill_from_bigger_matrix(&dist_change_mat,active_is_at_pos, total_actives);

    IVP_Linear_Constraint_Solver constraint_unilateral;
    
    if( actives_matrix.solve_great_matrix_many_zero() == IVP_OK )
    {
	if(( test_gauss_solution_suggestion(actives_matrix.result_vector,active_is_at_pos,total_actives,mem_friction) == IVP_OK))
	{
            IVP_IF(1) {
	        this->gauss_succed++;
            }
	    goto first_try;
	}
	// no result with cheap method, lets do some complex ...    
    }

    supposed_actives=my_fs->get_num_supposed_active_frdists();
    //if(dist_change_mat.solve_lp_with_own_complex(mem_friction)==IVP_OK) {
    if(constraint_unilateral.init_and_solve_lc(dist_change_mat.matrix_values, dist_change_mat.desired_vector, dist_change_mat.result_vector, dist_change_mat.columns, supposed_actives, mem_friction) == IVP_OK) {
first_try:
	this->factor_result_vec();
	IVP_DOUBLE e_before=my_fs->kinetic_energy_of_hole_frs();
	this->do_resulting_pushes(my_fs);
	IVP_DOUBLE e_after = my_fs->kinetic_energy_of_hole_frs();
	IVP_DOUBLE e_max_grown = my_fs->get_max_energy_gain();
	if( e_after <= e_before+e_max_grown ) {
	    my_fs->confirm_complex_pushes();
	    goto finish_slp;
	} else {
	    IVP_IF(1) {
		printf("too_much_energy_gained\n");
	    }
	    my_fs->undo_complex_pushes();
	}
    } else {
	IVP_IF(1) {
	    printf("static_friction_unequation failed\n");
	}
    }

    //this->complex_failed(my_fs);
    
finish_slp:
    actives_matrix.desired_vector=NULL;
    actives_matrix.result_vector=NULL;
    actives_matrix.matrix_values=NULL;
}

void IVP_Friction_System::do_pushes_distance_keepers(const IVP_Event_Sim *es_in) {
    IVP_Friction_System *fs=this;

    for ( IVP_Contact_Point *fr_dist=fs->get_first_friction_dist();
	  fr_dist;
	  fr_dist = this->get_next_friction_dist(fr_dist)){
	
	fr_dist->now_friction_pressure=0.0f;
	
	IVP_Core *core0=fr_dist->get_synapse(0)->l_obj->friction_core;
	IVP_Core *core1=fr_dist->get_synapse(1)->l_obj->friction_core;
	IVP_BOOL allow0=core0->fast_piling_allowed();
	IVP_BOOL allow1=core1->fast_piling_allowed();
	if(( (!allow0) || (!allow1) )) {
	    continue;
	}

	IVP_FLOAT speedup;
	speedup = (
		   (IVP_FLOAT)(IVP_SLOWLY_TURN_ON_KEEPER - fr_dist->slowly_turn_on_keeper) *
		   ( IVP_FLOAT(1.0f / IVP_SLOWLY_TURN_ON_KEEPER)  * es_in->i_delta_time)
		   );

	IVP_Impact_Solver_Long_Term *info = fr_dist->tmp_contact_info;
	IVP_DOUBLE closing_speed = info->get_closing_speed();
	IVP_DOUBLE desired_gap = ivp_mindist_settings.keeper_dist;
	IVP_DOUBLE gap_diff = desired_gap - fr_dist->get_gap_length();
	IVP_DOUBLE a = closing_speed + gap_diff * speedup;
	IVP_DOUBLE b = info->virtual_mass;
	IVP_DOUBLE impulse = a * b;

	if(impulse > 0.0f) {
	    fr_dist->now_friction_pressure = impulse * es_in->i_delta_time;    
	    IVP_Friction_Solver::apply_impulse( info, impulse );
	    //IVP_DOUBLE closing_speed2 = info->get_closing_speed();
	    //printf("%x gap %f single old %f new %f  impulse %f\n", this, gap_diff, closing_speed, closing_speed2, impulse);
	} else {
	    fr_dist->now_friction_pressure=0.0f;
	}
	
	if(fr_dist->slowly_turn_on_keeper <= 0) {
	    fr_dist->slowly_turn_on_keeper = 0;
	} else {
	    fr_dist->slowly_turn_on_keeper--;
	}
    }
}

IVP_FLOAT IVP_Friction_Solver::do_penalty_step( IVP_FLOAT *impulses, IVP_FLOAT *pretension, IVP_FLOAT force_factor, IVP_FLOAT damp_factor){
    // forward step
    {
    for (int i = contact_info_vector.len()-1; i>=0;i--){
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	IVP_DOUBLE delta_velocity = info->get_closing_speed();
	IVP_DOUBLE a = delta_velocity * damp_factor +  pretension[i] * force_factor;
	IVP_DOUBLE b = info->virtual_mass;
	IVP_DOUBLE impulse = a * b;

	IVP_DOUBLE new_sum = impulses[i] + impulse;
	if (new_sum < 0){
	    impulse = -impulses[i];
	    new_sum = 0.0f;
	}
	impulses[i] = new_sum;
	apply_impulse( info, impulse);
    }
    }
    {
    // update pretension
    for (int i = contact_info_vector.len()-1; i>=0;i--){
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	IVP_DOUBLE delta_velocity = info->get_closing_speed();
	IVP_DOUBLE a = pretension[i];
	a += delta_velocity;
	pretension[i] = a;
    }
    }

    {
    // backward step
    for (int i = 0; i< contact_info_vector.len(); i++){
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	IVP_DOUBLE delta_velocity = info->get_closing_speed();
	IVP_DOUBLE a = delta_velocity * damp_factor +  pretension[i] * force_factor;
	IVP_DOUBLE b = info->virtual_mass;
	IVP_DOUBLE impulse = a * b;

	IVP_DOUBLE new_sum = impulses[i] + impulse;
	if (new_sum < 0){
	    impulse = -impulses[i];
	    new_sum = 0.0f;
	}
	impulses[i] = new_sum;
	apply_impulse( info, impulse);
    }
    }
    // update pretension
    IVP_DOUBLE sum_quad_pretension = 0.0f;
    for (int i = contact_info_vector.len()-1; i>=0;i--){
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	IVP_DOUBLE delta_velocity = info->get_closing_speed();
	IVP_DOUBLE a = pretension[i];
	a += delta_velocity;
	if (impulses[i]){
	    sum_quad_pretension += a * a;
	}
	pretension[i] = a;
    }
    
    return sum_quad_pretension;
}
#if 0
void IVP_Friction_Solver::do_penalty_method(IVP_Friction_System *fs){
    IVP_FLOAT *impulses   = (IVP_FLOAT*)alloca( sizeof(IVP_FLOAT) * contact_info_vector.len());
    IVP_FLOAT *pretension = (IVP_FLOAT*)alloca( sizeof(IVP_FLOAT) * contact_info_vector.len());
    
    // compress now_friction press into shorter array
    for ( IVP_Contact_Point *fr_dist=fs->get_first_friction_dist();
	  fr_dist;
	  fr_dist = fs->get_next_friction_dist(fr_dist)){
	IVP_Impact_Solver_Long_Term *info = fr_dist->get_lt();
	int i = info->index_in_fs;
	if (i<0) continue;
	
	IVP_DOUBLE desired_gap = ivp_mindist_settings.keeper_dist;
	IVP_DOUBLE gap_diff = desired_gap - fr_dist->get_gap_length();
	//desired_speed_to_close_gap[i] = gap_diff * es->i_delta_time;
	pretension[i] = gap_diff * es->i_delta_time * 0.1f;
	//impulses[i] = 0.0f;
	impulses[i] = 0.5f * fr_dist->now_friction_pressure * es->delta_time;

	//	IVP_DOUBLE delta_velocity = info->get_closing_speed();
	//printf("%x %i old %f pressure %f\n", fr_dist, i, delta_velocity, impulses[i]);
    }

    IVP_IF(0){
	for (int i = contact_info_vector.len()-1; i>=0;i--){
	    IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	    IVP_DOUBLE delta_velocity = info->get_closing_speed();
	    printf("%i old %f\n", i, delta_velocity);
	}
    }
    
    // first do old now friction pressures
    for (int i = contact_info_vector.len()-1; i>=0;i--){
	IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	IVP_DOUBLE push_val = impulses[i];
	apply_impulse( info, push_val );
    }

    IVP_IF(0){
	for (int i = contact_info_vector.len()-1; i>=0;i--){
	    IVP_Impact_Solver_Long_Term *info = contact_info_vector.element_at(i);
	    IVP_DOUBLE delta_velocity = info->get_closing_speed();
	    printf("%i new %f\n", i, delta_velocity);
	}
    }
    
    // do the steps (max 50)
    if (1){
	//IVP_FLOAT max_sum_qdiff = 0.01f * 0.01f;
	printf("penalty step: ");
	for (int s = 10; s>=0; s--){
	    IVP_FLOAT sum_qdiff = do_penalty_step( impulses, pretension, 0.5f, 0.9f );
	    printf("%f ",sum_qdiff);
	    //if (sum_qdiff < max_sum_qdiff) break;
	};
	printf("\n");
    }
    
    { ; // uncompress now_friction_pressure
	for ( IVP_Contact_Point *fr_dist=fs->get_first_friction_dist();
	      fr_dist;
	      fr_dist = fs->get_next_friction_dist(fr_dist)){
	    IVP_Impact_Solver_Long_Term *info = fr_dist->get_lt();
	    int i = info->index_in_fs;
	    if (i<0){
		fr_dist->now_friction_pressure = 0.0f;
		continue;
	    }
	    fr_dist->now_friction_pressure = impulses[i] * es->i_delta_time;
	}
    }
}
#endif

// fuer glaettung des complex
void IVP_Friction_Solver::do_inactives_pushes(IVP_Friction_System *fs){
    int counter_two=0;
    for(IVP_Contact_Point *fr_dist = fs->get_first_friction_dist();
	fr_dist;
	fr_dist=fs->get_next_friction_dist(fr_dist), counter_two++) {	
	if(fr_dist->has_negative_pull_since==0) {	    
	    IVP_Impact_Solver_Long_Term *info = fr_dist->get_lt();
	    IVP_DOUBLE push_val = fr_dist->now_friction_pressure;
	    apply_impulse( info, push_val * es->i_delta_time );
	}
    }
}

void IVP_Friction_Solver::debug_distance_after_push(int counter)
{
    IVP_USE(counter);
#if 0 /* muss noch umgeschrieben werden */   
    IVP_Core *core0,*core1;
    core0=cores_of_dist[counter*2];
    core1=cores_of_dist[counter*2+1];
    IVP_U_Float_Point speed_world0,speed_world1;

    IVP_U_Float_Point rotation,translation;
    rotation.set(&core0->rot_speed);
    rotation.add(&core0->rot_speed_change);