static
char
rcsid[] =
"$Id: spchfctr.c,v 1.4 1994/01/13 05:31:32 des Exp $"
;
#include <stdio.h>
#include "sparse2.h"
#include <math.h>
#ifndef MALLOCDECL
#ifndef ANSI_C
extern
char
*
calloc
(), *
realloc
();
#endif
#endif
double
sprow_ip(row1, row2, lim)
SPROW *row1, *row2;
int
lim;
{
int
idx1, idx2, len1, len2, tmp;
int
sprow_idx();
register
row_elt *elts1, *elts2;
register
Real sum;
elts1 = row1->elt; elts2 = row2->elt;
len1 = row1->len; len2 = row2->len;
sum = 0.0;
if
( len1 <= 0 || len2 <= 0 )
return
0.0;
if
( elts1->col >= lim || elts2->col >= lim )
return
0.0;
idx1 = idx2 = 0;
if
( len1 > 2*len2 )
{
idx1 = sprow_idx(row1,elts2->col);
idx1 = (idx1 < 0) ? -(idx1+2) : idx1;
if
( idx1 < 0 )
error(E_UNKNOWN,
"sprow_ip"
);
len1 -= idx1;
}
else
if
( len2 > 2*len1 )
{
idx2 = sprow_idx(row2,elts1->col);
idx2 = (idx2 < 0) ? -(idx2+2) : idx2;
if
( idx2 < 0 )
error(E_UNKNOWN,
"sprow_ip"
);
len2 -= idx2;
}
if
( len1 <= 0 || len2 <= 0 )
return
0.0;
elts1 = &(elts1[idx1]); elts2 = &(elts2[idx2]);
for
( ; ; )
{
if
( (tmp=elts1->col-elts2->col) < 0 )
{
len1--; elts1++;
if
( ! len1 || elts1->col >= lim )
break
;
}
else
if
( tmp > 0 )
{
len2--; elts2++;
if
( ! len2 || elts2->col >= lim )
break
;
}
else
{
sum += elts1->val * elts2->val;
len1--; elts1++;
len2--; elts2++;
if
( ! len1 || ! len2 ||
elts1->col >= lim || elts2->col >= lim )
break
;
}
}
return
sum;
}
double
sprow_sqr(row, lim)
SPROW *row;
int
lim;
{
register
row_elt *elts;
int
idx, len;
register
Real sum, tmp;
sum = 0.0;
elts = row->elt; len = row->len;
for
( idx = 0; idx < len; idx++, elts++ )
{
if
( elts->col >= lim )
break
;
tmp = elts->val;
sum += tmp*tmp;
}
return
sum;
}
static
int
*scan_row = (
int
*)NULL, *scan_idx = (
int
*)NULL,
*col_list = (
int
*)NULL;
static
int
scan_len = 0;
int
set_scan(new_len)
int
new_len;
{
if
( new_len <= scan_len )
return
scan_len;
if
( new_len <= scan_len+5 )
new_len += 5;
if
( ! scan_row || ! scan_idx || ! col_list )
{
scan_row = (
int
*)
calloc
(new_len,
sizeof
(
int
));
scan_idx = (
int
*)
calloc
(new_len,
sizeof
(
int
));
col_list = (
int
*)
calloc
(new_len,
sizeof
(
int
));
}
else
{
scan_row = (
int
*)
realloc
((
char
*)scan_row,new_len*
sizeof
(
int
));
scan_idx = (
int
*)
realloc
((
char
*)scan_idx,new_len*
sizeof
(
int
));
col_list = (
int
*)
realloc
((
char
*)col_list,new_len*
sizeof
(
int
));
}
if
( ! scan_row || ! scan_idx || ! col_list )
error(E_MEM,
"set_scan"
);
return
new_len;
}
SPMAT *spCHfactor(A)
SPMAT *A;
{
register
int
i;
int
idx, k, m, minim, n, num_scan, diag_idx, tmp1;
Real pivot, tmp2;
SPROW *r_piv, *r_op;
row_elt *elt_piv, *elt_op, *old_elt;
if
( A == SMNULL )
error(E_NULL,
"spCHfactor"
);
if
( A->m != A->n )
error(E_SQUARE,
"spCHfactor"
);
sp_col_access(A);
sp_diag_access(A);
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,
"spCHfactor"
);
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;
}
num_scan = i;
tmp2 = elt_piv[diag_idx].val - sprow_sqr(r_piv,k);
if
( tmp2 <= 0.0 )
error(E_POSDEF,
"spCHfactor"
);
elt_piv[diag_idx].val = pivot =
sqrt
(tmp2);
for
( ; ; )
{
minim = n;
for
( i = 0; i < num_scan; i++ )
{
tmp1 = scan_row[i];
minim = ( tmp1 >= 0 && tmp1 < minim ) ? tmp1 : minim;
}
if
( minim >= n )
break
;
r_op = &(A->row[minim]);
elt_op = r_op->elt;
idx = sprow_idx2(r_op,k,scan_idx[num_scan-1]);
if
( idx < 0 )
{
sp_set_val(A,minim,k,
-sprow_ip(r_piv,r_op,k)/pivot);
elt_op = r_op->elt;
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;
}
else
elt_op[idx].val = (elt_op[idx].val -
sprow_ip(r_piv,r_op,k))/pivot;
idx = sprow_idx2(r_op,k,idx);
old_elt = &(r_op->elt[idx]);
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;
}
}
}
return
A;
}
VEC *spCHsolve(L,b,out)
SPMAT *L;
VEC *b, *out;
{
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;
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"
);
}
for
( i = n-1; i >= 0; i-- )
{
sum = out_ve[i];
row = &(L->row[i]);
elt = &(row->elt[row->diag]);
diag_val = elt->val;
scan_idx = elt->nxt_idx;
scan_row = elt->nxt_row;
while
( scan_row >= 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 *spICHfactor(A)
SPMAT *A;
{
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"
);
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;
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);
nxt_row = elt_piv[diag_idx].nxt_row;
nxt_idx = elt_piv[diag_idx].nxt_idx;
while
( nxt_row >= 0 && nxt_idx >= 0 )
{
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(A)
SPMAT *A;
{
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"
);
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]);
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;
}
num_scan = i;
for
( ; ; )
{
minim = n;
for
( i = 0; i < num_scan; i++ )
{
tmp1 = scan_row[i];
minim = ( tmp1 >= 0 && tmp1 < minim ) ? tmp1 : minim;
}
if
( minim >= n )
break
;
r_op = &(A->row[minim]);
elt_op = r_op->elt;
idx = sprow_idx2(r_op,k,scan_idx[num_scan-1]);
if
( idx < 0 )
{
sp_set_val(A,minim,k,0.0);
elt_op = r_op->elt;
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;
}
idx = sprow_idx2(r_op,k,idx);
old_elt = &(r_op->elt[idx]);
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;
}
}
}
return
A;
}
SPMAT *comp_AAT(A)
SPMAT *A;
{
SPMAT *AAT;
SPROW *r, *r2;
row_elt *elts, *elts2;
int
i, idx, idx2, j, m, minim, n, num_scan, tmp1;
Real ip;
if
( ! A )
error(E_NULL,
"comp_AAT"
);
m = A->m; n = A->n;
if
( ! A->flag_col )
sp_col_access(A);
AAT = sp_get(m,m,10);
for
( i = 0; i < m; i++ )
{
r = &(A->row[i]);
elts = r->elt;
if
( r->len > scan_len )
set_scan(r->len);
for
( j = 0; j < r->len; j++ )
{
col_list[j] = elts[j].col;
scan_row[j] = elts[j].nxt_row;
scan_idx[j] = elts[j].nxt_idx;
}
num_scan = r->len;
for
( ; ; )
{
minim = m;
for
( idx = 0; idx < num_scan; idx++ )
{
tmp1 = scan_row[idx];
minim = ( tmp1 >= 0 && tmp1 < minim ) ? tmp1 : minim;
}
if
( minim >= m )
break
;
r2 = &(A->row[minim]);
if
( minim > i )
{
ip = sprow_ip(r,r2,n);
sp_set_val(AAT,minim,i,ip);
sp_set_val(AAT,i,minim,ip);
}
elts2 = r2->elt;
for
( idx = 0; idx < num_scan; idx++ )
{
if
( scan_row[idx] != minim || scan_idx[idx] < 0 )
continue
;
idx2 = scan_idx[idx];
scan_row[idx] = elts2[idx2].nxt_row;
scan_idx[idx] = elts2[idx2].nxt_idx;
}
}
sp_set_val(AAT,i,i,sprow_sqr(r,n));
}
return
AAT;
}