lcp_solver.cpp

hl2 source code. Do not use it illegal.

#include <hk_math/vecmath.h>

#include <hk_math/densematrix.h>
#include <hk_math/gauss_elimination/gauss_elimination.h>
#include <hk_math/incr_lu/incr_lu.h>
#include <hk_math/vector_fpu/vector_fpu.h>
#include <hk_math/lcp/lcp_solver.h>

//#define HK_DEBUG
#ifdef HK_DEBUG
#	define HK_LCP_IF(flag)  if (flag)
#else
#	define HK_LCP_IF(flag)	if (0==1)
#endif

#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <ctype.h>

#define HK_LCP_SOLVER_EPS (1E-7f)
#define HK_LCP_TEST_EPS (HK_LCP_SOLVER_EPS*1000.0f)
#define HK_LCP_NUMERIC_TEST_EVERY_N 7
#define HK_LCP_MAX_STEP_LEN (10000.0f)
#define HK_LCP_REAL_MAX 1e20f
#define MAX_ERROR_BUFFER_LEN 1024


/* Algorithm explanation:
   The algorithm implemented here is basically the Algorithm described in
   David Baraff: "Fast Contact Force Computation for Nonpenetrating Rigid Bodies", SIGGRAPH '94
   The difference is we use here a incremental LU solution for the active variables
   In case the LU is invalid - e.g. numerical problems, Sub-Matrix is singular, ... - we use a
   gaussian elemination to get delta_x values of active variables per step (very costly :( )
   
   Optimization used here is to say what variables should be active at start. We than 
   try to setup the LU solver and start with an set of actives right from the start.

   The algorithm tries to solve full_A*full_x >= full_b, where full_x is the solution
   constraints: full_x >= 0.0 and if full_x[i]>0 than equation i has to hold
   Modification: with n_equations you can specify that the first part of A and x are treated as a system of
                 equations this means full_A*full_x = full_b for i<= n_equations. full_x is then allowed to be negative
   
   During all computations the most important variables are:
     vector int:  active_inactive_ignored (has information about what variables are active,inactive or not processed yet)
     integer      r_actives (number of active variables)
     integer      ignored_pos (number of next unknown to process)
   Given that information we can calculate all other information during a setup in case numerical errors accumulate.
   The most valuable thing to lose in that setup is the LU information. We can recompute it with
   lu_sub_solver::l_u_decomposition_with_pivoting() but that costs 3X a gaussian elemination
   
   lu_sub_solver: here is stored the LU decompostion of the sub Matrix A of active variables
                  in case an active variable is changed to inactive we call lu_sub_solver::decrement_l_u
		  in case an inactive/ignored variable becomes active we call lu_sub_solver::increment_l_u

   accel: stores vector values full_A*full_x - full_b 
          has to be zero for the active variables and >zero for inactives and some known value for ignored variables

   delta_x: during a step holds the changes of x when moving 1.0 with the latest ignored variable
            has to be zero for inactive variables (their x values are and shall stay zero)

   delta_accel: during a step holds the changes of accel when moving 1.0 with the latest ignored variable
                has to be zero for active variables (their accels are and shall stay zero)


   if accel > 0.0 for next ignored variable, we can move that variable to inactives. Else we have to do a standard step

   standard step:
     step begins with call to 'get_xdirection'. There the delta_x values are calculated. Delta_x for new ignored
     variable is 1.0 for the rest of the actives delta_x is unknown.
     So A*delta_x = delta_accel
        A* (? ? .. ? 1.0) = ( 0 0 0 .. 0 alpha )  sub matrix A is (r_actives+1)*(r_actives+1)
	A* delta_x + ai = ( 0 0 0 0 0 0 0)  sub matrix A is (r_actives*r_actives) and ai is ith column of A
	we have to solve:
	                   A * delta_x = -ai
        we solve this with our LU solver or with gauss-eleminiation

     After we have delta_x we calculate delta_accel: delta_accel = A * delta_x (and get value alpha)

     Then we calculate step size s:
     s = -accel[i]/delta_x[i] for current ignored variable to fullfill A*x>=b
     s = -full_x/delta_x for actives to avoid x-values getting negative
     s = -accel/delta_accel for inactives to avoid >=b violated
     Three possibilites: ignored variable becomes active, one active becomes inactive, one inactive becomes active
     Then we have to update accel and full_x and do next step


// */


