ivp_great_matrix.cxx

hl2 source code. Do not use it illegal.

	for(int i=0;i<columns;i++) {
	    IVP_DOUBLE *x = & matrix_values[ i * aligned_row_len ];
	    for(int j=columns; j < aligned_row_len; j++) {
		x[j] = 0.0f;
	    }
	}
	for(int j=columns; j < aligned_row_len; j++) {
	    desired_vector[j] = 0.0f;
	}
    }
}

void IVP_Great_Matrix_Many_Zero::copy_matrix(IVP_DOUBLE *values,IVP_DOUBLE *desired)
{
    for (int d = columns * columns - 1; d>=0; d--){
	values[d] = matrix_values[d];
    }
    for(int j=0;j<columns;j++){
	desired[j]=desired_vector[j];
    }
}

void IVP_Great_Matrix_Many_Zero::find_pivot_in_column(int col)
{
    IVP_DOUBLE best_val = IVP_Inline_Math::fabsd(matrix_values[col*aligned_row_len+col]);
    int pos=-1;
    for(int i=columns-1;i > col;i--)    {
	IVP_DOUBLE h=matrix_values[i*aligned_row_len+col];
	if( IVP_Inline_Math::fabsd(h) > best_val )	{
	    best_val = IVP_Inline_Math::fabsd(h);
	    pos=i;
	}
    }
    if(pos>=0) {
        exchange_rows(col,pos);
    }
}

//optimization: when called from transform_to_lower_null_triangle, begin by 'a' (first values are 0.0f)
void IVP_Great_Matrix_Many_Zero::exchange_rows(int a,int b)
{
    IVP_DOUBLE h;
#if 0 /* without Vector FPU */   
    for(int i=columns-1;i>=0;i--) {
	h=matrix_values[a*aligned_row_len+i];
	matrix_values[a*aligned_row_len+i]=matrix_values[b*aligned_row_len+i];
	matrix_values[b*aligned_row_len+i]=h;
    }
#endif
    IVP_VecFPU::fpu_exchange_rows(&matrix_values[b*aligned_row_len],&matrix_values[a*aligned_row_len],columns,IVP_TRUE);
    
    h=desired_vector[a];
    desired_vector[a]=desired_vector[b];
    desired_vector[b]=h;
}

// solve with Gauss-Algo. : upper triangle matrix with nulls in lower part
void IVP_Great_Matrix_Many_Zero::transform_to_lower_null_triangle()
{
    //matrix_out_before_gauss();
    int i,j;
    
    //j is counting columns, i is counting lines

    //walk through columns
    for(j=0;j<columns;j++)
    {
	IVP_DOUBLE diagonal_elem,negativ_inv_diagonal;
#if 0
	//not always pivot search
	diagonal_elem=matrix_values[j*aligned_row_len+j];
	if(IVP_Inline_Math::fabsd(diagonal_elem)<MATRIX_EPS) {
	    find_pivot_in_column(j);
	    diagonal_elem=matrix_values[j*aligned_row_len+j];
	    if(IVP_Inline_Math::fabsd(diagonal_elem)<MATRIX_EPS) {
		goto column_done;
	    }
	}
#endif
	//always pivot search
	find_pivot_in_column(j);
	diagonal_elem=matrix_values[j*aligned_row_len+j];
	if(IVP_Inline_Math::fabsd(diagonal_elem)<MATRIX_EPS) {
	    goto column_done;
	}	
	
	negativ_inv_diagonal=-1.0f/diagonal_elem;
	//walk through lines
	for(i=j+1;i<columns;i++)
	{
	    IVP_DOUBLE col_down_elem=matrix_values[i*aligned_row_len+j];
	    if(IVP_Inline_Math::fabsd(col_down_elem)>MATRIX_EPS)
	    {
		add_multiple_line(j,i,col_down_elem*negativ_inv_diagonal);
	    }
	}
column_done: ;	
    }

    //now matrix has zeros in lower-left part
    //solve columns from bottom to top
#if 0
    printf("matrix_after_gauss:\n");
    for(j=0;j<columns;j++)
    {
	//walk through lines
	for(i=0;i<columns;i++)
	{
	    printf("%.5f  ",matrix_values[j*columns+i]);
	    old_matrix[j*columns+i]=matrix_values[j*columns+i];
	}
	printf(" x -> %.5f\n",desired_vector[j]);
	res_p[j]=desired_vector[j];
    }
#endif

    //this->test_result(old_matrix,res_p);
    //P_DELETE(old_matrix);
    //return 0;
}


