lcp_solver.cpp

hl2 source code. Do not use it illegal.

		hk_Console::get_instance()->printf("gauss_lu_test_gauss_invalid\n");
	      }
	    }
	    if( ret_val3 == HK_OK ) {
		debug_mat.mult();
		for( i=0;i<r_actives;i++ ) {
		    IVP_DOUBLE diff=lu_sub_solver.out_vec[i]-debug_mat.result_vector[i];
		    if(hk_Math::fabs(diff) > 0.001f) {
		      HK_LCP_IF(debug_lcs) {
			hk_Console::get_instance()->printf("inv_lu_test_fail %f %f should be equal\n",lu_sub_solver.out_vec[i],debug_mat.result_vector[i]);
		      }
		    }
		}
	    }
	    sub_solver_mat.MATRIX_EPS = GAUSS_EPS;
	}
#endif
    } else {
	HK_LCP_IF((debug_lcs)) {
	    hk_Console::get_instance()->printf("sub_lu_is_off %d\n",r_actives);
	}
	debug_no_lu_count++;

	m_gauss_inp.m_n_columns = r_actives;
	m_gauss_inp.m_aligned_row_len = hk_VecFPU::calc_aligned_row_len(r_actives,m_gauss_inp.m_A);

	int ignored_index = actives_inactives_ignored[ ignored_pos ];
    
	for(i=0;i<r_actives;i++) {
	    int index=actives_inactives_ignored[i];
	    m_gauss_inp.m_b[i]= - this->full_A[index * aligned_size + ignored_index];

	    int j;
	    for(j=0;j<r_actives;j++) {
		int index2=actives_inactives_ignored[j];
		m_gauss_inp.m_A[i*m_gauss_inp.m_aligned_row_len + j] = this->full_A[ index * aligned_size + index2 ];

	    }
	}
	ret_val = m_gauss_sub_solver.solve_gauss_elemination( m_gauss_inp, m_gauss_out);
	for( i=0;i<r_actives;i++ ) {
	    lu_sub_solver.m_inout_vec[i]=m_gauss_out.m_result[i];
	}
    }
    
    //copy clamped
    for(i=0;i<r_actives;i++) {
	this->delta_x[ actives_inactives_ignored[i] ] = (hk_real)lu_sub_solver.m_inout_vec[i];
    }

    this->delta_x[ actives_inactives_ignored[ignored_pos] ] = 1.0f;
    
    return ret_val;
}

void hk_LCP_Solver::start_debug_lcs() {
    hk_Console::get_instance()->printf("b_vals ");
    int i;
    int j;
    for(i=0;i<n_variables;i++) {
	hk_Console::get_instance()->printf("%.4f ",full_b[i]);
    }
    hk_Console::get_instance()->printf("\n");
    
    hk_Console::get_instance()->printf("matrix:\n");
    for(i=0;i<n_variables;i++) {
	for(j=0;j<n_variables;j++) {
	    hk_Console::get_instance()->printf("%.4f ",full_A[i*aligned_size + j]);
	}
	hk_Console::get_instance()->printf("\n");
    }
    hk_Console::get_instance()->printf("\n");
}

