lcp_solver.cpp

hl2 source code. Do not use it illegal.

			int k;
			for(k=r_actives;k<ignored_pos;k++)
			{
				int idx=actives_inactives_ignored[k];
				HK_LCP_IF( hk_Math::fabs( full_x[idx] ) > 0.1f )
				{
					hk_Console::get_instance()->printf("incorrect_x %f at %d  should be zero\n",accel[idx],idx);
				}
				if(full_x[idx]<-0.001f)
				{
					hk_Console::get_instance()->printf("incorrect_negative_x %f at %d  should be zero\n",accel[idx],idx);
				}
				if( accel[idx] < 0.0f )
				{
					hk_Console::get_instance()->printf("warning_inactive_neg accel %d %f\n",idx,accel[idx]);
				}	
			}
		}
		
		HK_LCP_IF(debug_lcs)
		{
			debug_out_lcs();
		}

		HK_LCP_IF( hk_Math::fabs( full_x[ ignored_full_index ] ) < solver_eps )
		{
			debug_test_all_values();
		}
		
		if(( sub_solver_status > 0))
		{
			if( (sub_solver_status == 1) && (did_real_step) )
			{
				do_a_little_random_permutation();
				if(setup_l_u_solver()==HK_OK)
				{
					sub_solver_status=0; 
				}
				else
				{
					HK_LCP_IF(debug_lcs)
					{
						hk_Console::get_instance()->printf("setup_lu_notok\n");
					}
				}
			}
			else
			{
				sub_solver_status = 1; //ok then next time
			}
		}
		
		if( hk_Math::fabs( accel[ignored_full_index] ) < solver_eps )
		{
			// my processed variable has no acceleration and so could be an inactive var
			// except the force x is greater 0.0f than we must move it to actives
			limiting_var=ignored_pos;
			if( hk_Math::fabs( full_x[ignored_full_index] ) < solver_eps )
			{
				goto move_ignored_to_inactive;
			}
			else
			{
				goto move_ignored_to_active;
			}
		}
		
		if(accel[ignored_full_index] < 0.0f)
		{
			hk_real smallest_step;
			
			//first r vars are clamped
			//compute x with Acc x = b
			if(get_xdirection() == HK_FAULT)
			{
				HK_LCP_IF(debug_lcs)
				{
					hk_Console::get_instance()->printf("lcs_fdirection_total_fail\n");
				}
				goto perform_full_setup;
			}
			delta_x[ ignored_full_index ] = 1.0f;
			
			//delta_x is now filled in original var space
	    			
			//full_solver_mat.mult();  //delta_accel = A * delta_x
			//delta_accel is now filled in original var space
			mult_active_x_for_accel();

			int i;
			for(i=0;i<r_actives;i++)
			{
				int index_full = actives_inactives_ignored[i];
				delta_accel[index_full] = 0.0f; //just to overwrite rounding-errors
			}
			
			HK_LCP_IF(0)
			{
				for(int j=0;j<r_actives;j++)
				{
					int full_index = actives_inactives_ignored[ j ];
					hk_Console::get_instance()->printf("should be zero %f\n",delta_accel[full_index]);
				}
			}
			
			// now find smallest steps
			
			// step to make clamped active out of ignored_pos
			HK_LCP_IF( accel[ ignored_full_index ] > 0.0f )
			{
				hk_Console::get_instance()->printf("errror_accel_ignored_negativ\n");
			}
			if( -accel[ ignored_full_index ] < delta_accel[ ignored_full_index ] * HK_LCP_MAX_STEP_LEN )
			{
				smallest_step = - accel[ignored_full_index] / delta_accel[ ignored_full_index ];
				HK_ASSERT( smallest_step > 0.0f );
				limiting_var = ignored_pos;
			}
			else
			{
				limiting_var = -1;
				smallest_step = HK_LCP_REAL_MAX;
			}


			// smallest step of clamped (active)
			// if we increase value of ignored variable, equations will allways hold true whatever step size we choose
			// however the x-value of active variables are not allowed to pass through zero
			for(i=n_equations;i<r_actives;i++)
			{
				index = actives_inactives_ignored[i];
				hk_real del_f = delta_x[ index ];
				if( del_f < - solver_eps )
				{
					hk_real local_step = - full_x[ index ] / del_f;
					if( (hk_Math::fabs( local_step ) < solver_eps) && ( full_x[ index ] < solver_eps ) )
					{
						limiting_var = i;
						smallest_step = local_step;
						goto limiting_var_found;
					}
					if( local_step < smallest_step + solver_eps)
					{ //make it easier to move from active to inactive
						smallest_step = local_step;
						limiting_var = i;
					}
				}
			}

			// smallest step of unclamped
			for(i=r_actives;i<ignored_pos;i++)
			{
				index = actives_inactives_ignored[i];
				hk_real del_a = delta_accel[ index ];
				if( del_a < -HK_LCP_SOLVER_EPS )
				{
					hk_real local_step = - accel[ index ] / del_a;
					if( local_step < smallest_step - solver_eps )
					{ // make it harder to move from inactive to active
						HK_LCP_IF(debug_lcs)
						{
							hk_Console::get_instance()->printf("violate_inactive: accel %.4f  delta %.4f\n",accel[index],del_a);
						}			
						// we prefer making inactives out of actives instead other way round if both is possible ( both steps are nearly zero )
						// uuups this is all wrong
						if( (hk_Math::fabs( local_step ) > solver_eps ) || (limiting_var==ignored_pos) || 1)
						{
							smallest_step = local_step;
							limiting_var = i;
						}
					}
				}
			}

			if(smallest_step < 0.0f)
			{
				HK_LCP_IF(1)
				{
					hk_Console::get_instance()->printf("error_stepsize_negative %f\n",smallest_step);
				}
				smallest_step=0.0f;
			}	
			
			if( limiting_var < 0 )
			{
				return HK_FAULT;
			}

			HK_LCP_IF(debug_lcs)
			{
				hk_Console::get_instance()->printf("smallest_step_is %f\n",smallest_step);
			}

limiting_var_found:

			if(smallest_step > HK_LCP_MAX_STEP_LEN)
			{
				goto perform_full_setup;
			}
			if(smallest_step < HK_LCP_SOLVER_EPS)
			{
				nr_zero_steps++;
				if(nr_zero_steps > (n_variables>>1) +2 )
				{ //heuristic
					goto perform_full_setup; //does a little random permutation, we hope next time no endless loop
				}
			}
			else
			{
				nr_zero_steps=0;
			}
			did_real_step=1;
			update_step_vars( smallest_step );
			
			if(limiting_var < r_actives)
			{
				// one active var becomes inactive
				
				HK_LCP_IF(debug_lcs)
				{
					hk_Console::get_instance()->printf("\nactive %d becomes inactive (relp %d)\n\n",actives_inactives_ignored[limiting_var],limiting_var);
				}
				
				int indx=actives_inactives_ignored[limiting_var];
				HK_ASSERT( (hk_Math::fabs(full_x[indx]) < 0.001f) || (hk_Math::fabs( smallest_step ) < solver_eps) );
				full_x[indx]=0.0f;
				r_actives--;
				//exchange_activ_inactiv(limiting_var);
				exchange_lcs_variables(r_actives,limiting_var);
				decrement_sub_solver( limiting_var );
			}
			else
			{
				if(limiting_var < ignored_pos)
				{
					// one inactive var becomes active
					if(smallest_step > HK_LCP_SOLVER_EPS)
					{
						move_not_necessary_actives_to_inactives();
					}

					HK_LCP_IF(debug_lcs)
					{
						hk_Console::get_instance()->printf("\ninactive %d becomes active (relp %d)\n\n",actives_inactives_ignored[limiting_var],limiting_var);
					}
					int indx=actives_inactives_ignored[limiting_var];
					HK_ASSERT( hk_Math::fabs(accel[indx]) < 0.001f );
					accel[indx]=0.0f;
					//exchange_activ_inactiv(limiting_var);
					exchange_lcs_variables(r_actives,limiting_var);
					r_actives++;
					increment_sub_solver();
				}
				else
				{
	move_ignored_to_active:
					// ignored_pos comes to the active variables		    
					move_not_necessary_actives_to_inactives();		    
					HK_LCP_IF(debug_lcs)
					{
						hk_Console::get_instance()->printf("\nignored %d becomes active (relp %d)\n\n",actives_inactives_ignored[ignored_pos],ignored_pos);
					}
					int indx=actives_inactives_ignored[limiting_var];
					HK_ASSERT( hk_Math::fabs(accel[indx]) < 0.001f );
					accel[indx]=0.0f;
					if(full_x[indx]<0.0f)
					{ //really necessary ?
						full_x[indx]=0.0f;
					}
					//exchange_activ_inactiv(limiting_var);
					exchange_lcs_variables(r_actives,limiting_var);
					ignored_pos++;
					r_actives++;
					if(ignored_pos<n_variables)
					{
						increment_sub_solver();
					}
					else
					{
						next_numeric_stability_test=0;
					}
					HK_LCP_IF(0)
					{
						debug_test_all_values();
					}
				}
			}
		}
		else
		{
	move_ignored_to_inactive:	    
			// ignored_pos comes to the inactives variables
			did_real_step = 0; //active variables are unchanged
			HK_LCP_IF(debug_lcs)
			{
				hk_Console::get_instance()->printf("ignored %d becomes inactive (relp %d)\n\n",actives_inactives_ignored[ignored_pos],ignored_pos);
			}
			int indx = actives_inactives_ignored[ignored_pos];
			HK_LCP_IF( !(hk_Math::fabs(full_x[indx] ) < 0.001f) )
			{
				hk_Console::get_instance()->printf("error_move_ignored_to_inactive_x_not_zero\n");
			}
			
			full_x[indx] = 0.0f;
			if( accel[indx] < 0.0f)
			{ //really necessary ?
				accel[indx] = 0.0f;
			}
			ignored_pos++;
			if(ignored_pos >= n_variables)
			{
				next_numeric_stability_test=0;
			}	    
			//reset_values();
		}
	}

    HK_LCP_IF(debug_lcs)
	{
		debug_out_lcs();
    }
    
    return HK_OK;
}

