自学教程：C++ v_resize函数代码示例

//--------------------------------------------------------------------------void Omu_IntODE::resize(){  if (_dxt->dim == _nx + _nd && _ut->dim == _nu && _u->dim == _nd + _nu)    return;  int neq = _n * (1 + _nx + _nu);  v_resize(_y, neq);  v_resize(_u, _nd + _nu);  //  // variables for ADOL-C  //  v_resize(_x, _nd + 2 * _n + _nu);  v_resize(_v, _nd + 2 * _n + _nu);  m_resize(_X2, _nd + 2 * _n + _nu, _nx + _nu);  m_resize(_Y2, _n, _nx + _nu);  //  // variables for low level _sys->continuous callback  //  v_resize(_ut, _nu);  _dxt.resize(_nd + _n, _nx, _nu);  m_resize(_Yx, _nd + _n, _nx);  m_resize(_Yu, _nd + _n, _nu);}

static void test_mgcr(ITER *ip, int i, MAT *Q, MAT *R)#endif{    VEC vt, vt1;    static MAT *R1 = MNULL;    static VEC *r = VNULL, *r1 = VNULL;    VEC *rr;    int k, j;    Real sm;    /* check Q*Q^T = I */    vt.dim = vt.max_dim = ip->b->dim;    vt1.dim = vt1.max_dim = ip->b->dim;    Q = m_resize(Q, i + 1, ip->b->dim);    R1 = m_resize(R1, i + 1, i + 1);    r = v_resize(r, ip->b->dim);    r1 = v_resize(r1, ip->b->dim);    MEM_STAT_REG(R1, TYPE_MAT);    MEM_STAT_REG(r, TYPE_VEC);    MEM_STAT_REG(r1, TYPE_VEC);    m_zero(R1);    for (k = 1; k <= i; k++)        for (j = 1; j <= i; j++) {            vt.ve = Q->me[k];            vt1.ve = Q->me[j];            R1->me[k][j] = in_prod(&vt, &vt1);        }    for (j = 1; j <= i; j++)        R1->me[j][j] -= 1.0;#ifndef MEX    if (m_norm_inf(R1) > MACHEPS * ip->b->dim)        printf(" ! (mgcr:) m_norm_inf(Q*Q^T) = %g/n", m_norm_inf(R1));#endif    /* check (r_i,Ap_j) = 0 for j <= i */    ip->Ax(ip->A_par, ip->x, r);    v_sub(ip->b, r, r);    rr = r;    if (ip->Bx) {        ip->Bx(ip->B_par, r, r1);        rr = r1;    }#ifndef MEX    printf(" ||r|| = %g/n", v_norm2(rr));#endif    sm = 0.0;    for (j = 1; j <= i; j++) {        vt.ve = Q->me[j];        sm = max(sm, in_prod(&vt,rr));    }#ifndef MEX    if (sm >= MACHEPS * ip->b->dim)        printf(" ! (mgcr:) max_j (r,Ap_j) = %g/n", sm);#endif}

VEC	*QRsolve(const MAT *QR, const VEC *diag, const VEC *b, VEC *x)#endif{    int	limit;    STATIC	VEC	*tmp = VNULL;        if ( ! QR || ! diag || ! b )	error(E_NULL,"QRsolve");    limit = min(QR->m,QR->n);    if ( diag->dim < limit || b->dim != QR->m )	error(E_SIZES,"QRsolve");    tmp = v_resize(tmp,limit);    MEM_STAT_REG(tmp,TYPE_VEC);    x = v_resize(x,QR->n);    _Qsolve(QR,diag,b,x,tmp);    x = Usolve(QR,x,x,0.0);    v_resize(x,QR->n);#ifdef	THREADSAFE    V_FREE(tmp);#endif    return x;}

