ivp_great_matrix.cxx

hl2 source code. Do not use it illegal.

	}
    }
    for(k=r_actives; k<n_variables; k++) {
	val=IVP_Inline_Math::fabsd( temp[actives_inactives_ignored[k]] - accel[actives_inactives_ignored[k]] );
	IVP_IF(1) {
	    if(worst_error<val) {
		worst_error=val;
	    }
	}
	if( val > TEST_EPS ) {
	    IVP_IF(debug_lcs) {
		printf("numerical_unstable %d %.10f should be %.10f\n",ignored_pos,temp[actives_inactives_ignored[k]],accel[actives_inactives_ignored[k]]);
	    }
	    return IVP_FALSE;
	}
    }
    IVP_IF(0) {
	printf("numerical_worst %.5e\n",worst_error);
    }
    return IVP_TRUE;
}

void IVP_Linear_Constraint_Solver::alloc_memory(IVP_U_Memory *my_mem) {
    actives_inactives_ignored=(int*)my_mem->get_mem(sizeof(int)*aligned_size);
    variable_is_found_at=(int*)my_mem->get_mem(sizeof(int)*aligned_size);

    accel =        (IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    delta_accel =  (IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    delta_f     =  (IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    reset_x     =  (IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    reset_accel =  (IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);

    lu_sub_solver.L_matrix  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size*n_variables);
    lu_sub_solver.U_matrix  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size*n_variables);
    lu_sub_solver.input_vec =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    lu_sub_solver.out_vec   =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    lu_sub_solver.temp_vec  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    lu_sub_solver.mult_vec  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);

    sub_solver_mat.matrix_values  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size*n_variables);
    sub_solver_mat.desired_vector =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    sub_solver_mat.result_vector  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    
    inv_mat.matrix_values  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size*n_variables);
    inv_mat.desired_vector =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
    inv_mat.result_vector  =(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);

    IVP_IF(1){ // fill LU for fast align access
	for (int j = 0; j < n_variables; j++){
	    IVP_DOUBLE *L = &lu_sub_solver.L_matrix[ j * aligned_size ];
	    IVP_DOUBLE *U = &lu_sub_solver.U_matrix[ j * aligned_size ];
	    IVP_DOUBLE *S = &sub_solver_mat.matrix_values[ j * aligned_size ];
	    IVP_DOUBLE *M = &inv_mat.matrix_values[ j * aligned_size ];
	    for (int i = 0 /*n_variables*/; i< aligned_size; i++){
		L[i] = 0.0f;
		U[i] = 0.0f;
		S[i] = 0.0f;
		M[i] = 0.0f;
	    }
	}
	for (int k = n_variables; k< aligned_size; k++){
	    reset_accel[k] = 0.0f;
	}
    }
}