//can not be vector optimized because of indexed access
void hk_LCP_Solver::mult_active_x_for_accel() {
    int i,j;
    for(i=r_actives;i<n_variables;i++) {
	hk_real sum=0.0f;
	int row_of_A=actives_inactives_ignored[i];
	hk_real *row_vals=&full_A[ row_of_A * aligned_size ];
	int real_pos;
	for(j=0;j<r_actives;j++) {
	    real_pos=actives_inactives_ignored[j];
	    sum+= delta_x[real_pos]*row_vals[real_pos];
	}
	real_pos=actives_inactives_ignored[ignored_pos];
	sum+=row_vals[real_pos];
	delta_accel[row_of_A]=sum;
    }
}

void hk_LCP_Solver::mult_x_with_full_A_minus_b() {
#if 0
    for(int i=0;i<n_variables;i++) {
	    IVP_DOUBLE *base_a=&full_A[i*aligned_size];
	    IVP_DOUBLE sum=IVP_VecFPU::fpu_large_dot_product(base_a,full_x,n_variables,HK_TRUE);
	    temp[i] = sum-full_b[i];
    }
#endif
    input_struct->m_A->mult_vector(full_x,temp);
    hk_VecFPU::fpu_add_multiple_row(temp,full_b,-1.0f,n_variables,HK_TRUE);
}

//quite costly, because we have to multiply with the full matrix
// we check wether the active variables match with the right side full_b
//    and we check wether the inactives are greater equal the right side full_b
hk_bool hk_LCP_Solver::numerical_stability_ok() {
    mult_x_with_full_A_minus_b();
    int k;
    hk_real val;
    for(k=0;k<r_actives;k++) {
	val=hk_Math::fabs(temp[actives_inactives_ignored[k]]);
	if( val > HK_LCP_TEST_EPS ) {
	    return HK_FALSE;
	}
    }
    for(k=r_actives; k<n_variables; k++) {
	val=hk_Math::fabs( temp[actives_inactives_ignored[k]] - accel[actives_inactives_ignored[k]] );
	if( val > HK_LCP_TEST_EPS ) {
	    HK_LCP_IF(debug_lcs) {
		hk_Console::get_instance()->printf("numerical_unstable %d %.10f should be %.10f\n",ignored_pos,temp[actives_inactives_ignored[k]],accel[actives_inactives_ignored[k]]);
	    }
	    return HK_FALSE;
	}
    }
    return HK_TRUE;
}

hk_result hk_LCP_Solver::solve_lcp(hk_LCP_Input &in,hk_LCP_Temp &tmp,hk_LCP_Output &out) {
    input_struct=&in;
    m_gauss_inp.m_A=(hk_gauss_real*)tmp.m_L_matrix; //aligned Matrix
    m_gauss_inp.m_b=(hk_gauss_real*)tmp.m_temp_vec; //aligned vector  
    m_gauss_out.m_result=(hk_gauss_real*)tmp.m_inout_vec; //aligned vector
    //Gauss solver shares all its memory with LU_Solver. Gauss is only needed when LU is invalid

    lu_sub_solver.m_L_matrix=tmp.m_L_matrix; //aligned Matrix
    lu_sub_solver.m_U_matrix=tmp.m_U_matrix; //aligned Matrix	
    lu_sub_solver.m_inout_vec=tmp.m_inout_vec; //aligned vector
    lu_sub_solver.m_temp_vec=tmp.m_temp_vec;  //aligned vector

    full_A=in.m_A->getRealPointer(); //aligned matrix, points to input
    full_b=in.m_b; //aligned vector, points to input
    full_x=out.m_result; //aligned vector, points to output
    n_variables=in.m_n_variables;
    n_equations=in.m_n_equations;
    aligned_size=hk_VecFPU::calc_aligned_row_len(n_variables,full_A);
    
    r_actives=0;
    ignored_pos=0;

    sub_solver_status=0;
    
    lu_sub_solver.m_aligned_row_len=aligned_size;
    lu_sub_solver.m_n_sub=0;
    
    m_gauss_inp.m_gauss_eps=HK_LCP_SOLVER_EPS;     // for gauss a more liberal EPS is enough (otherwise it may happen the unilateral solver to fail completely)
                                            // sorry, but for xdirection its not acceptable to have very large values
    
    m_max_steps_lcp=3*in.m_n_variables;

    actives_inactives_ignored=tmp.m_actives_inactives_ignored;
    accel =        tmp.m_accel;
    delta_accel =  tmp.m_delta_accel;
    delta_x     =  tmp.m_delta_f;
    temp  =        (hk_real*)tmp.m_temp_vec;

    int i;
    for(i=0;i<aligned_size;i++)
	{
		actives_inactives_ignored[i]=i;
		full_x[i] = 0.0f;
		accel[i] = -full_b[i];
    }

    first_permute_index=0;
    second_permute_index=0;
    first_permute_ignored=0;
    second_permute_ignored=0;
    
    debug_no_lu_count=0;
    debug_lcs = 0;

#if 0 //only for debug
    HK_LCP_IF(debug_lcs)
	{
		hk_Console::get_instance()->printf("\n***********************************************************\n");
		start_debug_lcs();
    }
#endif

    startup_setup( in.m_n_actives_at_start );
    hk_result ret_val = solve_lc();

    for(i=r_actives-1;i>=0;i--) {
	out.m_active[actives_inactives_ignored[i]]=HK_TRUE;
    }
    for(i=r_actives;i<n_variables;i++) {
	out.m_active[this->actives_inactives_ignored[i]]=HK_FALSE;
    }

    //only for debug
    HK_LCP_IF((debug_no_lu_count>0) && (debug_lcs)) {
	hk_Console::get_instance()->printf("sum_lu_failed %d\n",debug_no_lu_count);
    }
    
    HK_LCP_IF(ret_val==HK_OK) {
	input_struct->m_A->mult_vector( out.m_result, temp );

	int j;
	for(j=0;j<n_variables;j++) {
	    if(temp[j] + 0.0001f < full_b[j]) {
		hk_Console::get_instance()->printf("linear_constraint_failed %d %f should be greater %f\n",j,temp[j],full_b[j]);
	    }
	}
	for(j=0;j<r_actives;j++) {
	    int index_full=actives_inactives_ignored[j];
	    hk_real diff=hk_Math::fabs(temp[index_full] - full_b[index_full]);
	    if(diff>0.0001f) {
		hk_Console::get_instance()->printf("linear_constraint_failed %d %f should be equal %f\n",j,temp[index_full],full_b[index_full]);
	    }
	}
    }    
    
    return ret_val;
}


