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 */