ivp_great_matrix.cxx

hl2 source code. Do not use it illegal.

    for(i=r_actives;i<ignored_pos;i++) {
	printf("%d ",variable_is_found_at[i]);
    }
    printf("  ");
    for(i=ignored_pos;i<n_variables;i++) {
	printf("%d ",variable_is_found_at[i]);
    }
    printf("\n");

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

    full_solver_mat.desired_vector = full_x;
    full_solver_mat.mult_aligned(); // temporary missuse
    full_solver_mat.desired_vector = delta_f;
    
    printf("accel1 ");
    for(i=0;i<n_variables;i++) {
	printf("%.4f ",full_solver_mat.result_vector[i] - full_b[i]);
    }
    printf("\n");

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

//cannot be vector optimized because of indexed access
IVP_RETURN_TYPE IVP_Linear_Constraint_Solver::setup_l_u_solver() {
    int i;
    IVP_IF(0) {
	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.input_vec[i]=full_b[index];
	    IVP_DOUBLE *source = & this->full_A[ index * aligned_size ];
	    IVP_DOUBLE *dest = & lu_sub_solver.U_matrix[ i * lu_sub_solver.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 IVP_Linear_Constraint_Solver::lcs_bubble_sort_x_vals() {
    int r1=0;
    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 > 0) && ( 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;
    }

    IVP_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 ] ] ) {
		printf("lcs_bubble_failed\n");
	    }
	}
    }
}

void IVP_Linear_Constraint_Solver::get_values_when_setup() {
    full_solver_mat.result_vector=reset_accel;
    full_solver_mat.desired_vector=reset_x; //temporary misuse
    full_solver_mat.mult_aligned();
    full_solver_mat.result_vector=delta_accel;
    full_solver_mat.desired_vector=delta_f;

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

    for(i=0;i<r_actives;i++) {
	reset_accel[actives_inactives_ignored[i]]=0.0f; //just to remove rounding errors
    }
    
    for(i=n_variables-1;i>=0;i--) {
	accel[i]=reset_accel[i];
	full_x[i]=reset_x[i];
    }

}

void IVP_Linear_Constraint_Solver::startup_setup(int try_actives) {
    r_actives=try_actives;
    aligned_sub_size=(r_actives + IVP_VECFPU_SIZE-1) & IVP_VECFPU_MASK;
    ignored_pos=try_actives;
    lu_sub_solver.n_sub = r_actives;

    int i;
    for(i=0;i<aligned_size;i++) {
	reset_x[i]=0.0f;
    }
    if(setup_l_u_solver() == IVP_OK) {
	sub_solver_status = 0;
        //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];
		reset_x[index]=lu_sub_solver.out_vec[i];
	    }
	    get_values_when_setup(); //fills x[i] and accel[i]

	    for(i=r_actives-1;i>=0;i--) {
		int index=actives_inactives_ignored[i];
		if(full_x[ index ] < 0.0f){
		    full_x[ index ] = 0.0f;
		    reset_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.input_vec[i]=full_b[index];
			}
			goto begin_of_while;
		    }
		}
	    }
	    break;
	}
    } else {
fast_setup_failed:	
	// sorry fast setup failed
	r_actives=0;
	ignored_pos=0;
	lu_sub_solver.n_sub=0;
    }
}

IVP_RETURN_TYPE IVP_Linear_Constraint_Solver::full_setup() {
    IVP_IF(0) {
	printf("doing_fulll_setup\n");
    }
    
    int i;
    lu_sub_solver.n_sub = r_actives;
    
    for(i=0;i<aligned_size;i++) {
	reset_x[i]=0.0f;
    }

    do_a_little_random_permutation();
    if( setup_l_u_solver() == IVP_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];
	    reset_x[index]=lu_sub_solver.out_vec[i];
	}
    } else {
	sub_solver_status = 2;
	IVP_IF(debug_lcs) {
	    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();
	
	sub_solver_mat.columns = r_actives;
	sub_solver_mat.calc_aligned_row_len();
	
	for(i=0;i<r_actives;i++) {
	    int index=actives_inactives_ignored[i];
	    sub_solver_mat.desired_vector[i]= full_b[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 ];

	    }
	}
	IVP_RETURN_TYPE ret_val = sub_solver_mat.solve_great_matrix_many_zero();

	if(ret_val!=IVP_OK) {
	    IVP_IF(1) {
		printf("error_setup_gauss_failed_too\n");
	    }
	    return ret_val;
	}
    
	for(i=0;i<r_actives;i++) {
	    int index=actives_inactives_ignored[i];
	    reset_x[index]=sub_solver_mat.result_vector[i];
	}
    }

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