IVP_RETURN_TYPE IVP_Linear_Constraint_Solver::init_and_solve_lc(IVP_DOUBLE *A_in,IVP_DOUBLE *b_in,IVP_DOUBLE *result_vec_out,int var_num,int actives_at_begin,IVP_U_Memory *my_mem) {
    n_variables=var_num;
    aligned_size=(n_variables+IVP_VECFPU_SIZE-1) & IVP_VECFPU_MASK;

    alloc_memory(my_mem);
    
    r_actives=0;
    ignored_pos=0;
    SOLVER_EPS=1E-7f; //do not make this one bigger
    GAUSS_EPS=SOLVER_EPS;
    TEST_EPS=SOLVER_EPS*1000.0f;
    sub_solver_status=0;
    MAX_STEP_LEN=10000;
    
    lu_sub_solver.aligned_row_len=aligned_size;
    lu_sub_solver.n_sub=0;
    lu_sub_solver.MATRIX_EPS=SOLVER_EPS*10; // makes incrementel lu more often to fail but prevents from getting numerical inaccurate

    temp=lu_sub_solver.temp_vec;
    
    sub_solver_mat.MATRIX_EPS = GAUSS_EPS; // for gauss a more liberal EPS is enough (otherwise it may happen the unilateral solver to fail completely)
                                            // sorry, but for fdirection its not acceptable to have very large values
    
    inv_mat.MATRIX_EPS = SOLVER_EPS;
    
    IVP_IF(1) {
	debug_mat.matrix_values=(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size*var_num);
	debug_mat.desired_vector=(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
	debug_mat.result_vector=(IVP_DOUBLE*)my_mem->get_mem(sizeof(IVP_DOUBLE)*aligned_size);
	debug_mat.MATRIX_EPS = SOLVER_EPS;
    }
    
    full_solver_mat.columns = var_num;
    full_solver_mat.aligned_row_len = aligned_size;
    full_solver_mat.matrix_values = A_in;
    full_solver_mat.desired_vector = delta_f;
    full_solver_mat.result_vector = delta_accel;
    full_solver_mat.MATRIX_EPS = SOLVER_EPS;
    
    int i;
    for(i=0;i<aligned_size;i++) {
	actives_inactives_ignored[i]=i;
	variable_is_found_at[i] = i;
	result_vec_out[i] = 0.0f;
	accel[i] = -b_in[i];
    }

    full_A = A_in;
    full_b = b_in;
    full_x = result_vec_out;

    first_permute_index=0;
    second_permute_index=0;
    first_permute_ignored=0;
    second_permute_ignored=0;
    
    debug_no_lu_count=0;
    debug_lcs=0;
    
    IVP_IF(debug_lcs) {
	printf("\n***********************************************************\n");
	start_debug_lcs();
    }

    startup_setup( actives_at_begin );
    IVP_RETURN_TYPE ret_val = solve_lc();

    IVP_IF((debug_no_lu_count>0) && (debug_lcs)) {
	printf("sum_lu_failed %d\n",debug_no_lu_count);
    }
    
    IVP_IF(ret_val==IVP_OK) {
	int j;
	full_solver_mat.desired_vector=full_x;
	full_solver_mat.mult_aligned();
	for(j=0;j<n_variables;j++) {
	    if(full_solver_mat.result_vector[j] + 0.0001f < full_b[j]) {
		printf("linear_constraint_failed %d %f should be greater %f\n",j,full_solver_mat.result_vector[j],full_b[j]);
	    }
	}
	for(j=0;j<r_actives;j++) {
	    int index_full=actives_inactives_ignored[j];
	    IVP_DOUBLE diff=IVP_Inline_Math::fabsd(full_solver_mat.result_vector[index_full] - full_b[index_full]);
	    if(diff>0.0001f) {
		printf("linear_constraint_failed %d %f should be equal %f\n",j,full_solver_mat.result_vector[index_full],full_b[index_full]);
	    }
	}
    }
    
    full_solver_mat.result_vector = NULL;
    full_solver_mat.desired_vector = NULL;
    full_solver_mat.matrix_values = NULL;
    sub_solver_mat.result_vector = NULL;
    sub_solver_mat.desired_vector = NULL;
    sub_solver_mat.matrix_values = NULL;
    inv_mat.result_vector = NULL;
    inv_mat.desired_vector = NULL;
    inv_mat.matrix_values = NULL;
    IVP_IF(1) {
	debug_mat.result_vector = NULL;
	debug_mat.desired_vector = NULL;
	debug_mat.matrix_values = NULL;
    }
    
    return ret_val;
}


void IVP_Linear_Constraint_Solver::decrement_sub_solver( int sub_pos ) {
    if(sub_solver_status==0) {
	IVP_ASSERT( lu_sub_solver.n_sub == r_actives + 1 );
	if( lu_sub_solver.decrement_l_u( sub_pos ) != IVP_OK ) {
	    IVP_IF(1) {
		printf("\n\ndecrement_lu_failed_need_setup\n\n");
	    }
	    sub_solver_status=2;
	}
    } else {
	lu_sub_solver.n_sub--;
    }
}

void IVP_Linear_Constraint_Solver::increment_sub_solver() {
    int k;
    int new_var_orig = actives_inactives_ignored[ r_actives-1 ];
    if(sub_solver_status==0) {
	IVP_ASSERT( lu_sub_solver.n_sub == r_actives - 1);
	for(k=0;k<r_actives;k++) {
	    lu_sub_solver.U_matrix[ lu_sub_solver.n_sub * lu_sub_solver.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.input_vec[k] = full_A[ aligned_size * actives_inactives_ignored[ k ] + new_var_orig ];
	}
	if( lu_sub_solver.increment_l_u() != IVP_OK ) {
	    IVP_IF(debug_lcs) {
		printf("\n\nincrement_lu_failed_need_setup\n\n");
	    }
	    sub_solver_status=2;
	}
    } else {
	lu_sub_solver.n_sub++;
    }
}

void IVP_Linear_Constraint_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 is distributed alternatively )

    if( r_actives < 2) {
	return;
    }

    first_permute_index +=1;
    second_permute_index +=2;

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

    first_permute_ignored +=1;
    second_permute_ignored +=2;

    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 IVP_Linear_Constraint_Solver::move_not_necessary_actives_to_inactives() {
    int i;
    for( i=0;i<r_actives;i++ ) {
	if( full_x[ actives_inactives_ignored[i] ] < SOLVER_EPS ) {
	    //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--;
	}
    }
}

void IVP_Linear_Constraint_Solver::increase_step_count(int *step_count) {
    (*step_count)++;
}

// 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)
IVP_RETURN_TYPE IVP_Linear_Constraint_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 = IVP_NUMERIC_TEST_EVERY_N;

    //printf("\n\n\n\n\ndoing_lcs_ %d\n",n_variables);
    
    IVP_DOUBLE solver_eps = SOLVER_EPS;
    
    while(1) {
	if(did_real_step) {
	    increase_step_count(&step_count);
	}
	if(step_count > IVP_UNILATERAL_SOLVER_MAX_STEP) {
	    IVP_IF(1) {
	        printf("\n\ngiveup_stepcount_linear_constraint_solver\n\n\n");
	    }
	    return IVP_FAULT;
	}
        int limiting_var;
	
	if( next_numeric_stability_test == 0 ) {
	    if( (numerical_stability_ok() == IVP_FALSE) ) {
perform_full_setup:
		increase_step_count(&step_count);
    		if(step_count > IVP_UNILATERAL_SOLVER_MAX_STEP) {
		    IVP_IF(1) {
			printf("\n\ngiveup_stepcount_linear_constraint_solver\n\n\n");
		    }
		    return IVP_FAULT;
		}
		nr_zero_steps=0;
		if( full_setup() == IVP_FAULT ) {
		    return IVP_FAULT;
		}
	    }
	    next_numeric_stability_test = IVP_NUMERIC_TEST_EVERY_N;
	} else {
	    next_numeric_stability_test--;
	}
	
	if( ignored_pos >= n_variables ) {
	    return IVP_OK;
	}
	
	int ignored_full_index = actives_inactives_ignored[ ignored_pos ];

	IVP_IF(debug_lcs) {
	    int k;
	    for(k=0;k<r_actives;k++) {
		int idx=actives_inactives_ignored[k];
		IVP_IF( IVP_Inline_Math::fabsd( accel[idx] ) > 0.01f ) {
		    printf("incorrect_accel %f at %d  should be zero\n",accel[idx],idx);
		}
		IVP_IF( IVP_Inline_Math::fabsd( full_x[idx] < solver_eps )) {
		    if ( IVP_Inline_Math::fabsd( full_x[ignored_full_index] ) < solver_eps) {
			printf("active_var %d shouldnt be active but inactive\n",idx);
		    }
		}
		if( full_x[idx]<0.0f ) {
		    IVP_IF(1) {
			printf("active_var %d should be positive %f\n",idx,full_x[idx]);
		    }
		}
	    }
	}
	IVP_IF(1) {
	    int k;
	    for(k=r_actives;k<ignored_pos;k++) {
		int idx=actives_inactives_ignored[k];
		IVP_IF( IVP_Inline_Math::fabsd( full_x[idx] ) > 0.1f ) {
		    printf("incorrect_x %f at %d  should be zero\n",accel[idx],idx);
		}
		if(full_x[idx]<-0.001f) {
		    printf("incorrect_negative_x %f at %d  should be zero\n",accel[idx],idx);
		}
		if( accel[idx] < 0.0f ) {
		    printf("warning_inactive_neg accel %d %f\n",idx,accel[idx]);
		}	
	    }
	}
	
	IVP_IF(debug_lcs) {
	    debug_out_lcs();
	}

	IVP_IF( IVP_Inline_Math::fabsd( 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()==IVP_OK) {
		    sub_solver_status=0; 
		} else {
		    IVP_IF(debug_lcs) {
		    printf("setup_lu_notok\n");
		    }
		}
	    } else {
		sub_solver_status = 1; //ok then next time
	    }
	}
	
	if( IVP_Inline_Math::fabsd( 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( IVP_Inline_Math::fabsd( 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) {
	    
	    IVP_DOUBLE smallest_step;
	    
	    //first r vars are clamped
	    //compute x with Acc x = b
	    if(get_fdirection() == IVP_FAULT) {
		IVP_IF(debug_lcs) {
		    printf("lcs_fdirection_total_fail\n");
		}
		goto perform_full_setup;
	    }
	    delta_f[ ignored_full_index ] = 1.0f;
	    
	    //delta_f is now filled in original var space

	    IVP_IF(0) {
		printf("fullmat:\n");
		full_solver_mat.print_great_matrix("");
		printf("\n");
	    }
	    	    
	    //full_solver_mat.mult();  //delta_accel = A * delta_f
	    //delta_accel is now filled in original var space
	    mult_active_x_for_accel();