//both numbers are in sub-space
void hk_LCP_Solver::exchange_lcs_variables( int first_nr, int second_nr ) {
    HK_ASSERT( first_nr >= 0 );
    HK_ASSERT( second_nr >= 0 );
    HK_ASSERT( first_nr < n_variables );
    HK_ASSERT( second_nr < n_variables );
    int full_space_0,full_space_1;
    full_space_0=actives_inactives_ignored[second_nr];
    full_space_1=actives_inactives_ignored[first_nr];
    actives_inactives_ignored[first_nr]=full_space_0;
    actives_inactives_ignored[second_nr]=full_space_1;
}
//please merge these two functions !!!!
void hk_LCP_Solver::exchange_activ_inactiv( int change_var ) {
    int first_inactiv=r_actives;
    int full_space_0,full_space_1;
    full_space_0=actives_inactives_ignored[change_var];
    full_space_1=actives_inactives_ignored[first_inactiv];
    actives_inactives_ignored[first_inactiv]=full_space_0;
    actives_inactives_ignored[change_var]=full_space_1;
}

//optimization: do not go from 0 to n_variables
//              make two loops: one for actives, one for others
void hk_LCP_Solver::update_step_vars( hk_real step_size ) {
    int i;
    HK_LCP_IF(debug_lcs) {
	hk_Console::get_instance()->printf("deltaa  ");
    }
    for( i = 0; i < n_variables; i++) {
	HK_LCP_IF(debug_lcs) {
	    hk_Console::get_instance()->printf("%f ",delta_accel[i]);
	}
	accel[ i ] = accel[i] + step_size * delta_accel[ i ]; //Vector Optimization possible
	full_x[ i ] = full_x[ i ] + step_size * delta_x[ i ];           //dito
    }
    HK_LCP_IF(debug_lcs) {
	hk_Console::get_instance()->printf("\n");
    }

    //in some cases (e.g. overriding an inactive) acceleration of inactives gets negative
    for( i=r_actives; i<ignored_pos; i++ ) {
	if(accel[actives_inactives_ignored[i]] < 0.0f) {
	    accel[actives_inactives_ignored[i]]=0.0f;  
	}
    }
    //same with actives
    for(i=0;i<r_actives;i++) {
	if(full_x[actives_inactives_ignored[i]] < 0.0f) {
	    full_x[actives_inactives_ignored[i]]=0.0f;
	}
    }
}
    