#ifdef DEBUG
void IVP_Great_Matrix_Many_Zero::plausible_input_check() {
    int i,j;
    for(i=0;i<columns;i++) {
	for(j=0;j<columns;j++) {
	    IVP_DOUBLE a=matrix_values[i*aligned_row_len+j];
	    IVP_ASSERT(a>-P_DOUBLE_MAX);
	    IVP_ASSERT(a<P_DOUBLE_MAX);
	}
	IVP_DOUBLE b=desired_vector[i];
	IVP_ASSERT(b>-P_DOUBLE_MAX);
	IVP_ASSERT(b<P_DOUBLE_MAX);
    }
}
#endif

// matrix values are in upper triangle form, solve from bottom to top
IVP_RETURN_TYPE IVP_Great_Matrix_Many_Zero::solve_lower_null_matrix()
{
    int i;
    for(i=columns-1;i>=0;i--)
    {
	IVP_DOUBLE *pointer_in_line=&matrix_values[i*aligned_row_len+columns-1];
     
	IVP_DOUBLE right_side=desired_vector[i];
	for(int j=columns-1;j>i;j--)
	{
	    right_side -= desired_vector[j] * *pointer_in_line;
	    pointer_in_line--;
	}
	IVP_DOUBLE left_side=*pointer_in_line;
	if(IVP_Inline_Math::fabsd(left_side) < MATRIX_EPS){
	    if(IVP_Inline_Math::fabsd(right_side) < MATRIX_EPS*1000.0f)	    {
		// rang(matrix) is not maximal and result space is greater 1
		// choose an arbitrary result element (0.0f)
		//printf("got_gauss_null %f %f\n",left_side,right_side);
		desired_vector[i]=0.0f;
		IVP_IF(0) {
		    printf("gauss_singular_matrix\n");
		}
	    } else {
		// no result possible
		if(1) {
		    i=columns-1;
		    while(i>=0) {
		        result_vector[i]=0.0f; //prevent SUN from detecting rui error
		        i--;
		    }
		}
		return IVP_FAULT;
	    }
	} else {
	    desired_vector[i]=right_side/left_side; // division left_side is not needed, it was done already above (make a vector of inverses)
	    if(desired_vector[i]>10E11f) {
		IVP_IF(1) { printf("\ngauss_failure\n\n"); }
	    }
	}
    }

    for(i=0;i<columns;i++) {
	result_vector[i]=desired_vector[i];
    }
    
    return IVP_OK;
}

void IVP_Great_Matrix_Many_Zero::matrix_multiplication(IVP_DOUBLE *first,IVP_DOUBLE *second) {
    int i,j,k;
    for(i=0;i<columns;i++) {
        for(j=0;j<columns;j++) {
	    IVP_DOUBLE sum=0.0f;
	    for(k=0;k<columns;k++) {
	        IVP_DOUBLE val=first[i*columns+k] * second[k*columns+j];
		sum += val;
	    }
	    matrix_values[i*columns+j] = sum;
        }
    }
}