void hk_LCP_Solver::debug_out_lcs() {
    hk_Console::get_instance()->printf("r_actives %d  ignored_nr %d\n",r_actives,ignored_pos);
    int i;

    if(ignored_pos>n_variables) {
	ignored_pos=n_variables;
    }
    
    hk_Console::get_instance()->printf("actives_inactives  ");
    for(i=0;i<r_actives;i++) {
	hk_Console::get_instance()->printf("%d ",actives_inactives_ignored[i]);
    }
    hk_Console::get_instance()->printf("  ");
    for(i=r_actives;i<ignored_pos;i++) {
	hk_Console::get_instance()->printf("%d ",actives_inactives_ignored[i]);
    }
    hk_Console::get_instance()->printf("  ");
    for(i=ignored_pos;i<n_variables;i++) {
	hk_Console::get_instance()->printf("%d ",actives_inactives_ignored[i]);
    }
    hk_Console::get_instance()->printf("\n");

    hk_Console::get_instance()->printf("x-vals ");
    for(i=0;i<n_variables;i++) {
	hk_Console::get_instance()->printf("%.4f ",full_x[i]);
    }
    hk_Console::get_instance()->printf("\n");

#if 0
    full_solver_mat.desired_vector = full_x;
    full_solver_mat.mult_aligned(); // temporary missuse
    full_solver_mat.desired_vector = delta_x;
    
    hk_Console::get_instance()->printf("accel1 ");
    for(i=0;i<n_variables;i++) {
	hk_Console::get_instance()->printf("%.4f ",full_solver_mat.result_vector[i] - full_b[i]);
    }
    hk_Console::get_instance()->printf("\n");
#endif

    hk_Console::get_instance()->printf("incr_accel ");
    for(i=0;i<n_variables;i++) {
	hk_Console::get_instance()->printf("%.4f ",accel[i]);
    }
    hk_Console::get_instance()->printf("\n\n");
  
}

//cannot be vector optimized because of indexed access
hk_result hk_LCP_Solver::setup_l_u_solver() {
    int i;
    HK_LCP_IF(0) {
	hk_Console::get_instance()->printf("setting_up_lu\n");
    }
    
    for(i=0; i <r_actives;i++) {
	    int index = actives_inactives_ignored[i];
	    //sub_solver_mat.desired_vector[i]= b[index];
	    lu_sub_solver.m_inout_vec[i]=full_b[index];
	    hk_real *source = & this->full_A[ index * aligned_size ];
	    hk_incrlu_real *dest = & lu_sub_solver.m_U_matrix[ i * lu_sub_solver.m_aligned_row_len ];
	    for(int j=0;j<r_actives;j++) {
	        int index2=actives_inactives_ignored[j];
		dest[j] = source[index2];
	    }
    }
    
    return lu_sub_solver.l_u_decomposition_with_pivoting();
}

void hk_LCP_Solver::lcs_bubble_sort_x_vals() {
    int r1=n_equations;
    int r2;
    while(1) {
	r2=r1+1;
	if(r2>=r_actives) {
	    break;
	}
	if( full_x[ actives_inactives_ignored[ r1 ] ] < full_x[ actives_inactives_ignored[ r2 ] ] ) {
	    exchange_lcs_variables( r1, r2 );
	    int r_t = r1;
	    while( (r_t > n_equations) && ( full_x[ actives_inactives_ignored [r_t-1] ] < full_x[ actives_inactives_ignored[ r_t ] ] ) ) {
		exchange_lcs_variables( r_t, r_t-1 );
		r_t--;
	    }
	}
	r1=r2;
    }

    HK_LCP_IF(1) {
	int i;
	for(i=1;i<r_actives;i++) {
	    if( full_x[ actives_inactives_ignored[ i-1 ] ] < full_x[ actives_inactives_ignored[ i ] ] ) {
		hk_Console::get_instance()->printf("lcs_bubble_failed\n");
	    }
	}
    }
}

// fills accel and full_x vectors
void hk_LCP_Solver::get_values_when_setup() {
#if 0
    full_solver_mat.result_vector=accel;
    full_solver_mat.desired_vector=full_x; //temporary misuse
    full_solver_mat.mult_aligned();
    full_solver_mat.result_vector=delta_accel;
    full_solver_mat.desired_vector=delta_x;
#endif

    input_struct->m_A->mult_vector( full_x, accel );

    int i;
    {
	for(i=0;i<n_variables;i++) {
	    accel[i]-=full_b[i];
	}
    }

    for(i=0;i<r_actives;i++) {
	accel[actives_inactives_ignored[i]]=0.0f; //just to remove rounding errors
    }
}

