spbkp.c

Meschach 可以解稠密或稀疏线性方程组、计算特征值和特征向量和解最小平方问题

	{	/* do 2 x 2 pivot */
	    int	idx_k, idx1_k, s_idx, s_idx2;
	    int	old_col;
	    row_elt	*e_tmp;

	    r_piv = &(A->row[i]);
	    idx_piv = unord_get_idx(r_piv,i);
	    aii = aip1i = 0.0;
	    e_tmp = r_piv->elt;
	    for ( idx_piv = 0; idx_piv < r_piv->len; idx_piv++, e_tmp++ )
		if ( e_tmp->col == i )
		    aii = e_tmp->val;
	        else if ( e_tmp->col == i+1 )
		    aip1i = e_tmp->val;

	    r1_piv = &(A->row[i+1]);
	    e_tmp = r1_piv->elt;
	    aip1 = unord_get_val(A,i+1,i+1);
	    det = aii*aip1 - aip1i*aip1i;	/* Must have det < 0 */
	    if ( aii == 0.0 && aip1i == 0.0 )
	    {
		/* error(E_RANGE,"spBKPfactor"); */
		onebyone = TRUE;
		continue;	/* cannot pivot */
	    }

	    if ( det == 0.0 )
	    {
		if ( aii != 0.0 )
		    error(E_RANGE,"spBKPfactor");
		onebyone = TRUE;
		continue;	/* cannot pivot */
	    }
	    aip1i = aip1i/det;
	    aii = aii/det;
	    aip1 = aip1/det;
	    
	    /* initialise scan_... etc for the 2 x 2 pivot */
	    s_idx = r_piv->len + r1_piv->len;
	    scan_row = iv_resize(scan_row,s_idx);
	    scan_idx = iv_resize(scan_idx,s_idx);
	    col_list = iv_resize(col_list,s_idx);
	    orig_idx = iv_resize(orig_idx,s_idx);
	    orig1_idx = iv_resize(orig1_idx,s_idx);

	    e = r_piv->elt;
	    for ( idx = 0; idx < r_piv->len; idx++, e++ )
	    {
		scan_row->ive[idx] = i;
		scan_idx->ive[idx] = idx;
		col_list->ive[idx] = e->col;
		orig_idx->ive[idx] = idx;
		orig1_idx->ive[idx] = -1;
	    }
	    e = r_piv->elt;
	    e1 = r1_piv->elt;
	    for ( idx = 0; idx < r1_piv->len; idx++, e1++ )
	    {
		scan_row->ive[idx+r_piv->len] = i+1;
		scan_idx->ive[idx+r_piv->len] = idx;
		col_list->ive[idx+r_piv->len] = e1->col;
		orig_idx->ive[idx+r_piv->len] = -1;
		orig1_idx->ive[idx+r_piv->len] = idx;
	    }

	    e1 = r1_piv->elt;
	    order = px_resize(order,scan_row->dim);
	    px_ident(order);
	    iv_sort(col_list,order);
	    tmp_iv = iv_resize(tmp_iv,scan_row->dim);
	    for ( idx = 0; idx < order->size; idx++ )
		tmp_iv->ive[idx] = scan_idx->ive[order->pe[idx]];
	    iv_copy(tmp_iv,scan_idx);
	    for ( idx = 0; idx < order->size; idx++ )
		tmp_iv->ive[idx] = scan_row->ive[order->pe[idx]];
	    iv_copy(tmp_iv,scan_row);
	    for ( idx = 0; idx < scan_row->dim; idx++ )
		tmp_iv->ive[idx] = orig_idx->ive[order->pe[idx]];
	    iv_copy(tmp_iv,orig_idx);
	    for ( idx = 0; idx < scan_row->dim; idx++ )
		tmp_iv->ive[idx] = orig1_idx->ive[order->pe[idx]];
	    iv_copy(tmp_iv,orig1_idx);

	    s_idx = 0;
	    old_col = -1;
	    for ( idx = 0; idx < scan_row->dim; idx++ )
	    {
		if ( col_list->ive[idx] == old_col )
		{
		    if ( scan_row->ive[idx] == i )
		    {
			scan_row->ive[s_idx-1] = scan_row->ive[idx];
			scan_idx->ive[s_idx-1] = scan_idx->ive[idx];
			col_list->ive[s_idx-1] = col_list->ive[idx];
			orig_idx->ive[s_idx-1] = orig_idx->ive[idx];
			orig1_idx->ive[s_idx-1] = orig1_idx->ive[idx-1];
		    }
		    else if ( idx > 0 )
		    {
			scan_row->ive[s_idx-1] = scan_row->ive[idx-1];
			scan_idx->ive[s_idx-1] = scan_idx->ive[idx-1];
			col_list->ive[s_idx-1] = col_list->ive[idx-1];
			orig_idx->ive[s_idx-1] = orig_idx->ive[idx-1];
			orig1_idx->ive[s_idx-1] = orig1_idx->ive[idx];
		    }
		}
		else
		{
		    scan_row->ive[s_idx] = scan_row->ive[idx];
		    scan_idx->ive[s_idx] = scan_idx->ive[idx];
		    col_list->ive[s_idx] = col_list->ive[idx];
		    orig_idx->ive[s_idx] = orig_idx->ive[idx];
		    orig1_idx->ive[s_idx] = orig1_idx->ive[idx];
		    s_idx++;
		}
		old_col = col_list->ive[idx];
	    }
	    scan_row = iv_resize(scan_row,s_idx);
	    scan_idx = iv_resize(scan_idx,s_idx);
	    col_list = iv_resize(col_list,s_idx);
	    orig_idx = iv_resize(orig_idx,s_idx);
	    orig1_idx = iv_resize(orig1_idx,s_idx);

	    /* for ( j = i+2; j < n; j++ )  { .... row operation .... } */
	    for ( s_idx = 0; s_idx < scan_row->dim; s_idx++ )
	    {
		int	idx_piv, idx1_piv;
		Real	aip1j, aij, aik, aip1k;
		row_elt	*e_ik, *e_ip1k;

		j = col_list->ive[s_idx];
		if ( j < i+2 )
		    continue;
		tracecatch(scan_to(A,scan_row,scan_idx,col_list,j),
			   "spBKPfactor");

		idx_piv = orig_idx->ive[s_idx];
		aij = ( idx_piv < 0 ) ? 0.0 : r_piv->elt[idx_piv].val;
		/* aij = ( s_idx < r_piv->len ) ? r_piv->elt[s_idx].val :
		    0.0; */
		/* aij   = sp_get_val(A,i,j); */
		idx1_piv = orig1_idx->ive[s_idx];
		aip1j = ( idx1_piv < 0 ) ? 0.0 : r1_piv->elt[idx1_piv].val;
		/* aip1j = ( s_idx < r_piv->len ) ? 0.0 :
		    r1_piv->elt[s_idx-r_piv->len].val; */
		/* aip1j = sp_get_val(A,i+1,j); */
		s = - aip1i*aip1j + aip1*aij;
		t = - aip1i*aij + aii*aip1j;

		/* for ( k = j; k < n; k++ )  { .... update entry .... } */
		row = &(A->row[j]);
		/* set idx_k and idx1_k indices */
		s_idx2 = s_idx;
		k = col_list->ive[s_idx2];
		idx_k = orig_idx->ive[s_idx2];
		idx1_k = orig1_idx->ive[s_idx2];

		while ( s_idx2 < scan_row->dim )
		{
		    k = col_list->ive[s_idx2];
		    idx_k = orig_idx->ive[s_idx2];
		    idx1_k = orig1_idx->ive[s_idx2];
		    e_ik = ( idx_k < 0 ) ? (row_elt *)NULL :
			&(r_piv->elt[idx_k]);
		    e_ip1k = ( idx1_k < 0 ) ? (row_elt *)NULL :
			&(r1_piv->elt[idx1_k]);
		    aik = ( idx_k >= 0 ) ? e_ik->val : 0.0;
		    aip1k = ( idx1_k >= 0 ) ? e_ip1k->val : 0.0;
		    if ( scan_row->ive[s_idx2] == j )
		    {	/* no fill-in */
			row = &(A->row[j]);
			/* idx = sprow_idx(row,k); */
			idx = scan_idx->ive[s_idx2];
			if ( idx < 0 )
			    error(E_INTERN,"spBKPfactor");
			row->elt[idx].val -= s*aik + t*aip1k;
		    }
		    else
		    {	/* fill-in -- insert entry & patch column */
			Real	tmp;
			int	old_row, old_idx;
			row_elt	*old_e, *new_e;

			tmp = - s*aik - t*aip1k;
			if ( tmp != 0.0 )
			{
			    row = &(A->row[j]);
			    old_row = scan_row->ive[s_idx2];
			    old_idx = scan_idx->ive[s_idx2];

			    idx = row->len;
			    if ( row->len >= row->maxlen )
			    {  tracecatch(sprow_xpd(row,2*row->maxlen+1,
						    TYPE_SPMAT),
					   "spBKPfactor");	    }

			    row->len = idx + 1;
			    /* idx = sprow_idx(row,k); */
			    new_e = &(row->elt[idx]);
			    new_e->val = tmp;
			    new_e->col = k;

			    if ( old_row < 0 )
				error(E_INTERN,"spBKPfactor");
			    /* old_idx = sprow_idx2(&(A->row[old_row]),
						  k,old_idx); */
			    old_e = &(A->row[old_row].elt[old_idx]);
			    new_e->nxt_row = old_e->nxt_row;
			    new_e->nxt_idx = old_e->nxt_idx;
			    old_e->nxt_row = j;
			    old_e->nxt_idx = idx;
			}
		    }

		    /* update idx_k, idx1_k, s_idx2 etc */
		    s_idx2++;
		}

		/* store multipliers -- may involve fill-in (!) */
		/* idx = sprow_idx(r_piv,j); */
		idx = orig_idx->ive[s_idx];
		if ( idx >= 0 )
		{
		    r_piv->elt[idx].val = s;
		}
		else if ( s != 0.0 )
		{
		    int		old_row, old_idx;
		    row_elt	*new_e, *old_e;

		    old_row = -1;	old_idx = j;

		    if ( i > 0 )
		    {
			tracecatch(chase_col(A,j,&old_row,&old_idx,i-1),
				   "spBKPfactor");
		    }
		    /* sprow_set_val(r_piv,j,s); */
		    idx = r_piv->len;
		    if ( r_piv->len >= r_piv->maxlen )
		    {	tracecatch(sprow_xpd(r_piv,2*r_piv->maxlen+1,
					     TYPE_SPMAT),
				   "spBKPfactor");		    }

		    r_piv->len = idx + 1;
		    /* idx = sprow_idx(r_piv,j); */
		    /* if ( idx < 0 )
			error(E_INTERN,"spBKPfactor"); */
		    new_e = &(r_piv->elt[idx]);
		    new_e->val = s;
		    new_e->col = j;
		    if ( old_row < 0 )
		    {
			new_e->nxt_row = A->start_row[j];
			new_e->nxt_idx = A->start_idx[j];
			A->start_row[j] = i;
			A->start_idx[j] = idx;
		    }
		    else
		    {
			/* old_idx = sprow_idx2(&(A->row[old_row]),j,old_idx);*/
			if ( old_idx < 0 )
			    error(E_INTERN,"spBKPfactor");
			old_e = &(A->row[old_row].elt[old_idx]);
			new_e->nxt_row = old_e->nxt_row;
			new_e->nxt_idx = old_e->nxt_idx;
			old_e->nxt_row = i;
			old_e->nxt_idx = idx;
		    }
		}
		/* idx1 = sprow_idx(r1_piv,j); */
		idx1 = orig1_idx->ive[s_idx];
		if ( idx1 >= 0 )
		{
		    r1_piv->elt[idx1].val = t;
		}
		else if ( t != 0.0 )
		{
		    int		old_row, old_idx;
		    row_elt	*new_e, *old_e;

		    old_row = -1;	old_idx = j;
		    tracecatch(chase_col(A,j,&old_row,&old_idx,i),
			       "spBKPfactor");
		    /* sprow_set_val(r1_piv,j,t); */
		    idx1 = r1_piv->len;
		    if ( r1_piv->len >= r1_piv->maxlen )
		    {	tracecatch(sprow_xpd(r1_piv,2*r1_piv->maxlen+1,
					     TYPE_SPMAT),
				   "spBKPfactor");		    }

		    r1_piv->len = idx1 + 1;
		    /* idx1 = sprow_idx(r1_piv,j); */
		    /* if ( idx < 0 )
			error(E_INTERN,"spBKPfactor"); */
		    new_e = &(r1_piv->elt[idx1]);
		    new_e->val = t;
		    new_e->col = j;
		    if ( idx1 < 0 )
			error(E_INTERN,"spBKPfactor");
		    new_e = &(r1_piv->elt[idx1]);
		    if ( old_row < 0 )
		    {
			new_e->nxt_row = A->start_row[j];
			new_e->nxt_idx = A->start_idx[j];
			A->start_row[j] = i+1;
			A->start_idx[j] = idx1;
		    }
		    else
		    {
			old_idx = sprow_idx2(&(A->row[old_row]),j,old_idx);
			if ( old_idx < 0 )
			    error(E_INTERN,"spBKPfactor");
			old_e = &(A->row[old_row].elt[old_idx]);
			new_e->nxt_row = old_e->nxt_row;
			new_e->nxt_idx = old_e->nxt_idx;
			old_e->nxt_row = i+1;
			old_e->nxt_idx = idx1;
		    }
		}
	    }
	}
    }

    /* now sort the rows arrays */
    for ( i = 0; i < A->m; i++ )
	qsort(A->row[i].elt,A->row[i].len,sizeof(row_elt),(int(*)())col_cmp);
    A->flag_col = A->flag_diag = FALSE;