MAT	*bifactor(MAT *A, MAT *U, MAT *V)#endif{	int	k;	STATIC VEC	*tmp1=VNULL, *tmp2=VNULL, *w=VNULL;	Real	beta;	if ( ! A )		error(E_NULL,"bifactor");	if ( ( U && ( U->m != U->n ) ) || ( V && ( V->m != V->n ) ) )		error(E_SQUARE,"bifactor");	if ( ( U && U->m != A->m ) || ( V && V->m != A->n ) )		error(E_SIZES,"bifactor");	tmp1 = v_resize(tmp1,A->m);	tmp2 = v_resize(tmp2,A->n);	w    = v_resize(w,   max(A->m,A->n));	MEM_STAT_REG(tmp1,TYPE_VEC);	MEM_STAT_REG(tmp2,TYPE_VEC);	MEM_STAT_REG(w,   TYPE_VEC);	if ( A->m >= A->n )	    for ( k = 0; k < A->n; k++ )	    {		get_col(A,k,tmp1);		hhvec(tmp1,k,&beta,tmp1,&(A->me[k][k]));		_hhtrcols(A,k,k+1,tmp1,beta,w);		if ( U )		    _hhtrcols(U,k,0,tmp1,beta,w);		if ( k+1 >= A->n )		    continue;		get_row(A,k,tmp2);		hhvec(tmp2,k+1,&beta,tmp2,&(A->me[k][k+1]));		hhtrrows(A,k+1,k+1,tmp2,beta);		if ( V )		    _hhtrcols(V,k+1,0,tmp2,beta,w);	    }	else	    for ( k = 0; k < A->m; k++ )	    {		get_row(A,k,tmp2);		hhvec(tmp2,k,&beta,tmp2,&(A->me[k][k]));		hhtrrows(A,k+1,k,tmp2,beta);		if ( V )		    _hhtrcols(V,k,0,tmp2,beta,w);		if ( k+1 >= A->m )		    continue;		get_col(A,k,tmp1);		hhvec(tmp1,k+1,&beta,tmp1,&(A->me[k+1][k]));		_hhtrcols(A,k+1,k+1,tmp1,beta,w);		if ( U )		    _hhtrcols(U,k+1,0,tmp1,beta,w);	    }#ifdef	THREADSAFE	V_FREE(tmp1);	V_FREE(tmp2);#endif	return A;}

MAT	*makeQ(const MAT *QR,const VEC *diag, MAT *Qout)#endif{    STATIC	VEC	*tmp1=VNULL,*tmp2=VNULL;    unsigned int	i, limit;    Real	beta, r_ii, tmp_val;    int	j;        limit = min(QR->m,QR->n);    if ( ! QR || ! diag )	error(E_NULL,"makeQ");    if ( diag->dim < limit )	error(E_SIZES,"makeQ");    if ( Qout==(MAT *)NULL || Qout->m < QR->m || Qout->n < QR->m )	Qout = m_get(QR->m,QR->m);        tmp1 = v_resize(tmp1,QR->m);	/* contains basis vec & columns of Q */    tmp2 = v_resize(tmp2,QR->m);	/* contains H/holder vectors */    MEM_STAT_REG(tmp1,TYPE_VEC);    MEM_STAT_REG(tmp2,TYPE_VEC);        for ( i=0; i<QR->m ; i++ )    {	/* get i-th column of Q */	/* set up tmp1 as i-th basis vector */	for ( j=0; j<QR->m ; j++ )	    tmp1->ve[j] = 0.0;	tmp1->ve[i] = 1.0;		/* apply H/h transforms in reverse order */	for ( j=limit-1; j>=0; j-- )	{	    get_col(QR,j,tmp2);	    r_ii = fabs(tmp2->ve[j]);	    tmp2->ve[j] = diag->ve[j];	    tmp_val = (r_ii*fabs(diag->ve[j]));	    beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;	    /* hhtrvec(tmp2,beta->ve[j],j,tmp1,tmp1); */	    hhtrvec(tmp2,beta,j,tmp1,tmp1);	}		/* insert into Q */	set_col(Qout,i,tmp1);    }#ifdef	THREADSAFE    V_FREE(tmp1);	V_FREE(tmp2);#endif    return (Qout);}

