ivp_great_matrix.cxx

hl2 source code. Do not use it illegal.

	    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
	    }
	    
	    IVP_IF(0) {
		for(int j=0;j<r_actives;j++) {
		    int full_index = actives_inactives_ignored[ j ];
		    printf("should be zero %f\n",delta_accel[full_index]);
		}
	    }
	    
	    // now find smallest steps
	    
	    // step to make clamped active out of ignored_pos
	    IVP_IF( accel[ ignored_full_index ] > 0.0f ) {
		printf("errror_accel_ignored_negativ\n");
	    }
	    if( -accel[ ignored_full_index ] < delta_accel[ ignored_full_index ] * MAX_STEP_LEN ) {
		smallest_step = - accel[ignored_full_index] / delta_accel[ ignored_full_index ];
		IVP_ASSERT( smallest_step > 0.0f );
		limiting_var = ignored_pos;
	    } else {
		limiting_var = -1;
		smallest_step = P_DOUBLE_MAX;
	    }

#if 0
	    if( delta_accel[ ignored_full_index ] > solver_eps ) {
		smallest_step = - accel[ignored_full_index] / delta_accel[ ignored_full_index ];
		IVP_ASSERT( smallest_step > 0.0f );
		limiting_var = ignored_pos;
	    } else {
		limiting_var = -1;
		smallest_step = P_DOUBLE_MAX;
	    }