hk_result hk_LCP_Solver::get_xdirection() {
    // clear all
    int i,k;
    for(i=0;i<aligned_size;i++) {
        this->delta_x[ i ] = 0.0f;
    }

    if(r_actives==0) {
	return HK_OK;
    }

    hk_result ret_val = HK_FAULT;
    

    if(sub_solver_status == 0) {
	int ignored_index = actives_inactives_ignored[ ignored_pos ];

	for(k=0;k<r_actives;k++) {
	    int index=actives_inactives_ignored[k];
	    lu_sub_solver.m_inout_vec[k]= - this->full_A[index * aligned_size + ignored_index];
	}
	lu_sub_solver.solve_lin_equ();
	ret_val = HK_OK;
#if 0	
	HK_LCP_IF(debug_lcs) {
	    sub_solver_mat.columns = r_actives;
	    sub_solver_mat.calc_aligned_row_len();
	    sub_solver_mat.MATRIX_EPS = GAUSS_EPS * 0.001f; //we know our matrix is stable, so it is save to raise EPS
	    inv_mat.columns = r_actives;
	    debug_mat.columns = r_actives;
	    
	    for(i=0;i<r_actives;i++) {
		int index=actives_inactives_ignored[i];
		sub_solver_mat.desired_vector[i]= - this->full_A[index * aligned_size + ignored_index];
		debug_mat.desired_vector[i]= - this->full_A[index * aligned_size + ignored_index];
		int j;
		for(j=0;j<r_actives;j++) {
		    int index2=actives_inactives_ignored[j];
		    sub_solver_mat.matrix_values[i*sub_solver_mat.aligned_row_len + j] = this->full_A[ index * aligned_size + index2 ];
		    inv_mat.matrix_values[i*r_actives + j] = this->full_A[ index * aligned_size + index2 ];
		}
	    }
	    hk_result ret_val2 = sub_solver_mat.solve_great_matrix_many_zero();
	    hk_result ret_val3 = HK_FAULT; //inv_mat.invert(&debug_mat);
	    if( ret_val2 == HK_OK ) {
		for( i=0;i<r_actives;i++ ) {
		    IVP_DOUBLE diff=lu_sub_solver.out_vec[i]-sub_solver_mat.result_vector[i];
		    if(hk_Math::fabs(diff) > 0.001f) {
		      HK_LCP_IF(debug_lcs) {
			hk_Console::get_instance()->printf("gauss_lu_test_fail %f %f should be equal\n",lu_sub_solver.out_vec[i],sub_solver_mat.result_vector[i]);
		      }
		    }
		}
	    } else {
	      HK_LCP_IF(debug_lcs) {