void hk_LCP_Solver::decrement_sub_solver( int sub_pos ) {
    if(sub_solver_status==0) {
	HK_ASSERT( lu_sub_solver.m_n_sub == r_actives + 1 );
	if( lu_sub_solver.decrement_l_u( sub_pos ) != HK_OK ) {
#if 0
	    HK_LCP_IF(1) {
		hk_Console::get_instance()->printf("\n\ndecrement_lu_failed_need_setup\n\n");
	    }
#endif
	    sub_solver_status=2;
	}
    } else {
	lu_sub_solver.m_n_sub--;
    }
}

void hk_LCP_Solver::increment_sub_solver() {
    int k;
    int new_var_orig = actives_inactives_ignored[ r_actives-1 ];
    if(sub_solver_status==0) {
	HK_ASSERT( lu_sub_solver.m_n_sub == r_actives - 1);
	for(k=0;k<r_actives;k++) {
	    lu_sub_solver.m_U_matrix[ lu_sub_solver.m_n_sub * lu_sub_solver.m_aligned_row_len + k ] = full_A[ new_var_orig*aligned_size + actives_inactives_ignored[ k ] ]; 
	}
	for(k=0;k<r_actives-1;k++) {
	    lu_sub_solver.m_inout_vec[k] = full_A[ aligned_size * actives_inactives_ignored[ k ] + new_var_orig ];
	}
	if( lu_sub_solver.increment_l_u() != HK_OK ) {
	    HK_LCP_IF(debug_lcs) {
		hk_Console::get_instance()->printf("\n\nincrement_lu_failed_need_setup\n\n");
	    }
	    sub_solver_status=2;
	}
    } else {
	lu_sub_solver.m_n_sub++;
    }
}

void hk_LCP_Solver::do_a_little_random_permutation() {
    // in case numeric gets unstable, neverending loops can occur
    // do some permutions to get fresh values ( in gauss the last variable gets zero, this "wrong" values are distributed alternatively )

    if( r_actives-n_equations < 2) {
	goto permute_free_variables;
    }

    first_permute_index +=1;
    second_permute_index +=2;

    while(first_permute_index >= (r_actives-n_equations)) {
	first_permute_index-=(r_actives-n_equations);
    }
    while(second_permute_index >= (r_actives-n_equations)) {
	second_permute_index-=(r_actives-n_equations);
    }
    exchange_lcs_variables( first_permute_index+n_equations, second_permute_index+n_equations );

    first_permute_ignored +=1;
    second_permute_ignored +=2;

permute_free_variables:
    int free_space=n_variables-ignored_pos-1;
    if(free_space<2) {
	return;
    }
    while(first_permute_ignored >= free_space) {
	first_permute_ignored-=free_space;
    }
    while(second_permute_ignored >= free_space) {
	second_permute_ignored-=free_space;
    }
    exchange_lcs_variables( first_permute_ignored+ignored_pos+1, second_permute_ignored+ignored_pos+1 );

}