#endif
	    // smallest step of clamped
	    for(i=0;i<r_actives;i++) {
		index = actives_inactives_ignored[i];
		IVP_DOUBLE del_f = delta_f[ index ];
		if( del_f < - solver_eps ) {
		    IVP_DOUBLE local_step = - full_x[ index ] / del_f;
		    if( (IVP_Inline_Math::fabsd( 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];
		IVP_DOUBLE del_a = delta_accel[ index ];
		if( del_a < -SOLVER_EPS ) {
		    IVP_DOUBLE local_step = - accel[ index ] / del_a;
		    if( local_step < smallest_step - solver_eps ) { // make it harder to move from inactive to active
			IVP_IF(debug_lcs) {
			    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( (IVP_Inline_Math::fabsd( local_step ) > solver_eps ) || (limiting_var==ignored_pos) || 1) {
			    smallest_step = local_step;
			    limiting_var = i;
			}
		    }
		}
	    }

	    if(smallest_step < 0.0f) {
		IVP_IF(1) {
		    printf("error_stepsize_negative %f\n",smallest_step);
		}
		smallest_step=0.0f;
	    }	
	    
	    if( limiting_var < 0 ) {
		return IVP_FAULT;
	    }

	    IVP_IF(debug_lcs) {
		printf("smallest_step_is %f\n",smallest_step);
	    }

limiting_var_found:
	    if(smallest_step > MAX_STEP_LEN) {
		goto perform_full_setup;
	    }
	    if(smallest_step < 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
		
		IVP_IF(debug_lcs) {
		    printf("\nactive %d becomes inactive (relp %d)\n\n",actives_inactives_ignored[limiting_var],limiting_var);
		}
		
		int indx=actives_inactives_ignored[limiting_var];
		IVP_ASSERT( (IVP_Inline_Math::fabsd(full_x[indx]) < 0.001f) || (IVP_Inline_Math::fabsd( 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 > SOLVER_EPS) {
			move_not_necessary_actives_to_inactives();
		    }

		    IVP_IF(debug_lcs) {
			printf("\ninactive %d becomes active (relp %d)\n\n",actives_inactives_ignored[limiting_var],limiting_var);
		    }
		    int indx=actives_inactives_ignored[limiting_var];
		    IVP_ASSERT( IVP_Inline_Math::fabsd(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();		    
		    IVP_IF(debug_lcs) {
			printf("\nignored %d becomes active (relp %d)\n\n",actives_inactives_ignored[ignored_pos],ignored_pos);
		    }
		    int indx=actives_inactives_ignored[limiting_var];
		    IVP_ASSERT( IVP_Inline_Math::fabsd(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;
		    }
		    IVP_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
	    IVP_IF(debug_lcs) {
		printf("ignored %d becomes inactive (relp %d)\n\n",actives_inactives_ignored[ignored_pos],ignored_pos);
	    }
	    int indx=actives_inactives_ignored[ignored_pos];
	    IVP_IF( !(IVP_Inline_Math::fabsd(full_x[indx] ) < 0.001f) ) {
		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();
	}
    }

    IVP_IF(debug_lcs) {
	debug_out_lcs();
    }
    
    return IVP_OK;
}

//both numbers are in sub-space
void IVP_Linear_Constraint_Solver::exchange_lcs_variables( int first_nr, int second_nr ) {
    IVP_IF(1) {
	IVP_ASSERT(variable_is_found_at[ actives_inactives_ignored[ second_nr ] ] == second_nr );
	IVP_ASSERT(variable_is_found_at[ actives_inactives_ignored[ first_nr ] ] == first_nr );
    }
    IVP_ASSERT( first_nr >= 0 );
    IVP_ASSERT( second_nr >= 0 );
    IVP_ASSERT( first_nr < n_variables );
    IVP_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;
    variable_is_found_at[full_space_0]=first_nr;
    variable_is_found_at[full_space_1]=second_nr;
}
//please merge these two functions !!!!
void IVP_Linear_Constraint_Solver::exchange_activ_inactiv( int change_var ) {
    IVP_IF(1) {
	IVP_ASSERT(variable_is_found_at[ actives_inactives_ignored[ change_var ] ] == change_var );
	IVP_ASSERT(variable_is_found_at[ actives_inactives_ignored[ r_actives ] ] == r_actives );
    }
    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;
    variable_is_found_at[full_space_0]=first_inactiv;
    variable_is_found_at[full_space_1]=change_var;
}

//optimization: do not go from 0 to n_variables
//              make two loops: one for actives, one for others
void IVP_Linear_Constraint_Solver::update_step_vars( IVP_DOUBLE step_size ) {
    int i;
    IVP_IF(debug_lcs) {
	printf("deltaa  ");
    }
    for( i = 0; i < n_variables; i++) {
	IVP_IF(debug_lcs) {
	    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_f[ i ];           //dito
    }
    IVP_IF(debug_lcs) {
	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;
	}
    }
}
    
IVP_RETURN_TYPE IVP_Linear_Constraint_Solver::get_fdirection() {
    // clear all
    int i,k;
    for(i=0;i<aligned_size;i++) {
        this->delta_f[ i ] = 0.0f;
    }

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

    IVP_RETURN_TYPE ret_val = IVP_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.input_vec[k]= - this->full_A[index * aligned_size + ignored_index];
	}
	lu_sub_solver.solve_lin_equ();
	ret_val = IVP_OK;
	
	IVP_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 ];
		}
	    }
	    IVP_RETURN_TYPE ret_val2 = sub_solver_mat.solve_great_matrix_many_zero();
	    IVP_RETURN_TYPE ret_val3 = IVP_FAULT; //inv_mat.invert(&debug_mat);
	    if( ret_val2 == IVP_OK ) {
		for( i=0;i<r_actives;i++ ) {
		    IVP_DOUBLE diff=lu_sub_solver.out_vec[i]-sub_solver_mat.result_vector[i];
		    if(IVP_Inline_Math::fabsd(diff) > 0.001f) {
		      IVP_IF(debug_lcs) {
			printf("gauss_lu_test_fail %f %f should be equal\n",lu_sub_solver.out_vec[i],sub_solver_mat.result_vector[i]);
		      }
		    }
		}
	    } else {
	      IVP_IF(debug_lcs) {
		printf("gauss_lu_test_gauss_invalid\n");
	      }
	    }
	    if( ret_val3 == IVP_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(IVP_Inline_Math::fabsd(diff) > 0.001f) {
		      IVP_IF(debug_lcs) {
			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;
	}
    } else {
	IVP_IF((debug_lcs)) {
	    printf("sub_lu_is_off %d\n",r_actives);
	}
	debug_no_lu_count++;
	sub_solver_mat.columns = r_actives;
	sub_solver_mat.calc_aligned_row_len();
	int ignored_index = actives_inactives_ignored[ ignored_pos ];
    
	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];

	    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 ];

	    }
	}
	ret_val = sub_solver_mat.solve_great_matrix_many_zero();
	for( i=0;i<r_actives;i++ ) {
	    lu_sub_solver.out_vec[i]=sub_solver_mat.result_vector[i]; //Vector Optimization possible
	}
    }
    
    //copy clamped
    for(i=0;i<r_actives;i++) {
	this->delta_f[ actives_inactives_ignored[i] ] = lu_sub_solver.out_vec[i];
    }

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

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

void IVP_Linear_Constraint_Solver::debug_out_lcs() {
    printf("r_actives %d  ignored_nr %d\n",r_actives,ignored_pos);
    int i;

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