static MAT *calc_VinvIminAw(MAT *Vw, MAT *X, MAT *VinvIminAw, int calc_Aw) {/* * calculate V_w^-1(I-A_w) (==VinvIminAw), * A = X(X'X)^-1 X' (AY = XBeta; Beta = (X'X)^-1 X'Y) * * on second thought (Nov 1998 -- more than 4 years later :-)) * calc (I-Aw) only once and keep this constant during iteration. */ 	MAT *tmp = MNULL, *V = MNULL; 	VEC *b = VNULL, *rhs = VNULL; 	int i, j;	if (X->m != Vw->n || VinvIminAw->m != X->m)		ErrMsg(ER_IMPOSVAL, "calc_VinvIminAw: sizes don't match");		if (calc_Aw) {		IminAw = m_resize(IminAw, X->m, X->m);		tmp = m_resize(tmp, X->n, X->n);		tmp = mtrm_mlt(X, X, tmp); /* X'X */		m_inverse(tmp, tmp); /* (X'X)-1 */		/* X(X'X)-1 -> X(X'X)-1 X') */		IminAw = XVXt_mlt(X, tmp, IminAw);		for (i = 0; i < IminAw->m; i++) /* I - Aw */			for (j = 0; j <= i; j++)				if (i == j)					IminAw->me[i][j] = 1.0 - IminAw->me[i][j];				else					IminAw->me[i][j] = IminAw->me[j][i] = -IminAw->me[i][j];	}	V = m_copy(Vw, V);	LDLfactor(V);	rhs = v_resize(rhs, X->m);	b = v_resize(b, X->m);	for (i = 0; i < X->m; i++) { /* solve Vw X = (I-A) for X -> V-1(I-A) */		rhs = get_col(IminAw, i, rhs);		LDLsolve(V, rhs, b);		set_col(VinvIminAw, i, b);	}	v_free(rhs);	v_free(b);	m_free(V);	if (tmp) 		m_free(tmp);	return VinvIminAw;}

MAT	*makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout)#endif{	int	i, j, limit;	STATIC	VEC	*tmp1 = VNULL, *tmp2 = VNULL;	if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL )		error(E_NULL,"makeHQ");	limit = H->m - 1;	if ( diag->dim < limit || beta->dim < limit )		error(E_SIZES,"makeHQ");	if ( H->m != H->n )		error(E_SQUARE,"makeHQ");	Qout = m_resize(Qout,H->m,H->m);	tmp1 = v_resize(tmp1,H->m);	tmp2 = v_resize(tmp2,H->m);	MEM_STAT_REG(tmp1,TYPE_VEC);	MEM_STAT_REG(tmp2,TYPE_VEC);	for ( i = 0; i < H->m; i++ )	{		/* tmp1 = i'th basis vector */		for ( j = 0; j < H->m; j++ )			/* tmp1->ve[j] = 0.0; */		    v_set_val(tmp1,j,0.0);		/* tmp1->ve[i] = 1.0; */		v_set_val(tmp1,i,1.0);		/* apply H/h transforms in reverse order */		for ( j = limit-1; j >= 0; j-- )		{			get_col(H,(unsigned int)j,tmp2);			/* tmp2->ve[j+1] = diag->ve[j]; */			v_set_val(tmp2,j+1,v_entry(diag,j));			hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1);		}		/* insert into Qout */		set_col(Qout,(unsigned int)i,tmp1);	}#ifdef THREADSAFE	V_FREE(tmp1);	V_FREE(tmp2);#endif	return (Qout);}

void booz_sensors_model_accel_run( double time ) {  if (time < bsm.accel_next_update)    return;  static VEC* accel_ltp = VNULL;  accel_ltp = v_resize(accel_ltp, AXIS_NB);  /* substract gravity to acceleration in ltp frame */  accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);  /* convert to body frame */  static VEC* accel_body = VNULL;  accel_body = v_resize(accel_body, AXIS_NB);  mv_mlt(bfm.dcm, accel_ltp, accel_body);  /* convert to imu frame */  static VEC* accel_imu = VNULL;  accel_imu = v_resize(accel_imu, AXIS_NB);  mv_mlt(bsm.body_to_imu, accel_body, accel_imu);  //  printf(" accel_body ~ %f %f %f/n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);  /* compute accel reading */  mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);  v_add(bsm.accel, bsm.accel_neutral, bsm.accel);  /* compute accel error readings */  static VEC *accel_error = VNULL;  accel_error = v_resize(accel_error, AXIS_NB);  accel_error = v_zero(accel_error);  /* add a gaussian noise */  accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);  /* constant bias  */  accel_error = v_add(accel_error, bsm.accel_bias, accel_error);  /* scale to adc units FIXME : should use full adc gain ? sum ? */  accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];  accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];  accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];  /* add per accel error reading */  bsm.accel =  v_add(bsm.accel, accel_error, bsm.accel);  /* round signal to account for adc discretisation */  RoundSensor(bsm.accel);  /* saturation                                     */  BoundSensor(bsm.accel, 0, bsm.accel_resolution);  //  printf("sim adc %f %f %f/n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);  bsm.accel_next_update += BSM_ACCEL_DT;  bsm.accel_available = TRUE;}