void hk_LCP_Solver::move_not_necessary_actives_to_inactives() {
    int i;
    for( i=n_equations;i<r_actives;i++ ) {
	if( full_x[ actives_inactives_ignored[i] ] < HK_LCP_SOLVER_EPS ) {
	    //hk_Console::get_instance()->printf("juhu_removed_not_necessary_active %d full %d\n",i,actives_inactives_ignored[i]);
	    full_x[ actives_inactives_ignored[i] ]=0.0f;
	    exchange_lcs_variables( i, r_actives-1 );
	    r_actives--;
	    decrement_sub_solver( i );
	    i--;
	}
    }
}

// actives_inactives_ignored : array to transform index from small variable space (that is growing) to full variable space
// first are active variables: their accel - val is 0.0f and x val is > 0.0f
// then inactives: their x val is 0.0f and accel val is > 0.0f
// last ignored: x and accel unknown, first of ignored is processed

// doctrin:
// when step size and variables are near to 0.0f than try to move corresponding variable into inactive variables (makes algorithm faster (sub matrix small) and robust (sub matrix not singular)) 
// but moving variables with x=0.0f into active cannot be avoided: sometimes it is necessary when moving first ignored into actives (as second step)
hk_result hk_LCP_Solver::solve_lc()
{
    int index; //var to go from sub space to full space
    int step_count=0;
    int nr_zero_steps=0; //to avoid endless loops
    int did_real_step=0; //real step means that active variables have changed
    int next_numeric_stability_test = HK_LCP_NUMERIC_TEST_EVERY_N;

    //hk_Console::get_instance()->printf("\n\n\n\n\ndoing_lcs_ %d\n",n_variables);
    
    hk_incrlu_real solver_eps = HK_LCP_SOLVER_EPS;
    
    while(1)
	{
		if(did_real_step)
		{
			increase_step_count(&step_count);
		}
		if(step_count > m_max_steps_lcp)
		{
			HK_LCP_IF(1)
			{
				hk_Console::get_instance()->printf("\n\ngiveup_stepcount_linear_constraint_solver\n\n\n");
			}
			return HK_FAULT;
		}
		
		int limiting_var;
		
		if( next_numeric_stability_test == 0 )
		{
			if( (numerical_stability_ok() == HK_FALSE) )
			{
perform_full_setup:
				increase_step_count(&step_count);
    			if(step_count > m_max_steps_lcp)
				{
					HK_LCP_IF(1)
					{
						hk_Console::get_instance()->printf("\n\ngiveup_stepcount_linear_constraint_solver\n\n\n");
					}
					return HK_FAULT;
				}
				
				nr_zero_steps=0;
				if( full_setup() == HK_FAULT )
				{
					return HK_FAULT;
				}
			}
			next_numeric_stability_test = HK_LCP_NUMERIC_TEST_EVERY_N;
		}
		else
		{
			next_numeric_stability_test--;
		}
		
		if( ignored_pos >= n_variables )
		{
			return HK_OK;
		}
		
		int ignored_full_index = actives_inactives_ignored[ ignored_pos ];

		HK_LCP_IF(debug_lcs)
		{
			int k;
			for(k=n_equations;k<r_actives;k++)
			{
				int idx=actives_inactives_ignored[k];
				HK_LCP_IF( hk_Math::fabs( accel[idx] ) > 0.01f )
				{
					hk_Console::get_instance()->printf("incorrect_accel %f at %d  should be zero\n",accel[idx],idx);
				}
				HK_LCP_IF( hk_Math::fabs( full_x[idx] < solver_eps ))
				{
					if ( hk_Math::fabs( full_x[ignored_full_index] ) < solver_eps)
					{
						hk_Console::get_instance()->printf("active_var %d shouldnt be active but inactive\n",idx);
					}
				}
				if( full_x[idx]<0.0f )
				{
					HK_LCP_IF(1)
					{
						hk_Console::get_instance()->printf("active_var %d should be positive %f\n",idx,full_x[idx]);
					}
				}
			}
		}
		HK_LCP_IF(1)
		{