void IVP_Linear_Constraint_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 IVP_Linear_Constraint_Solver::full_setup_test_ranges() {
    int illegal_vars=0;
    int i;
    for(i=0;i<r_actives;i++) {
	if( full_x[actives_inactives_ignored[i]] < SOLVER_EPS ) {
	    if( full_x[actives_inactives_ignored[i]] > -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.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 IVP_Linear_Constraint_Solver::debug_test_all_values() {
    sub_solver_mat.columns = r_actives;
    int i;

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


    full_solver_mat.result_vector = reset_accel;
    full_solver_mat.desired_vector = reset_x; //temporary misuse
    full_solver_mat.mult_aligned();
    full_solver_mat.result_vector = delta_accel;
    full_solver_mat.desired_vector = delta_f;

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

    IVP_IF(debug_lcs) {
	for(i=0;i<n_variables;i++) {
	    IVP_DOUBLE diff=IVP_Inline_Math::fabsd( reset_accel[i] - accel[i] );
	    if( diff > 0.001f ) {
		printf("accel_violated at %d %f should be %f\n",i,reset_accel[i],accel[i]);
	    }
	    diff=IVP_Inline_Math::fabsd( full_x[i] - reset_x[i] );
	    if( diff > 0.001f ) {
		printf("x_violated at %d\n",i);
	    }
	}
    }
}

#if 1
IVP_RETURN_TYPE IVP_Great_Matrix_Many_Zero::lu_crout(int *index_vec,IVP_DOUBLE *sign)
{
    /* use desired_vector to store scalings */
    
   int    i, imax = 0, j, k, zeros = 0 ;
   IVP_DOUBLE   sum ;
   IVP_DOUBLE     big, temp ;

   int n_columns = this->columns;
   
   *sign = 1.0f ;

   //Find implicit scaling factors

   for ( i = 0; i < n_columns ; i++ ) {
      big = 0.0f;
      for ( j = 0; j < n_columns; j++ ) {
         temp = IVP_Inline_Math::fabsd( (IVP_DOUBLE) matrix_values[i*n_columns+j] ) ;
         if ( temp > big )
            big = temp ;
      } 
      desired_vector[i] = ( big == 0.0f ? 0.0f : 1.0f / big ) ;
   }

   for ( j = 0; j < n_columns; j++ ) {
      /*************************************
       Run down jth column from top to diag,
       to form the elements of U.
      **************************************/
      for ( i = 0; i < j; i++ ) {
         sum = matrix_values[i*n_columns+j] ;
         for ( k = 0; k < i; k++ ) {
	     sum -= matrix_values[i*n_columns+k] * matrix_values[k*n_columns+j] ; }
         matrix_values[i*n_columns+j] = sum ;
      }
      
      /******************************************
       Run down jth subdiag to form the residuals
       after the elimination of the first j-1
       subdiags.  These residuals divided by the
       appropriate diagonal term will become the
       multipliers in the elimination of the jth.
       subdiag. Find index of largest scaled term
       in imax.
      *******************************************/
      big = 0.0f ;
      for ( i = j; i < n_columns; i++ ) {
         sum = matrix_values[i*n_columns+j] ;
         for ( k = 0; k < j; k++ )
            sum -= matrix_values[i*n_columns+k] * matrix_values[k*n_columns+j] ;
         matrix_values[i*n_columns+j] = sum ;
         temp = desired_vector[i] * ( IVP_Inline_Math::fabsd( sum ) ) ;
         if ( temp >= big ) {
            big = temp ;
            imax = i ;
         } 
      }  
      
      //Permute current row with imax
    
      if ( j != imax ) {
         for ( k = 0; k < n_columns; k++ ) {
            temp = matrix_values[ imax*n_columns+ k ] ;
            matrix_values[ imax*n_columns + k ] = matrix_values[j*n_columns+k] ;
            matrix_values[j*n_columns+k] = temp ;
         } 
         *sign = - *sign ;
         desired_vector[ imax ] = desired_vector[j] ;
      }
      index_vec[j] = imax;
      
      //If diag term is not zero divide subdiag to form multipliers.
      
      if ( IVP_Inline_Math::fabsd( matrix_values[j*n_columns+j] ) < MATRIX_EPS ) 
         zeros++;
      else if ( j != n_columns-1 ) {
         temp = 1.0f / matrix_values[j*n_columns+j] ;
         for ( i = j+1; i < n_columns; i++ )
            matrix_values[i*n_columns+j] *= temp;
      } 
   } 
   if ( zeros ) {
      return IVP_FAULT;
   }
   return IVP_OK;
}

IVP_RETURN_TYPE IVP_Great_Matrix_Many_Zero::lu_inverse(IVP_Great_Matrix_Many_Zero *matrix_out,int *index_vec) {
// solve the Identity matrix column for column to get inverse
    int i,j;
    for(i=0;i<columns;i++) {
	for(j=0;j<columns;j++) {
	    desired_vector[j]=0.0f;
	}
	desired_vector[i]=1.0f;

	if( this->lu_solve(index_vec) == IVP_OK ) {
	    for(j=0;j<columns;j++) {
		matrix_out->matrix_values[j*columns+i]=this->result_vector[j];