VEC	*pxinv_vec(PERM *px, const VEC *x, VEC *out)#endif{    unsigned int	i, size;        if ( ! px || ! x )	error(E_NULL,"pxinv_vec");    if ( px->size > x->dim )	error(E_SIZES,"pxinv_vec");    /* if ( x == out )	error(E_INSITU,"pxinv_vec"); */    if ( ! out || out->dim < x->dim )	out = v_resize(out,x->dim);        size = px->size;    if ( size == 0 )	return v_copy(x,out);    if ( out != x )    {	for ( i=0; i<size; i++ )	    if ( px->pe[i] >= size )		error(E_BOUNDS,"pxinv_vec");	    else		out->ve[px->pe[i]] = x->ve[i];    }    else    {	/* in situ algorithm --- cheat's way out */	px_inv(px,px);	px_vec(px,x,out);	px_inv(px,px);    }    return out;}

VEC	*vm_mlt(const MAT *A, const VEC *b, VEC *out)#endif{	unsigned int	j,m,n;	/* Real	sum,**A_v,*b_v; */	if ( A==(MAT *)NULL || b==(VEC *)NULL )		error(E_NULL,"vm_mlt");	if ( A->m != b->dim )		error(E_SIZES,"vm_mlt");	if ( b == out )		error(E_INSITU,"vm_mlt");	if ( out == (VEC *)NULL || out->dim != A->n )		out = v_resize(out,A->n);	m = A->m;		n = A->n;	v_zero(out);	for ( j = 0; j < m; j++ )		if ( b->ve[j] != 0.0 )		    __mltadd__(out->ve,A->me[j],b->ve[j],(int)n);	/**************************************************	A_v = A->me;		b_v = b->ve;	for ( j=0; j<n; j++ )	{		sum = 0.0;		for ( i=0; i<m; i++ )			sum += b_v[i]*A_v[i][j];		out->ve[j] = sum;	}	**************************************************/	return out;}

VEC	*pxinv_vec(PERM *px, const VEC *x, VEC *out){    unsigned int	i, size;        if ( ! px || ! x )	error(E_NULL,"pxinv_vec");    if ( px->size > x->dim )	error(E_SIZES,"pxinv_vec");    if ( ! out || out->dim < x->dim )	out = v_resize(out,x->dim);        size = px->size;    if ( size == 0 )	return v_copy(x,out);    if ( out != x )    {	for ( i=0; i<size; i++ )	    if ( px->pe[i] >= size )		error(E_BOUNDS,"pxinv_vec");	    else		out->ve[px->pe[i]] = x->ve[i];    }    else    {		px_inv(px,px);	px_vec(px,x,out);	px_inv(px,px);    }    return out;}

/* hhtrcols -- transform a matrix by a Householder vector by columns	starting at row i0 from column j0 -- in-situ */MAT	*hhtrcols(MAT *M,unsigned int i0,unsigned int j0,VEC *hh,double beta){	/* Real	ip, scale; */	int	i /*, k */;	static	VEC	*w = VNULL;	if ( M==(MAT *)NULL || hh==(VEC *)NULL )		error(E_NULL,"hhtrcols");	if ( M->m != hh->dim )		error(E_SIZES,"hhtrcols");	if ( i0 > M->m || j0 > M->n )		error(E_BOUNDS,"hhtrcols");	if ( beta == 0.0 )	return (M);	w = v_resize(w,M->n);	MEM_STAT_REG(w,TYPE_VEC);	v_zero(w);	for ( i = i0; i < M->m; i++ )	    if ( hh->ve[i] != 0.0 )		__mltadd__(&(w->ve[j0]),&(M->me[i][j0]),hh->ve[i],							(int)(M->n-j0));	for ( i = i0; i < M->m; i++ )	    if ( hh->ve[i] != 0.0 )		__mltadd__(&(M->me[i][j0]),&(w->ve[j0]),-beta*hh->ve[i],							(int)(M->n-j0));	return (M);}