//for debug only
int IVP_Great_Matrix_Many_Zero::test_result(IVP_DOUBLE *old_matrix,IVP_DOUBLE *old_desired)
{
    //desired_vector and matrix is changed during calculation, old_matrix and old_desired has values needed for test

    IVP_IF(1) {
    int i,j;
    for(j=0;j<columns;j++)
    {
	//walk through lines
	IVP_DOUBLE left_side=0.0f;
	for(i=0;i<columns;i++)
	{
	    left_side+=old_matrix[j*columns+i]*result_vector[i];
	    printf("multip %.5f %.5f  ",old_matrix[j*columns+i],result_vector[i]);
	}
	printf("\nwanted_res was %.5f now %.5f\n",old_desired[j],left_side);
    }
    }
    return 0;
}

IVP_Great_Matrix_Many_Zero::IVP_Great_Matrix_Many_Zero(int n) {
    IVP_ASSERT(0); //does not work any longer: because of Vector-FPU adress has to be aligned
                   //normal malloc + free is no good
    
    MATRIX_EPS=1.0f/10E8f;
    columns=n;
    calc_aligned_row_len();
    matrix_values=(IVP_DOUBLE*)p_malloc(n*aligned_row_len*sizeof(IVP_DOUBLE));
    desired_vector=(IVP_DOUBLE*)p_malloc(aligned_row_len*sizeof(IVP_DOUBLE));
    result_vector=(IVP_DOUBLE*)p_malloc(aligned_row_len*sizeof(IVP_DOUBLE));
}

IVP_Great_Matrix_Many_Zero::IVP_Great_Matrix_Many_Zero() {
    MATRIX_EPS=1.0f/10E8f; 
    matrix_values=NULL;
    desired_vector=NULL;
    result_vector=NULL;
    columns=0;
}

// both matrices are aligned
// my matrix is smaller than big_matrix, copy only those lines/columns that are wanted
// which are wanted is written in array original_pos
void IVP_Great_Matrix_Many_Zero::fill_from_bigger_matrix(IVP_Great_Matrix_Many_Zero *big_matrix,int *original_pos, int n_column)
{
    if (n_column == 0) return;
    if ( original_pos[ n_column-1 ] == n_column-1 ){ // no reorder
	IVP_IF(1){
	    for (int i = 0; i < n_column; i++){
		IVP_ASSERT( original_pos[i] == i);
	    }
	}
	for(int i=0;i<n_column;i++) {
	    IVP_DOUBLE *source = & big_matrix->matrix_values[  original_pos[i]*big_matrix->aligned_row_len ];
	    IVP_DOUBLE *dest = &matrix_values[i * aligned_row_len];
	    IVP_VecFPU::fpu_copy_rows(dest,source, aligned_row_len, IVP_TRUE);
	    this->desired_vector[i] = big_matrix->desired_vector[ original_pos[i] ];	    
	}
    }else{
	for(int i=0;i<n_column;i++) {
	    IVP_DOUBLE *dest = &matrix_values[i * aligned_row_len];
	    IVP_IF(( original_pos[i] != i )) {
		printf("original_pos_diff %d %d\n",i,original_pos[i]);
	    }
	    
	    IVP_DOUBLE *source = & big_matrix->matrix_values[  original_pos[i]*big_matrix->aligned_row_len ];
	    for(int j=0;j<n_column;j++){
		dest[j] = source[ original_pos[j] ];
	    }
	    this->desired_vector[i] = big_matrix->desired_vector[ original_pos[i] ];
	}
    }
}
    
// copies a column from original matrix to sub_matrix
void IVP_Great_Matrix_Many_Zero::copy_to_sub_matrix(IVP_DOUBLE *values_big_matrix,IVP_Great_Matrix_Many_Zero *sub_matrix,int *original_pos)
{
    for(int i=0;i<sub_matrix->columns;i++)
    {
	for(int j=0;j<sub_matrix->columns;j++)
	{
	    sub_matrix->matrix_values[i*sub_matrix->columns+j] = values_big_matrix[ original_pos[i]*columns + original_pos[j] ];
	}
    }
}