#ifdef	THREADSAFE
    IV_FREE(scan_row);	IV_FREE(scan_idx);	IV_FREE(col_list);
    IV_FREE(tmp_iv);	IV_FREE(deg_list);	IV_FREE(orig_idx);
    IV_FREE(orig1_idx);	PX_FREE(order);
#endif
    return A;
}

/* spBKPsolve -- solves A.x = b where A has been factored a la BKPfactor()
   -- returns x, which is created if NULL */
#ifndef ANSI_C
VEC	*spBKPsolve(A,pivot,block,b,x)
SPMAT	*A;
PERM	*pivot, *block;
VEC	*b, *x;
#else
VEC	*spBKPsolve(SPMAT *A, PERM *pivot, PERM *block,
		    const VEC *b, VEC *x)
#endif
{
    STATIC VEC	*tmp=VNULL;	/* dummy storage needed */
    int		i /* , j */, n, onebyone;
    int		row_num, idx;
    Real	a11, a12, a22, b1, b2, det, sum, *tmp_ve, tmp_diag;
    SPROW	*r;
    row_elt	*e;
    
    if ( ! A || ! pivot || ! block || ! b )
	error(E_NULL,"spBKPsolve");
    if ( A->m != A->n )
	error(E_SQUARE,"spBKPsolve");
    n = A->n;
    if ( b->dim != n || pivot->size != n || block->size != n )
	error(E_SIZES,"spBKPsolve");
    x = v_resize(x,n);
    tmp = v_resize(tmp,n);
    MEM_STAT_REG(tmp,TYPE_VEC);
    
    tmp_ve = tmp->ve;

    if ( ! A->flag_col )
	sp_col_access(A);

    px_vec(pivot,b,tmp);
    /* printf("# BKPsolve: effect of pivot: tmp =\n");	v_output(tmp); */

    /* solve for lower triangular part */
    for ( i = 0; i < n; i++ )
    {
	sum = tmp_ve[i];
	if ( block->pe[i] < i )
	{
	    /* for ( j = 0; j < i-1; j++ )
		  sum -= A_me[j][i]*tmp_ve[j]; */
	    row_num = -1;	idx = i;
	    e = bump_col(A,i,&row_num,&idx);
	    while ( row_num >= 0 && row_num < i-1 )
	    {
		sum -= e->val*tmp_ve[row_num];
		e = bump_col(A,i,&row_num,&idx);
	    }
	}
	else
	{
	    /* for ( j = 0; j < i; j++ )
	          sum -= A_me[j][i]*tmp_ve[j]; */
	    row_num = -1; idx = i;
	    e = bump_col(A,i,&row_num,&idx);
	    while ( row_num >= 0 && row_num < i )
	    {
		sum -= e->val*tmp_ve[row_num];
		e = bump_col(A,i,&row_num,&idx);
	    }
	}
	tmp_ve[i] = sum;
    }

    /* printf("# BKPsolve: solving L part: tmp =\n");	v_output(tmp); */
    /* solve for diagonal part */
    for ( i = 0; i < n; i = onebyone ? i+1 : i+2 )
    {
	onebyone = ( block->pe[i] == i );
	if ( onebyone )
	{
	    /* tmp_ve[i] /= A_me[i][i]; */
	    tmp_diag = sp_get_val(A,i,i);
	    if ( tmp_diag == 0.0 )
		error(E_SING,"spBKPsolve");
	    tmp_ve[i] /= tmp_diag;
	}
	else
	{
	    a11 = sp_get_val(A,i,i);
	    a22 = sp_get_val(A,i+1,i+1);
	    a12 = sp_get_val(A,i,i+1);
	    b1 = tmp_ve[i];
	    b2 = tmp_ve[i+1];
	    det = a11*a22-a12*a12;	/* < 0 : see BKPfactor() */
	    if ( det == 0.0 )
		error(E_SING,"BKPsolve");
	    det = 1/det;
	    tmp_ve[i]   = det*(a22*b1-a12*b2);
	    tmp_ve[i+1] = det*(a11*b2-a12*b1);
	}
    }

    /* printf("# BKPsolve: solving D part: tmp =\n");	v_output(tmp); */
    /* solve for transpose of lower triangular part */
    for ( i = n-2; i >= 0; i-- )
    {
	sum = tmp_ve[i];
	if ( block->pe[i] > i )
	{
	    /* onebyone is false */
	    /* for ( j = i+2; j < n; j++ )
		  sum -= A_me[i][j]*tmp_ve[j]; */
	    if ( i+2 >= n )
		continue;
	    r = &(A->row[i]);
	    idx = sprow_idx(r,i+2);
	    idx = fixindex(idx);
	    e = &(r->elt[idx]);
	    for ( ; idx < r->len; idx++, e++ )
		sum -= e->val*tmp_ve[e->col];
	}
	else /* onebyone */
	{
	    /* for ( j = i+1; j < n; j++ )
		  sum -= A_me[i][j]*tmp_ve[j]; */
	    r = &(A->row[i]);
	    idx = sprow_idx(r,i+1);
	    idx = fixindex(idx);
	    e = &(r->elt[idx]);
	    for ( ; idx < r->len; idx++, e++ )
		sum -= e->val*tmp_ve[e->col];
	}
	tmp_ve[i] = sum;
    }

    /* printf("# BKPsolve: solving L^T part: tmp =\n");v_output(tmp); */
    /* and do final permutation */
    x = pxinv_vec(pivot,tmp,x);

#ifdef	THREADSAFE
    V_FREE(tmp);
#endif

    return x;
}