void hk_LCP_Solver::startup_setup(int try_actives)
{
    r_actives = try_actives;
    aligned_sub_size=hk_VecFPU::calc_aligned_row_len(r_actives,lu_sub_solver.m_L_matrix);
    ignored_pos=try_actives;
    lu_sub_solver.m_n_sub = r_actives;

    int i;
    for(i=0;i<aligned_size;i++)
	{
		full_x[i]=0.0f;
    }
    if(setup_l_u_solver() == HK_OK)
	{
		sub_solver_status = 0;
		HK_LCP_IF(debug_lcs)
		{
			lu_sub_solver.debug_print_l_u();
			int j;
			hk_Console::get_instance()->printf("\n\nline48:\n ");
			for(j=0;j<r_actives;j++)
			{
				hk_Console::get_instance()->printf("%.3f ",lu_sub_solver.m_L_matrix[lu_sub_solver.m_aligned_row_len*48+j]);
			}
			hk_Console::get_instance()->printf("\n\n");
		}
        //input vec for lu_sub_solver is filled within setup_l_u_solver

		while(1)
		{
begin_of_while:	    
			//solution_not_found=0;
			lu_sub_solver.solve_lin_equ();
			for(i=0;i<r_actives;i++)
			{
				int index=actives_inactives_ignored[i];
				full_x[index]=(hk_real)lu_sub_solver.m_inout_vec[i];
			}
			get_values_when_setup(); //fills x[i] and accel[i]

			for(i=r_actives-1;i>=n_equations;i--)
			{
				int index=actives_inactives_ignored[i];
				if(full_x[ index ] < 0.0f)
				{
					full_x[ index ] = 0.0f;
					r_actives--;
					ignored_pos--;
					exchange_lcs_variables(r_actives,i);
					decrement_sub_solver(i);
					if(sub_solver_status > 0) 
					{
						for(i=0;i<n_variables;i++)
						{
						   accel[i]=0.0f;
						   full_x[i]=0.0f;
						}
						goto fast_setup_failed;
					}
					else 
					{
						for(i=0;i<r_actives;i++) 
						{
							index=actives_inactives_ignored[i];
							lu_sub_solver.m_inout_vec[i]=full_b[index];
						}
						goto begin_of_while;
					}
				}
			}
			break;
		}
    }
	else
	{
fast_setup_failed:	
		// sorry fast setup failed
		r_actives=n_equations;
		ignored_pos=n_equations;
		lu_sub_solver.m_n_sub=n_equations;
    }
}

// full setup is done when numerical problems are encountered.
// lu_solver is setup with pivoting to get a stable inverted matrix
// for the active variables
hk_result hk_LCP_Solver::full_setup() {
    HK_LCP_IF(0) {
	hk_Console::get_instance()->printf("doing_fulll_setup\n");
    }
    
    int i;
    lu_sub_solver.m_n_sub = r_actives;
    
    for(i=0;i<aligned_size;i++) {
	full_x[i]=0.0f;
    }

    do_a_little_random_permutation();
    if( setup_l_u_solver() == HK_OK ) {
	sub_solver_status = 0;
        //input vec for lu_sub_solver is filled within setup_l_u_solver
	lu_sub_solver.solve_lin_equ();
	for(i=0;i<r_actives;i++) {
	    int index=actives_inactives_ignored[i];
	    full_x[index]=(hk_real)lu_sub_solver.m_inout_vec[i];
	}
    } else {
	sub_solver_status = 2;
	HK_LCP_IF(debug_lcs) {
	    hk_Console::get_instance()->printf("setup_lu_failed\n");
	}

	lcs_bubble_sort_x_vals(); //move actives with lowest x to the end (matrix is singular and last vars get a zero)
	                          //but giving the lowest x's the zeros is just a heuristic
	do_a_little_random_permutation();
	
	m_gauss_inp.m_n_columns = r_actives;
	m_gauss_inp.m_aligned_row_len = hk_VecFPU::calc_aligned_row_len(r_actives,m_gauss_inp.m_A);
	
	for(i=0;i<r_actives;i++) {
	    int index=actives_inactives_ignored[i];
	    m_gauss_inp.m_b[i]= full_b[index];

	    int j;
	    for(j=0;j<r_actives;j++) {
		int index2=actives_inactives_ignored[j];
		m_gauss_inp.m_A[i*m_gauss_inp.m_aligned_row_len + j] = this->full_A[ index * aligned_size + index2 ];

	    }
	}
	hk_result ret_val = m_gauss_sub_solver.solve_gauss_elemination( m_gauss_inp, m_gauss_out);

	if(ret_val!=HK_OK) {
	    HK_LCP_IF(1) {
		hk_Console::get_instance()->printf("error_setup_gauss_failed_too\n");
	    }
	    return ret_val;
	}
    
	for(i=0;i<r_actives;i++) {
	    int index=actives_inactives_ignored[i];
	    full_x[index]=(hk_real)m_gauss_out.m_result[i];
	}
    }

    get_values_when_setup();
    
    if( full_setup_test_ranges() > 0 ) {
	HK_LCP_IF(debug_lcs) {
	    hk_Console::get_instance()->printf("\ncleared_some_illegal_vars\n\n");
	}
	return full_setup();
    } else {
	
    }
    
    return HK_OK;
    //optimization:
    //fill directly in data, not versus reset_*
}