void IVP_Great_Matrix_Many_Zero::set_value(IVP_DOUBLE val, int col, int row)
{
    // ATT: slow through mults
    IVP_ASSERT(col>=0 && col<columns);
    IVP_ASSERT(row>=0 && row<columns);

    *(this->matrix_values+col+columns*row) = val;
}


IVP_DOUBLE IVP_Great_Matrix_Many_Zero::get_value(int col, int row) const
{
    // ATT: slow through mults
    IVP_ASSERT(col>=0 && col<columns);
    IVP_ASSERT(row>=0 && row<columns);

    return *(this->matrix_values+col+aligned_row_len*row);
}

int IVP_Great_Matrix_Many_Zero::print_great_matrix(const char *comment) const
{
    ivp_message("Matrix: %s\n", comment);
    int i;
    for(i=0; i<columns; i++){ // rows
	int j;
	for(j=0; j<columns; j++){ // columns
	    IVP_DOUBLE val = this->get_value(j, i);
	    if(IVP_Inline_Math::fabsd(val)<1e-20f){
		ivp_message("    0  ");
	    }else{
		ivp_message("%2.6g  ", val);
	    }
	}
	ivp_message("\n");
    }
    ivp_message("desired ");
    for(i=0;i<columns;i++) {
	ivp_message("%.6f ",desired_vector[i]);
    }
    ivp_message("\n");
    return 0; // for dbx
}

// #+# use vector FPU
void IVP_Great_Matrix_Many_Zero::mult_aligned() {
// can be optimized -> usage of vector FPU
    
	int j; // row
    // for each row
    for(j=0;j<columns;j++)    {
	int i; // col
	//walk through row
	IVP_DOUBLE left_side=0.0f;
	IVP_DOUBLE *source = &matrix_values[ j * aligned_row_len ];
	for(i=0;i<columns;i++)	{
	    left_side += source[i] * desired_vector[i];
	}
	result_vector[j] = left_side;
    }
}

void IVP_Great_Matrix_Many_Zero::mult()
{
    // from desired_vector to result_vector

    // @@OG superslow copypaste, can be optimized!! (rowoffset, revert loop, etc)
    
    int j; // row
    // for each row
    for(j=0;j<columns;j++)
    {
	int i; // col
	//walk through row
	IVP_DOUBLE left_side=0.0f;
	for(i=0;i<columns;i++)
	{
	    left_side+=matrix_values[j*columns+i]*desired_vector[i];
	}
	result_vector[j] = left_side;
    }
}

//can not be vector optimized because of indexed access
void IVP_Linear_Constraint_Solver::mult_active_x_for_accel() {
    int i,j;
    for(i=r_actives;i<n_variables;i++) {
	IVP_DOUBLE sum=0.0f;
	int row_of_A=actives_inactives_ignored[i];
	IVP_DOUBLE *row_vals=&full_solver_mat.matrix_values[ row_of_A * aligned_size ];
	int real_pos;
	for(j=0;j<r_actives;j++) {
	    real_pos=actives_inactives_ignored[j];
	    sum+= delta_f[real_pos]*row_vals[real_pos];
	}
	real_pos=actives_inactives_ignored[ignored_pos];
	sum+=row_vals[real_pos]*1.0f;
	delta_accel[row_of_A]=sum;
    }
}

void IVP_Linear_Constraint_Solver::mult_x_with_full_A_minus_b() {
    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,IVP_TRUE);
	    temp[i] = sum-full_b[i];
    }
}

IVP_BOOL IVP_Linear_Constraint_Solver::numerical_stability_ok() {
    mult_x_with_full_A_minus_b();
    int k;
    IVP_DOUBLE val;
    IVP_DOUBLE worst_error=0.0f;
    for(k=0;k<r_actives;k++) {
	val=IVP_Inline_Math::fabsd(temp[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 zero\n",ignored_pos,val);
	    }
	    return IVP_FALSE;