MAT	*hhtrcols(MAT *M, unsigned int i0, unsigned int j0,		  const VEC *hh, double beta)#endif{  STATIC VEC	*w = VNULL;  if ( M == MNULL || hh == VNULL || w == VNULL )    error(E_NULL,"hhtrcols");  if ( M->m != hh->dim )    error(E_SIZES,"hhtrcols");  if ( i0 > M->m || j0 > M->n )    error(E_BOUNDS,"hhtrcols");  if ( ! w || w->dim < M->n )    w = v_resize(w,M->n);  MEM_STAT_REG(w,TYPE_VEC);  M = _hhtrcols(M,i0,j0,hh,beta,w);#ifdef THREADSAFE  V_FREE(w);#endif  return M;}

/* v_conv -- computes convolution product of two vectors */VEC	*v_conv(VEC *x1, VEC *x2, VEC *out){    int		i;    if ( ! x1 || ! x2 )	error(E_NULL,"v_conv");    if ( x1 == out || x2 == out )	error(E_INSITU,"v_conv");    if ( x1->dim == 0 || x2->dim == 0 )	return out = v_resize(out,0);    out = v_resize(out,x1->dim + x2->dim - 1);    v_zero(out);    for ( i = 0; i < x1->dim; i++ )	__mltadd__(&(out->ve[i]),x2->ve,x1->ve[i],x2->dim);    return out;}

void booz_sensors_model_mag_run( double time ) {  if (time < bsm.mag_next_update)    return;  /* rotate h to body frame */  static VEC *h_body = VNULL;  h_body = v_resize(h_body, AXIS_NB);  mv_mlt(bfm.dcm, bfm.h_ltp, h_body);  /* rotate to imu frame */  static VEC *h_imu = VNULL;  h_imu = v_resize(h_imu, AXIS_NB);  mv_mlt(bsm.body_to_imu, h_body, h_imu);  /* rotate to sensor frame */  static VEC *h_sensor = VNULL;  h_sensor = v_resize(h_sensor, AXIS_NB);  mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);  mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);  v_add(bsm.mag, bsm.mag_neutral, bsm.mag);  /* compute mag error readings */  static VEC *mag_error = VNULL;  mag_error = v_resize(mag_error, AXIS_NB);  /* add hard iron now ? */  mag_error = v_zero(mag_error);  /* add a gaussian noise */  mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);  mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];  mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];  mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];  /* add error */  v_add(bsm.mag, mag_error, bsm.mag);  //  printf("h body %f %f %f/n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);  //  printf("mag %f %f %f/n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);  /* round signal to account for adc discretisation */  RoundSensor(bsm.mag);  bsm.mag_next_update += BSM_MAG_DT;  bsm.mag_available = TRUE;}

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;}

double	LUcondest(const MAT *LU, PERM *pivot)#endif{    STATIC	VEC	*y = VNULL, *z = VNULL;    Real	cond_est, L_norm, U_norm, sum, tiny;    int		i, j, n;    if ( ! LU || ! pivot )	error(E_NULL,"LUcondest");    if ( LU->m != LU->n )	error(E_SQUARE,"LUcondest");    if ( LU->n != pivot->size )	error(E_SIZES,"LUcondest");    tiny = 10.0/HUGE_VAL;    n = LU->n;    y = v_resize(y,n);    z = v_resize(z,n);    MEM_STAT_REG(y,TYPE_VEC);    MEM_STAT_REG(z,TYPE_VEC);    for ( i = 0; i < n; i++ )    {	sum = 0.0;	for ( j = 0; j < i; j++ )	    sum -= LU->me[j][i]*y->ve[j];	sum -= (sum < 0.0) ? 1.0 : -1.0;	if ( fabs(LU->me[i][i]) <= tiny*fabs(sum) )	    return HUGE_VAL;	y->ve[i] = sum / LU->me[i][i];    }    Catch(E_SING,	  LTsolve(LU,y,y,1.0);	  LUsolve(LU,pivot,y,z);	  ,	  return HUGE_VAL);

