extern int px_resize_vars(int new_dim,...)
{
va_list ap;
int i=0;
PERM **par;
va_start(ap, new_dim);
while (par = va_arg(ap,PERM **)) { /* NULL ends the list*/
*par = px_resize(*par,new_dim);
i++;
}
va_end(ap);
return i;
}
/* px_copy -- copies permutation 'in' to 'out' */
PERM *px_copy(PERM *in,PERM *out)
{
/* int i; */
if ( in == PNULL )
error(E_NULL,"px_copy");
if ( in == out )
return out;
if ( out == PNULL || out->size != in->size )
out = px_resize(out,in->size);
MEM_COPY(in->pe,out->pe,in->size*sizeof(unsigned int));
/* for ( i = 0; i < in->size; i++ )
out->pe[i] = in->pe[i]; */
return out;
}
MAT *m_inverse(const MAT *A, MAT *out)
#endif
{
int i;
STATIC VEC *tmp = VNULL, *tmp2 = VNULL;
STATIC MAT *A_cp = MNULL;
STATIC PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
A_cp = m_resize(A_cp,A->m,A->n);
A_cp = m_copy(A,A_cp);
tmp = v_resize(tmp,A->m);
tmp2 = v_resize(tmp2,A->m);
pivot = px_resize(pivot,A->m);
MEM_STAT_REG(A_cp,TYPE_MAT);
MEM_STAT_REG(tmp, TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
MEM_STAT_REG(pivot,TYPE_PERM);
tracecatch(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ )
{
v_zero(tmp);
tmp->ve[i] = 1.0;
tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
#ifdef THREADSAFE
V_FREE(tmp); V_FREE(tmp2);
M_FREE(A_cp); PX_FREE(pivot);
#endif
return out;
}
/* px_mlt -- permutation multiplication (composition) */
PERM *px_mlt(const PERM *px1, const PERM *px2, PERM *out)
{
unsigned int i,size;
if ( px1==(PERM *)NULL || px2==(PERM *)NULL )
error(E_NULL,"px_mlt");
if ( px1->size != px2->size )
error(E_SIZES,"px_mlt");
if ( px1 == out || px2 == out )
error(E_INSITU,"px_mlt");
if ( out==(PERM *)NULL || out->size < px1->size )
out = px_resize(out,px1->size);
size = px1->size;
for ( i=0; i<size; i++ )
if ( px2->pe[i] >= size )
error(E_BOUNDS,"px_mlt");
else
out->pe[i] = px1->pe[px2->pe[i]];
return out;
}
/* v_sort -- sorts vector x, and generates permutation that gives the order
of the components; x = [1.3, 3.7, 0.5] -> [0.5, 1.3, 3.7] and
the permutation is order = [2, 0, 1].
-- if order is NULL on entry then it is ignored
-- the sorted vector x is returned */
VEC *v_sort(VEC *x, PERM *order)
{
Real *x_ve, tmp, v;
/* int *order_pe; */
int dim, i, j, l, r, tmp_i;
int stack[MAX_STACK], sp;
if ( ! x )
error(E_NULL,"v_sort");
if ( order != PNULL && order->size != x->dim )
order = px_resize(order, x->dim);
x_ve = x->ve;
dim = x->dim;
if ( order != PNULL )
px_ident(order);
if ( dim <= 1 )
return x;
/* using quicksort algorithm in Sedgewick,
"Algorithms in C", Ch. 9, pp. 118--122 (1990) */
sp = 0;
l = 0; r = dim-1;
/* v = x_ve[0]; valeur inutilisee ET v n'est pas statique */
for ( ; ; )
{
while ( r > l )
{
/* "i = partition(x_ve,l,r);" */
v = x_ve[r];
i = l-1;
j = r;
for ( ; ; )
{
while ( x_ve[++i] < v )
;
while ( x_ve[--j] > v )
;
if ( i >= j ) break;
tmp = x_ve[i];
x_ve[i] = x_ve[j];
x_ve[j] = tmp;
if ( order != PNULL )
{
tmp_i = order->pe[i];
order->pe[i] = order->pe[j];
order->pe[j] = tmp_i;
}
}
tmp = x_ve[i];
x_ve[i] = x_ve[r];
x_ve[r] = tmp;
if ( order != PNULL )
{
tmp_i = order->pe[i];
order->pe[i] = order->pe[r];
order->pe[r] = tmp_i;
}
if ( i-l > r-i )
{ stack[sp++] = l; stack[sp++] = i-1; l = i+1; }
else
{ stack[sp++] = i+1; stack[sp++] = r; r = i-1; }
}
/* recursion elimination */
if ( sp == 0 )
break;
r = stack[--sp];
l = stack[--sp];
}
return x;
}
/*
* n_vars is the number of variables to be considered,
* d is the data array of variables d[0],...,d[n_vars-1],
* pred determines which estimate is required: BLUE, BLUP, or BLP
*/
void gls(DATA **d /* pointer to DATA array */,
int n_vars, /* length of DATA array (to consider) */
enum GLS_WHAT pred, /* what type of prediction is requested */
DPOINT *where, /* prediction location */
double *est /* output: array that holds the predicted values and variances */)
{
GLM *glm = NULL; /* to be copied to/from d */
static MAT *X0 = MNULL, *C0 = MNULL, *MSPE = MNULL, *CinvC0 = MNULL,
*Tmp1 = MNULL, *Tmp2 = MNULL, *Tmp3 = MNULL, *R = MNULL;
static VEC *blup = VNULL, *tmpa = VNULL, *tmpb = VNULL;
PERM *piv = PNULL;
volatile unsigned int i, rows_C;
unsigned int j, k, l = 0, row, col, start_i, start_j, start_X, global,
one_nbh_empty;
VARIOGRAM *v = NULL;
static enum GLS_WHAT last_pred = GLS_INIT; /* the initial value */
double c_value, *X_ori;
int info;
if (d == NULL) { /* clean up */
if (X0 != MNULL) M_FREE(X0);
if (C0 != MNULL) M_FREE(C0);
if (MSPE != MNULL) M_FREE(MSPE);
if (CinvC0 != MNULL) M_FREE(CinvC0);
if (Tmp1 != MNULL) M_FREE(Tmp1);
if (Tmp2 != MNULL) M_FREE(Tmp2);
if (Tmp3 != MNULL) M_FREE(Tmp3);
if (R != MNULL) M_FREE(R);
if (blup != VNULL) V_FREE(blup);
if (tmpa != VNULL) V_FREE(tmpa);
if (tmpb != VNULL) V_FREE(tmpb);
last_pred = GLS_INIT;
return;
}
if (DEBUG_COV) {
printlog("we're at %s X: %g Y: %g Z: %g\n",
IS_BLOCK(where) ? "block" : "point",
where->x, where->y, where->z);
}
if (pred != UPDATE) /* it right away: */
last_pred = pred;
assert(last_pred != GLS_INIT);
if (d[0]->glm == NULL) { /* allocate and initialize: */
glm = new_glm();
d[0]->glm = (void *) glm;
} else
glm = (GLM *) d[0]->glm;
glm->mu0 = v_resize(glm->mu0, n_vars);
MSPE = m_resize(MSPE, n_vars, n_vars);
if (pred == GLS_BLP || UPDATE_BLP) {
X_ori = where->X;
for (i = 0; i < n_vars; i++) { /* mu(0) */
glm->mu0->ve[i] = calc_mu(d[i], where);
blup = v_copy(glm->mu0, v_resize(blup, glm->mu0->dim));
where->X += d[i]->n_X; /* shift to next x0 entry */
}
where->X = X_ori; /* ... and set back */
for (i = 0; i < n_vars; i++) { /* Cij(0,0): */
for (j = 0; j <= i; j++) {
v = get_vgm(LTI(d[i]->id,d[j]->id));
ME(MSPE, i, j) = ME(MSPE, j, i) = COVARIANCE0(v, where, where, d[j]->pp_norm2);
}
}
fill_est(NULL, blup, MSPE, n_vars, est); /* in case of empty neighbourhood */
}
/* xxx */
/*
logprint_variogram(v, 1);
*/
/*
* selection dependent problem dimensions:
*/
for (i = rows_C = 0, one_nbh_empty = 0; i < n_vars; i++) {
rows_C += d[i]->n_sel;
if (d[i]->n_sel == 0)
one_nbh_empty = 1;
}
if (rows_C == 0 /* all selection lists empty */
|| one_nbh_empty == 1) { /* one selection list empty */
if (pred == GLS_BLP || UPDATE_BLP)
debug_result(blup, MSPE, pred);
return;
}
for (i = 0, global = 1; i < n_vars && global; i++)
global = (d[i]->sel == d[i]->list
&& d[i]->n_list == d[i]->n_original
&& d[i]->n_list == d[i]->n_sel);
/*
* global things: enter whenever (a) first time, (b) local selections or
* (c) the size of the problem grew since the last call (e.g. simulation)
*/
if (glm->C == NULL || !global || rows_C > glm->C->m) {
/*
* fill y:
*/
glm->y = get_y(d, glm->y, n_vars);
if (pred != UPDATE) {
glm->C = m_resize(glm->C, rows_C, rows_C);
if (gl_choleski == 0) /* use LDL' decomposition, allocate piv: */
piv = px_resize(piv, rows_C);
m_zero(glm->C);
glm->X = get_X(d, glm->X, n_vars);
M_DEBUG(glm->X, "X");
glm->CinvX = m_resize(glm->CinvX, rows_C, glm->X->n);
glm->XCinvX = m_resize(glm->XCinvX, glm->X->n, glm->X->n);
glm->beta = v_resize(glm->beta, glm->X->n);
for (i = start_X = start_i = 0; i < n_vars; i++) { /* row var */
/* fill C, mu: */
for (j = start_j = 0; j <= i; j++) { /* col var */
v = get_vgm(LTI(d[i]->id,d[j]->id));
for (k = 0; k < d[i]->n_sel; k++) { /* rows */
row = start_i + k;
for (l = 0, col = start_j; col <= row && l < d[j]->n_sel; l++, col++) {
if (pred == GLS_BLUP)
c_value = GCV(v, d[i]->sel[k], d[j]->sel[l]);
else
c_value = COVARIANCE(v, d[i]->sel[k], d[j]->sel[l]);
/* on the diagonal, if necessary, add measurement error variance */
if (d[i]->colnvariance && i == j && k == l)
c_value += d[i]->sel[k]->variance;
ME(glm->C, col, row) = c_value; /* fill upper */
if (col != row)
ME(glm->C, row, col) = c_value; /* fill all */
} /* for l */
} /* for k */
start_j += d[j]->n_sel;
} /* for j */
start_i += d[i]->n_sel;
if (d[i]->n_sel > 0)
start_X += d[i]->n_X - d[i]->n_merge;
} /* for i */
/*
if (d[0]->colnvmu)
glm->C = convert_vmuC(glm->C, d[0]);
*/
if (d[0]->variance_fn) {
glm->mu = get_mu(glm->mu, glm->y, d, n_vars);
convert_C(glm->C, glm->mu, d[0]->variance_fn);
}
if (DEBUG_COV && pred == GLS_BLUP)
printlog("[using generalized covariances: max_val - semivariance()]");
M_DEBUG(glm->C, "Covariances (x_i, x_j) matrix C (upper triangle)");
/*
* factorize C:
*/
CHfactor(glm->C, piv, &info);
if (info != 0) { /* singular: */
pr_warning("Covariance matrix singular at location [%g,%g,%g]: skipping...",
where->x, where->y, where->z);
m_free(glm->C);
glm->C = MNULL; /* assure re-entrance if global */
P_FREE(piv);
return;
}
if (piv == NULL)
M_DEBUG(glm->C, "glm->C, Choleski decomposed:")
else
M_DEBUG(glm->C, "glm->C, LDL' decomposed:")
} /* if (pred != UPDATE) */
//--------------------------------------------------------------------------
void Hqp_IpRedSpBKP::init(const Hqp_Program *qp)
{
IVEC *degree, *neigh_start, *neighs;
SPMAT *QCTC;
SPROW *r1, *r2;
int i, j;
int len, dim;
Real sum;
_n = qp->c->dim;
_me = qp->b->dim;
_m = qp->d->dim;
dim = _n + _me;
// reallocations
_pivot = px_resize(_pivot, dim);
_blocks = px_resize(_blocks, dim);
_zw = v_resize(_zw, _m);
_scale = v_resize(_scale, _n);
_r12 = v_resize(_r12, dim);
_xy = v_resize(_xy, dim);
// store C' for further computations
// analyze structure of C'*C
_CT = sp_transp(qp->C, _CT);
sp_ones(_CT);
v_ones(_zw);
QCTC = sp_get(_n, _n, 10);
r1 = _CT->row;
for (i=0; i<_n; i++, r1++) {
r2 = r1;
for (j=i; j<_n; j++, r2++) {
sum = sprow_inprod(r1, _zw, r2);
if (sum != 0.0) {
sp_set_val(QCTC, i, j, sum);
if (i != j)
sp_set_val(QCTC, j, i, sum);
}
}
}
_CTC_degree = iv_resize(_CTC_degree, _n);
_CTC_neigh_start = iv_resize(_CTC_neigh_start, _n + 1);
_CTC_neighs = sp_rcm_scan(QCTC, SMNULL, SMNULL,
_CTC_degree, _CTC_neigh_start, _CTC_neighs);
// initialize structure of reduced qp
QCTC = sp_add(qp->Q, QCTC, QCTC);
// determine RCM ordering
degree = iv_get(dim);
neigh_start = iv_get(dim + 1);
neighs = sp_rcm_scan(QCTC, qp->A, SMNULL, degree, neigh_start, IVNULL);
_QP2J = sp_rcm_order(degree, neigh_start, neighs, _QP2J);
_sbw = sp_rcm_sbw(neigh_start, neighs, _QP2J);
_J2QP = px_inv(_QP2J, _J2QP);
iv_free(degree);
iv_free(neigh_start);
iv_free(neighs);
len = 1 + (int)(log((double)dim) / log(2.0));
sp_free(_J);
sp_free(_J_raw);
_J_raw = sp_get(dim, dim, len);
_J = SMNULL;
// fill up data (to allocate _J_raw)
sp_into_symsp(QCTC, -1.0, _J_raw, _QP2J, 0, 0);
spT_into_symsp(qp->A, 1.0, _J_raw, _QP2J, 0, _n);
sp_into_symsp(qp->A, 1.0, _J_raw, _QP2J, _n, 0);
sp_free(QCTC);
// prepare iterations
update(qp);
}
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;
}
static void wls_fit(VARIOGRAM *vp) {
/*
* non-linear iterative reweighted least squares fitting of variogram model to
* sample variogram (..covariogram model to sample covariogram, cross, etc.)
* all information necessary is contained in *vp.
*
* uses Marquardt-Levenberg algorithm;
* the implementation follows gnuplot's fit.c
*/
static PERM *p = PNULL;
int i, j, n_iter = 0, bounded = 0, timetostop;
double SSErr, oldSSErr = DBL_MAX, step;
LM *lm;
p = px_resize(p, vp->ev->n_est);
if (! vp->ev->cloud) {
for (i = j = 0; i < (vp->ev->zero == ZERO_AVOID ?
vp->ev->n_est-1 : vp->ev->n_est); i++) {
if (vp->ev->nh[i] > 0)
p->pe[j++] = i;
}
p->size = j;
}
lm = init_lm(NULL);
/* oldSSErr = getSSErr(vp, p, lm); */
do {
print_progress(n_iter, gl_iter);
/* if (DEBUG_VGMFIT)
printlog("%s: ", vp->descr); */
if ((vp->fit_is_singular = fit_GaussNewton(vp, p, lm, n_iter, &bounded))) {
pr_warning("singular model in variogram fit");
print_progress(gl_iter, gl_iter);
vp->SSErr = getSSErr(vp, p, lm);
return;
}
update_variogram(vp);
SSErr = getSSErr(vp, p, lm);
/* we can't use lm->SSErr here since that's only in the
X-filled-with-derivatives, not the true residuals */
step = oldSSErr - SSErr;
if (SSErr > gl_zero)
step /= SSErr;
n_iter++;
if (DEBUG_VGMFIT)
printlog("after it. %d: SSErr %g->%g, step=%g (fit_limit %g%s)\n",
n_iter, oldSSErr, SSErr, step, gl_fit_limit,
bounded ? "; bounded" : "");
oldSSErr = SSErr;
timetostop = (step < gl_fit_limit && step >= 0.0 && bounded == 0) || n_iter == gl_iter;
} while (! timetostop);
print_progress(gl_iter, gl_iter);
if (n_iter == gl_iter)
pr_warning("No convergence after %d iterations: try different initial values?", n_iter);
if (DEBUG_VGMFIT) {
printlog("# iterations: %d, SSErr %g, last step %g", n_iter, SSErr, step);
if (step < 0.0)
printlog(", last step was in the wrong direction.\n");
else
printlog("\n");
}
free_lm(lm);
vp->SSErr = SSErr;
return;
} /* wls_fit */