void hk_LCP_Solver::move_variable_to_end( int var_nr ) {
    int i;
    for(i=var_nr+1;i<n_variables;i++) {
	exchange_lcs_variables( i-1, i );
    }
}	

// returns number of removed actives
int hk_LCP_Solver::full_setup_test_ranges() {
    int illegal_vars=0;
    int i;
    for(i=n_equations;i<r_actives;i++) {
	if( full_x[actives_inactives_ignored[i]] < HK_LCP_SOLVER_EPS ) {
	    if( full_x[actives_inactives_ignored[i]] > -HK_LCP_SOLVER_EPS ) {
		full_x[actives_inactives_ignored[i]] = 0.0f;
		exchange_lcs_variables( i, r_actives-1 );
	    } else {
		move_variable_to_end( i );
		illegal_vars++;
		ignored_pos--;
	    }
	    i--;
	    r_actives--;
	    lu_sub_solver.m_n_sub--;
	    sub_solver_status=1;
	}
    }
    for(i=r_actives;i<ignored_pos;i++) {
	if( accel[actives_inactives_ignored[i]] < 0.0f ) {
	    move_variable_to_end( i );
	    i--;
	    ignored_pos--;
	}
    }
    return illegal_vars;
}

void hk_LCP_Solver::debug_test_all_values() {
    //TL: doesn't make sense, because reset_x and reset_accel have been removed 
#if 0
    int i;

    for(i=0;i<aligned_size;i++) {
	full_x[i] = full_x[i];
    }

    input_struct->m_A->mult_vector(full_x,accel);

#if 0
    full_solver_mat.result_vector = accel;
    full_solver_mat.desired_vector = full_x; //temporary misuse
    full_solver_mat.mult_aligned();
    full_solver_mat.result_vector = delta_accel;
    full_solver_mat.desired_vector = delta_x;
#endif

    for(i=0;i<aligned_size;i++) {
	accel[i]-=full_b[i];
    }

    HK_LCP_IF(debug_lcs) {
	for(i=0;i<n_variables;i++) {
	    hk_real diff=hk_Math::fabs( accel[i] - accel[i] );
	    if( diff > 0.001f ) {
		hk_Console::get_instance()->printf("accel_violated at %d %f should be %f\n",i,accel[i],accel[i]);
	    }
	    diff=hk_Math::fabs( full_x[i] - full_x[i] );
	    if( diff > 0.001f ) {
		hk_Console::get_instance()->printf("x_violated at %d\n",i);
	    }
	}
    }
#endif
}