VEC	*LTsolve(const MAT *L, const VEC *b, VEC *out, double diag){    unsigned int	dim;    int		i, i_lim;    MatrixReal	**L_me, *b_ve, *out_ve, tmp, invdiag, tiny;        if ( ! L || ! b )	error(E_NULL,"LTsolve");    dim = mat_min(L->m,L->n);    if ( b->dim < dim )	error(E_SIZES,"LTsolve");    out = v_resize(out,L->n);    L_me = L->me;	b_ve = b->ve;	out_ve = out->ve;    tiny = (10.0/HUGE_VAL);        for ( i=dim-1; i>=0; i-- )	if ( b_ve[i] != 0.0 )	    break;    i_lim = i;    if ( b != out )    {	__zero__(out_ve,out->dim);	MEM_COPY(b_ve,out_ve,(i_lim+1)*sizeof(MatrixReal));    }    if ( diag == 0.0 )    {	for (        ; i>=0; i-- )	{	    tmp = L_me[i][i];	    if ( fabs(tmp) <= tiny*fabs(out_ve[i]) )		error(E_SING,"LTsolve");	    out_ve[i] /= tmp;	    __mltadd__(out_ve,L_me[i],-out_ve[i],i);	}    }    else    {	invdiag = 1.0/diag;	for (        ; i>=0; i-- )	{	    out_ve[i] *= invdiag;	    __mltadd__(out_ve,L_me[i],-out_ve[i],i);	}    }        return (out);}

VEC	*bdLUsolve(const BAND *bA, PERM *pivot, const VEC *b, VEC *x)#endif{   int i,j,l,n,n1,pi,lb,ub,jmin, maxj;   Real c;   Real **bA_v;   if ( bA==(BAND *)NULL || b==(VEC *)NULL || pivot==(PERM *)NULL )     error(E_NULL,"bdLUsolve");   if ( bA->mat->n != b->dim || bA->mat->n != pivot->size)     error(E_SIZES,"bdLUsolve");    lb = bA->lb;   ub = bA->ub;   n = b->dim;   n1 = n-1;   bA_v = bA->mat->me;   x = v_resize(x,b->dim);   px_vec(pivot,b,x);   /* solve Lx = b; implicit diagonal = 1       L is not permuted, therefore it must be permuted now    */      px_inv(pivot,pivot);   for (j=0; j < n; j++) {      jmin = j+1;      c = x->ve[j];      maxj = max(0,j+lb-n1);      for (i=jmin,l=lb-1; l >= maxj; i++,l--) {	 if ( (pi = pivot->pe[i]) < jmin) 	   pi = pivot->pe[i] = pivot->pe[pi];	 x->ve[pi] -= bA_v[l][j]*c;      }   }   /* solve Ux = b; explicit diagonal */   x->ve[n1] /= bA_v[lb][n1];   for (i=n-2; i >= 0; i--) {      c = x->ve[i];      for (j=min(n1,i+ub), l=lb+j-i; j > i; j--,l--)	c -= bA_v[l][j]*x->ve[j];      x->ve[i] = c/bA_v[lb][i];   }      return (x);}

/* v_move -- copies selected pieces of a vector	-- moves the length dim0 subvector with initial index i0	   to the corresponding subvector of out with initial index i1	-- out is resized if necessary */VEC	*v_move(VEC *in,int i0,int dim0,VEC *out,int i1){    if ( ! in )	error(E_NULL,"v_move");    if ( i0 < 0 || dim0 < 0 || i1 < 0 ||	 i0+dim0 > in->dim )	error(E_BOUNDS,"v_move");    if ( (! out) || i1+dim0 > out->dim )	out = v_resize(out,i1+dim0);    MEM_COPY(&(in->ve[i0]),&(out->ve[i1]),dim0*sizeof(Real));    return out;}

extern  int v_resize_vars(int new_dim,...){   va_list ap;   int i=0;   VEC **par;      va_start(ap, new_dim);   while (par = va_arg(ap,VEC **)) {   /* NULL ends the list*/      *par = v_resize(*par,new_dim);      i++;   }    va_end(ap);   return i;}