SPMAT *spICHfactor(SPMAT *A)
#endif
{
int k, m, n, nxt_row, nxt_idx, diag_idx;
Real pivot, tmp2;
SPROW *r_piv, *r_op;
row_elt *elt_piv, *elt_op;
if ( A == SMNULL )
error(E_NULL,"spICHfactor");
if ( A->m != A->n )
error(E_SQUARE,"spICHfactor");
/* set up access paths if not already done so */
if ( ! A->flag_col )
sp_col_access(A);
if ( ! A->flag_diag )
sp_diag_access(A);
m = A->m; n = A->n;
for ( k = 0; k < m; k++ )
{
r_piv = &(A->row[k]);
diag_idx = r_piv->diag;
if ( diag_idx < 0 )
error(E_POSDEF,"spICHfactor");
elt_piv = r_piv->elt;
/* set diagonal entry of Cholesky factor */
tmp2 = elt_piv[diag_idx].val - sprow_sqr(r_piv,k);
if ( tmp2 <= 0.0 )
error(E_POSDEF,"spICHfactor");
elt_piv[diag_idx].val = pivot = sqrt(tmp2);
/* find next row where something (non-trivial) happens */
nxt_row = elt_piv[diag_idx].nxt_row;
nxt_idx = elt_piv[diag_idx].nxt_idx;
/* now set the k-th column of the Cholesky factors */
while ( nxt_row >= 0 && nxt_idx >= 0 )
{
/* nxt_row and nxt_idx give next next row (& index)
of the entry to be modified */
r_op = &(A->row[nxt_row]);
elt_op = r_op->elt;
elt_op[nxt_idx].val = (elt_op[nxt_idx].val -
sprow_ip(r_piv,r_op,k))/pivot;
nxt_row = elt_op[nxt_idx].nxt_row;
nxt_idx = elt_op[nxt_idx].nxt_idx;
}
}
return A;
}
SPMAT *spCHsymb(SPMAT *A)
#endif
{
register int i;
int idx, k, m, minim, n, num_scan, diag_idx, tmp1;
SPROW *r_piv, *r_op;
row_elt *elt_piv, *elt_op, *old_elt;
if ( A == SMNULL )
error(E_NULL,"spCHsymb");
if ( A->m != A->n )
error(E_SQUARE,"spCHsymb");
/* set up access paths if not already done so */
if ( ! A->flag_col )
sp_col_access(A);
if ( ! A->flag_diag )
sp_diag_access(A);
/* printf("spCHsymb() -- checkpoint 1\n"); */
m = A->m; n = A->n;
for ( k = 0; k < m; k++ )
{
r_piv = &(A->row[k]);
if ( r_piv->len > scan_len )
set_scan(r_piv->len);
elt_piv = r_piv->elt;
diag_idx = sprow_idx2(r_piv,k,r_piv->diag);
if ( diag_idx < 0 )
error(E_POSDEF,"spCHsymb");
old_elt = &(elt_piv[diag_idx]);
for ( i = 0; i < r_piv->len; i++ )
{
if ( elt_piv[i].col > k )
break;
col_list[i] = elt_piv[i].col;
scan_row[i] = elt_piv[i].nxt_row;
scan_idx[i] = elt_piv[i].nxt_idx;
}
/* printf("spCHsymb() -- checkpoint 2\n"); */
num_scan = i; /* number of actual entries in scan_row etc. */
/* printf("num_scan = %d\n",num_scan); */
/* now set the k-th column of the Cholesky factors */
/* printf("k = %d\n",k); */
for ( ; ; ) /* forever do... */
{
/* printf("spCHsymb() -- checkpoint 3\n"); */
/* find next row where something (non-trivial) happens
i.e. find min(scan_row) */
minim = n;
for ( i = 0; i < num_scan; i++ )
{
tmp1 = scan_row[i];
/* printf("%d ",tmp1); */
minim = ( tmp1 >= 0 && tmp1 < minim ) ? tmp1 : minim;
}
if ( minim >= n )
break; /* nothing more to do for this column */
r_op = &(A->row[minim]);
elt_op = r_op->elt;
/* set next entry in column k of Cholesky factors */
idx = sprow_idx2(r_op,k,scan_idx[num_scan-1]);
if ( idx < 0 )
{ /* fill-in */
sp_set_val(A,minim,k,0.0);
/* in case a realloc() has occurred... */
elt_op = r_op->elt;
/* now set up column access path again */
idx = sprow_idx2(r_op,k,-(idx+2));
tmp1 = old_elt->nxt_row;
old_elt->nxt_row = minim;
r_op->elt[idx].nxt_row = tmp1;
tmp1 = old_elt->nxt_idx;
old_elt->nxt_idx = idx;
r_op->elt[idx].nxt_idx = tmp1;
}
/* printf("spCHsymb() -- checkpoint 4\n"); */
/* remember current element in column k for column chain */
idx = sprow_idx2(r_op,k,idx);
old_elt = &(r_op->elt[idx]);
/* update scan_row */
/* printf("spCHsymb() -- checkpoint 5\n"); */
/* printf("minim = %d\n",minim); */
for ( i = 0; i < num_scan; i++ )
{
if ( scan_row[i] != minim )
continue;
idx = sprow_idx2(r_op,col_list[i],scan_idx[i]);
if ( idx < 0 )
{ scan_row[i] = -1; continue; }
scan_row[i] = elt_op[idx].nxt_row;
scan_idx[i] = elt_op[idx].nxt_idx;
/* printf("scan_row[%d] = %d\n",i,scan_row[i]); */
/* printf("scan_idx[%d] = %d\n",i,scan_idx[i]); */
}
}
/* printf("spCHsymb() -- checkpoint 6\n"); */
}
return A;
}
VEC *spCHsolve(SPMAT *L, const VEC *b, VEC *out)
#endif
{
int i, j_idx, n, scan_idx, scan_row;
SPROW *row;
row_elt *elt;
Real diag_val, sum, *out_ve;
if ( L == SMNULL || b == VNULL )
error(E_NULL,"spCHsolve");
if ( L->m != L->n )
error(E_SQUARE,"spCHsolve");
if ( b->dim != L->m )
error(E_SIZES,"spCHsolve");
if ( ! L->flag_col )
sp_col_access(L);
if ( ! L->flag_diag )
sp_diag_access(L);
out = v_copy(b,out);
out_ve = out->ve;
/* forward substitution: solve L.x=b for x */
n = L->n;
for ( i = 0; i < n; i++ )
{
sum = out_ve[i];
row = &(L->row[i]);
elt = row->elt;
for ( j_idx = 0; j_idx < row->len; j_idx++, elt++ )
{
if ( elt->col >= i )
break;
sum -= elt->val*out_ve[elt->col];
}
if ( row->diag >= 0 )
out_ve[i] = sum/(row->elt[row->diag].val);
else
error(E_SING,"spCHsolve");
}
/* backward substitution: solve L^T.out = x for out */
for ( i = n-1; i >= 0; i-- )
{
sum = out_ve[i];
row = &(L->row[i]);
/* Note that row->diag >= 0 by above loop */
elt = &(row->elt[row->diag]);
diag_val = elt->val;
/* scan down column */
scan_idx = elt->nxt_idx;
scan_row = elt->nxt_row;
while ( scan_row >= 0 /* && scan_idx >= 0 */ )
{
row = &(L->row[scan_row]);
elt = &(row->elt[scan_idx]);
sum -= elt->val*out_ve[scan_row];
scan_idx = elt->nxt_idx;
scan_row = elt->nxt_row;
}
out_ve[i] = sum/diag_val;
}
return out;
}
//--------------------------------------------------------------------------
SPMAT *Hqp_IpRedSpBKP::sub_CTC(const PERM *px, SPMAT *Q)
// return Q - _CT * diag(_zw) * _CT'
// read: _CT, _zw
// write: _scale
{
int i, j, j_idx, j_end;
int qi, qj, qj_idx;
SPROW *crow, *qrow;
Real sum, val;
IVEC neigh_header;
IVEC *neigh = &neigh_header;
assert(Q->n == Q->m);
assert((int)px->size == Q->m && Q->m >= _n);
if (!Q->flag_diag)
sp_diag_access(Q);
neigh_header.max_dim = 0;
crow = _CT->row;
for (i=0; i<_n; i++, crow++) {
qrow = Q->row + px->pe[i];
if (crow->len <= 0) {
val = qrow->elt[qrow->diag].val;
_scale->ve[i] = min(1.0, sqrt(-1.0 / val));
}
else {
// calculate diagonal entry
sum = sprow_inprod(crow, _zw, crow);
j_idx = qrow->diag;
val = qrow->elt[j_idx].val -= sum;
_scale->ve[i] = min(1.0, sqrt(-1.0 / val));
// calculate resting entries
neigh->ive = _CTC_neighs->ive + _CTC_neigh_start->ive[i];
neigh->dim = _CTC_neigh_start->ive[i + 1] - _CTC_neigh_start->ive[i];
j_end = neigh->dim;
for (j_idx = 0; j_idx < j_end; j_idx++) {
j = neigh->ive[j_idx];
if (j < i) {
sum = sprow_inprod(crow, _zw, _CT->row + j);
qi = px->pe[i];
qj = px->pe[j];
// substract sum from Qij or Qji (entry from upper part only)
if (qi < qj) {
qrow = Q->row + qi;
qj_idx = sprow_idx(qrow, qj);
if (qj_idx < 0) {
// the structure must already have been allocated in init()
m_error(E_INTERN, "Hqp_IpRedSpBKP");
}
else {
qrow->elt[qj_idx].val -= sum;
}
}
else {
qrow = Q->row + qj;
qj_idx = sprow_idx(qrow, qi);
if (qj_idx < 0) {
// the structure must already have been allocated in init()
m_error(E_INTERN, "Hqp_IpRedSpBKP");
}
else {
qrow->elt[qj_idx].val -= sum;
}
}
}
}
}
}
return Q;
}
SPMAT *spLUfactor(SPMAT *A, PERM *px, double alpha)
#endif
{
int i, best_i, k, idx, len, best_len, m, n;
SPROW *r, *r_piv, tmp_row;
STATIC SPROW *merge = (SPROW *)NULL;
Real max_val, tmp;
STATIC VEC *col_vals=VNULL;
if ( ! A || ! px )
error(E_NULL,"spLUfctr");
if ( alpha <= 0.0 || alpha > 1.0 )
error(E_RANGE,"alpha in spLUfctr");
if ( px->size <= A->m )
px = px_resize(px,A->m);
px_ident(px);
col_vals = v_resize(col_vals,A->m);
MEM_STAT_REG(col_vals,TYPE_VEC);
m = A->m; n = A->n;
if ( ! A->flag_col )
sp_col_access(A);
if ( ! A->flag_diag )
sp_diag_access(A);
A->flag_col = A->flag_diag = FALSE;
if ( ! merge ) {
merge = sprow_get(20);
MEM_STAT_REG(merge,TYPE_SPROW);
}
for ( k = 0; k < n; k++ )
{
/* find pivot row/element for partial pivoting */
/* get first row with a non-zero entry in the k-th column */
max_val = 0.0;
for ( i = k; i < m; i++ )
{
r = &(A->row[i]);
idx = sprow_idx(r,k);
if ( idx < 0 )
tmp = 0.0;
else
tmp = r->elt[idx].val;
if ( fabs(tmp) > max_val )
max_val = fabs(tmp);
col_vals->ve[i] = tmp;
}
if ( max_val == 0.0 )
continue;
best_len = n+1; /* only if no possibilities */
best_i = -1;
for ( i = k; i < m; i++ )
{
tmp = fabs(col_vals->ve[i]);
if ( tmp == 0.0 )
continue;
if ( tmp >= alpha*max_val )
{
r = &(A->row[i]);
idx = sprow_idx(r,k);
len = (r->len) - idx;
if ( len < best_len )
{
best_len = len;
best_i = i;
}
}
}
/* swap row #best_i with row #k */
MEM_COPY(&(A->row[best_i]),&tmp_row,sizeof(SPROW));
MEM_COPY(&(A->row[k]),&(A->row[best_i]),sizeof(SPROW));
MEM_COPY(&tmp_row,&(A->row[k]),sizeof(SPROW));
/* swap col_vals entries */
tmp = col_vals->ve[best_i];
col_vals->ve[best_i] = col_vals->ve[k];
col_vals->ve[k] = tmp;
px_transp(px,k,best_i);
r_piv = &(A->row[k]);
for ( i = k+1; i < n; i++ )
{
/* compute and set multiplier */
tmp = col_vals->ve[i]/col_vals->ve[k];
if ( tmp != 0.0 )
sp_set_val(A,i,k,tmp);
else
continue;
/* perform row operations */
merge->len = 0;
r = &(A->row[i]);
sprow_mltadd(r,r_piv,-tmp,k+1,merge,TYPE_SPROW);
idx = sprow_idx(r,k+1);
if ( idx < 0 )
idx = -(idx+2);
/* see if r needs expanding */
if ( r->maxlen < idx + merge->len )
sprow_xpd(r,idx+merge->len,TYPE_SPMAT);
r->len = idx+merge->len;
MEM_COPY((char *)(merge->elt),(char *)&(r->elt[idx]),
merge->len*sizeof(row_elt));
}
}
#ifdef THREADSAFE
sprow_free(merge); V_FREE(col_vals);
#endif
return A;
}
SPMAT *spILUfactor(SPMAT *A, double alpha)
#endif
{
int i, k, idx, idx_piv, m, n, old_idx, old_idx_piv;
SPROW *r, *r_piv;
Real piv_val, tmp;
/* printf("spILUfactor: entered\n"); */
if ( ! A )
error(E_NULL,"spILUfactor");
if ( alpha < 0.0 )
error(E_RANGE,"[alpha] in spILUfactor");
m = A->m; n = A->n;
sp_diag_access(A);
sp_col_access(A);
for ( k = 0; k < n; k++ )
{
/* printf("spILUfactor(l.%d): checkpoint A: k = %d\n",__LINE__,k); */
/* printf("spILUfactor(l.%d): A =\n", __LINE__); */
/* sp_output(A); */
r_piv = &(A->row[k]);
idx_piv = r_piv->diag;
if ( idx_piv < 0 )
{
sprow_set_val(r_piv,k,alpha);
idx_piv = sprow_idx(r_piv,k);
}
/* printf("spILUfactor: checkpoint B\n"); */
if ( idx_piv < 0 )
error(E_BOUNDS,"spILUfactor");
old_idx_piv = idx_piv;
piv_val = r_piv->elt[idx_piv].val;
/* printf("spILUfactor: checkpoint C\n"); */
if ( fabs(piv_val) < alpha )
piv_val = ( piv_val < 0.0 ) ? -alpha : alpha;
if ( piv_val == 0.0 ) /* alpha == 0.0 too! */
error(E_SING,"spILUfactor");
/* go to next row with a non-zero in this column */
i = r_piv->elt[idx_piv].nxt_row;
old_idx = idx = r_piv->elt[idx_piv].nxt_idx;
while ( i >= k )
{
/* printf("spILUfactor: checkpoint D: i = %d\n",i); */
/* perform row operations */
r = &(A->row[i]);
/* idx = sprow_idx(r,k); */
/* printf("spLUfactor(l.%d) i = %d, idx = %d\n",
__LINE__, i, idx); */
if ( idx < 0 )
{
idx = r->elt[old_idx].nxt_idx;
i = r->elt[old_idx].nxt_row;
continue;
}
/* printf("spILUfactor: checkpoint E\n"); */
/* compute and set multiplier */
r->elt[idx].val = tmp = r->elt[idx].val/piv_val;
/* printf("spILUfactor: piv_val = %g, multiplier = %g\n",
piv_val, tmp); */
/* printf("spLUfactor(l.%d) multiplier = %g\n", __LINE__, tmp); */
if ( tmp == 0.0 )
{
idx = r->elt[old_idx].nxt_idx;
i = r->elt[old_idx].nxt_row;
continue;
}
/* idx = sprow_idx(r,k+1); */
/* if ( idx < 0 )
idx = -(idx+2); */
idx_piv++; idx++; /* now look beyond the multiplier entry */
/* printf("spILUfactor: checkpoint F: idx = %d, idx_piv = %d\n",
idx, idx_piv); */
while ( idx_piv < r_piv->len && idx < r->len )
{
/* printf("spILUfactor: checkpoint G: idx = %d, idx_piv = %d\n",
idx, idx_piv); */
if ( r_piv->elt[idx_piv].col < r->elt[idx].col )
idx_piv++;
else if ( r_piv->elt[idx_piv].col > r->elt[idx].col )
idx++;
else /* column numbers match */
{
/* printf("spILUfactor(l.%d) subtract %g times the ",
__LINE__, tmp); */
/* printf("(%d,%d) entry to the (%d,%d) entry\n",
k, r_piv->elt[idx_piv].col,
i, r->elt[idx].col); */
r->elt[idx].val -= tmp*r_piv->elt[idx_piv].val;
idx++; idx_piv++;
}
}
/* bump to next row with a non-zero in column k */
/* printf("spILUfactor(l.%d) column = %d, row[%d] =\n",
__LINE__, r->elt[old_idx].col, i); */
/* sprow_foutput(stdout,r); */
i = r->elt[old_idx].nxt_row;
old_idx = idx = r->elt[old_idx].nxt_idx;
/* printf("spILUfactor(l.%d) i = %d, idx = %d\n", __LINE__, i, idx); */
/* and restore idx_piv to index of pivot entry */
idx_piv = old_idx_piv;
}
}
/* printf("spILUfactor: exiting\n"); */
return A;
}
VEC *spLUTsolve(SPMAT *A, PERM *pivot, const VEC *b, VEC *x)
#endif
{
int i, idx, lim, rownum;
Real sum, *tmp_ve;
/* SPROW *r; */
row_elt *elt;
STATIC VEC *tmp=VNULL;
if ( ! A || ! b )
error(E_NULL,"spLUTsolve");
if ( (pivot != PNULL && A->m != pivot->size) || A->m != b->dim )
error(E_SIZES,"spLUTsolve");
tmp = v_copy(b,tmp);
MEM_STAT_REG(tmp,TYPE_VEC);
if ( ! A->flag_col )
sp_col_access(A);
if ( ! A->flag_diag )
sp_diag_access(A);
lim = min(A->m,A->n);
tmp_ve = tmp->ve;
/* solve U^T.tmp = b */
for ( i = 0; i < lim; i++ )
{
sum = tmp_ve[i];
rownum = A->start_row[i];
idx = A->start_idx[i];
if ( rownum < 0 || idx < 0 )
error(E_SING,"spLUTsolve");
while ( rownum < i && rownum >= 0 && idx >= 0 )
{
elt = &(A->row[rownum].elt[idx]);
sum -= elt->val*tmp_ve[rownum];
rownum = elt->nxt_row;
idx = elt->nxt_idx;
}
if ( rownum != i )
error(E_SING,"spLUTsolve");
elt = &(A->row[rownum].elt[idx]);
if ( elt->val == 0.0 )
error(E_SING,"spLUTsolve");
tmp_ve[i] = sum/elt->val;
}
/* now solve L^T.tmp = (old) tmp */
for ( i = lim-1; i >= 0; i-- )
{
sum = tmp_ve[i];
rownum = i;
idx = A->row[rownum].diag;
if ( idx < 0 )
error(E_NULL,"spLUTsolve");
elt = &(A->row[rownum].elt[idx]);
rownum = elt->nxt_row;
idx = elt->nxt_idx;
while ( rownum < lim && rownum >= 0 && idx >= 0 )
{
elt = &(A->row[rownum].elt[idx]);
sum -= elt->val*tmp_ve[rownum];
rownum = elt->nxt_row;
idx = elt->nxt_idx;
}
tmp_ve[i] = sum;
}
if ( pivot != PNULL )
x = pxinv_vec(pivot,tmp,x);
else
x = v_copy(tmp,x);
#ifdef THREADSAFE
V_FREE(tmp);
#endif
return x;
}