/* 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, VEC *w)
#endif
{
/* Real ip, scale; */
int i /*, k */;
/* 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 ( beta == 0.0 ) return (M);
if ( w->dim < M->n )
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);
}
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 *UTsolve(const MAT *U, const VEC *b, VEC *out, double diag)
{
unsigned int dim, i, i_lim;
MatrixReal **U_me, *b_ve, *out_ve, tmp, invdiag, tiny;
if ( ! U || ! b )
error(E_NULL,"UTsolve");
dim = mat_min(U->m,U->n);
if ( b->dim < dim )
error(E_SIZES,"UTsolve");
out = v_resize(out,U->n);
U_me = U->me; b_ve = b->ve; out_ve = out->ve;
tiny = (10.0/HUGE_VAL);
for ( i=0; i<dim; i++ )
if ( b_ve[i] != 0.0 )
break;
else
out_ve[i] = 0.0;
i_lim = i;
if ( b != out )
{
__zero__(out_ve,out->dim);
MEM_COPY(&(b_ve[i_lim]),&(out_ve[i_lim]),(dim-i_lim)*sizeof(MatrixReal));
}
if ( diag == 0.0 )
{
for ( ; i<dim; i++ )
{
tmp = U_me[i][i];
if ( fabs(tmp) <= tiny*fabs(out_ve[i]) )
error(E_SING,"UTsolve");
out_ve[i] /= tmp;
__mltadd__(&(out_ve[i+1]),&(U_me[i][i+1]),-out_ve[i],dim-i-1);
}
}
else
{
invdiag = 1.0/diag;
for ( ; i<dim; i++ )
{
out_ve[i] *= invdiag;
__mltadd__(&(out_ve[i+1]),&(U_me[i][i+1]),-out_ve[i],dim-i-1);
}
}
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;
}
MAT *mtrm_mlt(const MAT *A, const MAT *B, MAT *OUT)
#endif
{
int i, k, limit;
/* Real *B_row, *OUT_row, multiplier; */
if ( ! A || ! B )
error(E_NULL,"mmtr_mlt");
if ( A == OUT || B == OUT )
error(E_INSITU,"mtrm_mlt");
if ( A->m != B->m )
error(E_SIZES,"mmtr_mlt");
if ( ! OUT || OUT->m != A->n || OUT->n != B->n )
OUT = m_resize(OUT,A->n,B->n);
limit = B->n;
m_zero(OUT);
for ( k = 0; k < A->m; k++ )
for ( i = 0; i < A->n; i++ )
{
if ( A->me[k][i] != 0.0 )
__mltadd__(OUT->me[i],B->me[k],A->me[k][i],(int)limit);
/**************************************************
multiplier = A->me[k][i];
OUT_row = OUT->me[i];
B_row = B->me[k];
for ( j = 0; j < limit; j++ )
*(OUT_row++) += multiplier*(*B_row++);
**************************************************/
}
return OUT;
}
/* v_mltadd -- scalar/vector multiplication and addition
-- out = v1 + scale.v2 */
VEC *v_mltadd(VEC *v1,VEC *v2,double scale,VEC *out)
{
/* register u_int dim, i; */
/* Real *out_ve, *v1_ve, *v2_ve; */
if ( v1==(VEC *)NULL || v2==(VEC *)NULL )
error(E_NULL,"v_mltadd");
if ( v1->dim != v2->dim )
error(E_SIZES,"v_mltadd");
if ( scale == 0.0 )
return v_copy(v1,out);
if ( scale == 1.0 )
return v_add(v1,v2,out);
if ( v2 != out )
{
tracecatch(out = v_copy(v1,out),"v_mltadd");
/* dim = v1->dim; */
__mltadd__(out->ve,v2->ve,scale,(int)(v1->dim));
}
else
{
tracecatch(out = sv_mlt(scale,v2,out),"v_mltadd");
out = v_add(v1,out,out);
}
/************************************************************
out_ve = out->ve; v1_ve = v1->ve; v2_ve = v2->ve;
for ( i=0; i < dim ; i++ )
out->ve[i] = v1->ve[i] + scale*v2->ve[i];
(*out_ve++) = (*v1_ve++) + scale*(*v2_ve++);
************************************************************/
return (out);
}
VEC *vm_mltadd(const VEC *v1, const VEC *v2, const MAT *A,
double alpha, VEC *out)
#endif
{
int /* i, */ j, m, n;
Real tmp, /* *A_e, */ *out_ve;
if ( ! v1 || ! v2 || ! A )
error(E_NULL,"vm_mltadd");
if ( v2 == out )
error(E_INSITU,"vm_mltadd");
if ( v1->dim != A->n || A->m != v2->dim )
error(E_SIZES,"vm_mltadd");
tracecatch(out = v_copy(v1,out),"vm_mltadd");
out_ve = out->ve; m = A->m; n = A->n;
for ( j = 0; j < m; j++ )
{
tmp = v2->ve[j]*alpha;
if ( tmp != 0.0 )
__mltadd__(out_ve,A->me[j],tmp,(int)n);
/**************************************************
A_e = A->me[j];
for ( i = 0; i < n; i++ )
out_ve[i] += A_e[i]*tmp;
**************************************************/
}
return out;
}
/* v_pconv -- computes a periodic convolution product
-- the period is the dimension of x2 */
VEC *v_pconv(VEC *x1, VEC *x2, VEC *out)
{
int i;
if ( ! x1 || ! x2 )
error(E_NULL,"v_pconv");
if ( x1 == out || x2 == out )
error(E_INSITU,"v_pconv");
out = v_resize(out,x2->dim);
if ( x2->dim == 0 )
return out;
v_zero(out);
for ( i = 0; i < x1->dim; i++ )
{
__mltadd__(&(out->ve[i]),x2->ve,x1->ve[i],x2->dim - i);
if ( i > 0 )
__mltadd__(out->ve,&(x2->ve[x2->dim - i]),x1->ve[i],i);
}
return out;
}
/* 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;
}
BAND *bds_mltadd(const BAND *A, const BAND *B, double alpha, BAND *OUT)
#endif
{
int i;
if ( ! A || ! B )
error(E_NULL,"bds_mltadd");
if ( A->mat->n != B->mat->n )
error(E_SIZES,"bds_mltadd");
if ( A == OUT || B == OUT )
error(E_INSITU,"bds_mltadd");
OUT = bd_copy(A,OUT);
OUT = bd_resize(OUT,max(A->lb,B->lb),max(A->ub,B->ub),A->mat->n);
for ( i = 0; i <= B->lb + B->ub; i++ )
__mltadd__(OUT->mat->me[i+OUT->lb-B->lb],B->mat->me[i],alpha,B->mat->n);
return OUT;
}
MAT *m_mlt(const MAT *A, const MAT *B, MAT *OUT)
#endif
{
unsigned int i, /* j, */ k, m, n, p;
Real **A_v, **B_v /*, *B_row, *OUT_row, sum, tmp */;
if ( A==(MAT *)NULL || B==(MAT *)NULL )
error(E_NULL,"m_mlt");
if ( A->n != B->m )
error(E_SIZES,"m_mlt");
if ( A == OUT || B == OUT )
error(E_INSITU,"m_mlt");
m = A->m; n = A->n; p = B->n;
A_v = A->me; B_v = B->me;
if ( OUT==(MAT *)NULL || OUT->m != A->m || OUT->n != B->n )
OUT = m_resize(OUT,A->m,B->n);
/****************************************************************
for ( i=0; i<m; i++ )
for ( j=0; j<p; j++ )
{
sum = 0.0;
for ( k=0; k<n; k++ )
sum += A_v[i][k]*B_v[k][j];
OUT->me[i][j] = sum;
}
****************************************************************/
m_zero(OUT);
for ( i=0; i<m; i++ )
for ( k=0; k<n; k++ )
{
if ( A_v[i][k] != 0.0 )
__mltadd__(OUT->me[i],B_v[k],A_v[i][k],(int)p);
/**************************************************
B_row = B_v[k]; OUT_row = OUT->me[i];
for ( j=0; j<p; j++ )
(*OUT_row++) += tmp*(*B_row++);
**************************************************/
}
return OUT;
}
MAT *hhtrrows(MAT *M, unsigned int i0, unsigned int j0,
const VEC *hh, double beta)
#endif
{
Real ip, scale;
int i /*, j */;
if ( M==MNULL || hh==VNULL )
error(E_NULL,"hhtrrows");
if ( M->n != hh->dim )
error(E_RANGE,"hhtrrows");
if ( i0 > M->m || j0 > M->n )
error(E_BOUNDS,"hhtrrows");
if ( beta == 0.0 ) return (M);
/* for each row ... */
for ( i = i0; i < M->m; i++ )
{ /* compute inner product */
ip = __ip__(&(M->me[i][j0]),&(hh->ve[j0]),(int)(M->n-j0));
/**************************************************
ip = 0.0;
for ( j = j0; j < M->n; j++ )
ip += M->me[i][j]*hh->ve[j];
**************************************************/
scale = beta*ip;
if ( scale == 0.0 )
continue;
/* do operation */
__mltadd__(&(M->me[i][j0]),&(hh->ve[j0]),-scale,
(int)(M->n-j0));
/**************************************************
for ( j = j0; j < M->n; j++ )
M->me[i][j] -= scale*hh->ve[j];
**************************************************/
}
return (M);
}
/* hhtrvec -- apply Householder transformation to vector -- may be in-situ */
VEC *hhtrvec(VEC *hh,double beta,unsigned int i0,VEC *in,VEC *out)
/* VEC *hh,*in,*out; hh = Householder vector */
{
Real scale;
/* unsigned int i; */
if ( hh==(VEC *)NULL || in==(VEC *)NULL )
error(E_NULL,"hhtrvec");
if ( in->dim != hh->dim )
error(E_SIZES,"hhtrvec");
if ( i0 > in->dim )
error(E_BOUNDS,"hhtrvec");
scale = beta*_in_prod(hh,in,i0);
out = v_copy(in,out);
__mltadd__(&(out->ve[i0]),&(hh->ve[i0]),-scale,(int)(in->dim-i0));
/************************************************************
for ( i=i0; i<in->dim; i++ )
out->ve[i] = in->ve[i] - scale*hh->ve[i];
************************************************************/
return (out);
}
MAT *ms_mltadd(const MAT *A1, const MAT *A2, double s, MAT *out)
#endif
{
/* register Real *A1_e, *A2_e, *out_e; */
/* register int j; */
int i, m, n;
if ( ! A1 || ! A2 )
error(E_NULL,"ms_mltadd");
if ( A1->m != A2->m || A1->n != A2->n )
error(E_SIZES,"ms_mltadd");
if ( out != A1 && out != A2 )
out = m_resize(out,A1->m,A1->n);
if ( s == 0.0 )
return m_copy(A1,out);
if ( s == 1.0 )
return m_add(A1,A2,out);
tracecatch(out = m_copy(A1,out),"ms_mltadd");
m = A1->m; n = A1->n;
for ( i = 0; i < m; i++ )
{
__mltadd__(out->me[i],A2->me[i],s,(int)n);
/**************************************************
A1_e = A1->me[i];
A2_e = A2->me[i];
out_e = out->me[i];
for ( j = 0; j < n; j++ )
out_e[j] = A1_e[j] + s*A2_e[j];
**************************************************/
}
return out;
}
/* LUfactor -- gaussian elimination with scaled partial pivoting
-- Note: returns LU matrix which is A */
MAT *LUfactor(MAT *A, PERM *pivot)
{
unsigned int i, j, m, n;
unsigned int k, k_max;
int i_max;
char MatrixTempBuffer[ 1000 ];
MatrixReal **A_v, *A_piv, *A_row;
MatrixReal max1, temp, tiny;
VEC *scale = VNULL;
if ( A==(MAT *)NULL || pivot==(PERM *)NULL ){
error(E_NULL,"LUfactor");
}
if ( pivot->size != A->m )
error(E_SIZES,"LUfactor");
m = A->m; n = A->n;
if( SET_VEC_SIZE( A->m ) <1000 )
vec_get( &scale, (void *)MatrixTempBuffer, A->m );
else
scale = v_get( A->m );
//MEM_STAT_REG(scale,TYPE_VEC);
A_v = A->me;
tiny = (MatrixReal)(10.0/HUGE_VAL);
/* initialise pivot with identity permutation */
for ( i=0; i<m; i++ )
pivot->pe[i] = i;
/* set scale parameters */
for ( i=0; i<m; i++ )
{
max1 = 0.0;
for ( j=0; j<n; j++ )
{
temp = (MatrixReal)fabs(A_v[i][j]);
max1 = mat_max(max1,temp);
}
scale->ve[i] = max1;
}
/* main loop */
k_max = mat_min(m,n)-1;
for ( k=0; k<k_max; k++ )
{
/* find best pivot row */
max1 = 0.0; i_max = -1;
for ( i=k; i<m; i++ )
if ( fabs(scale->ve[i]) >= tiny*fabs(A_v[i][k]) )
{
temp = (MatrixReal)fabs(A_v[i][k])/scale->ve[i];
if ( temp > max1 )
{ max1 = temp; i_max = i; }
}
/* if no pivot then ignore column k... */
if ( i_max == -1 )
{
/* set pivot entry A[k][k] exactly to zero,
rather than just "small" */
A_v[k][k] = 0.0;
continue;
}
/* do we pivot ? */
if ( i_max != (int)k ) /* yes we do... */
{
px_transp(pivot,i_max,k);
for ( j=0; j<n; j++ )
{
temp = A_v[i_max][j];
A_v[i_max][j] = A_v[k][j];
A_v[k][j] = temp;
}
}
/* row operations */
for ( i=k+1; i<m; i++ ) /* for each row do... */
{ /* Note: divide by zero should never happen */
temp = A_v[i][k] = A_v[i][k]/A_v[k][k];
A_piv = &(A_v[k][k+1]);
A_row = &(A_v[i][k+1]);
if ( k+1 < n )
__mltadd__(A_row,A_piv,-temp,(int)(n-(k+1)));
}
}
if( scale != (VEC *)MatrixTempBuffer ) // память выделялась, надо освободить
V_FREE(scale);
return A;
}
MAT *LUfactor(MAT *A, PERM *pivot)
#endif
{
unsigned int i, j, m, n;
int i_max, k, k_max;
Real **A_v, *A_piv, *A_row;
Real max1, temp, tiny;
STATIC VEC *scale = VNULL;
if ( A==(MAT *)NULL || pivot==(PERM *)NULL )
error(E_NULL,"LUfactor");
if ( pivot->size != A->m )
error(E_SIZES,"LUfactor");
m = A->m; n = A->n;
scale = v_resize(scale,A->m);
MEM_STAT_REG(scale,TYPE_VEC);
A_v = A->me;
tiny = 10.0/HUGE_VAL;
/* initialise pivot with identity permutation */
for ( i=0; i<m; i++ )
pivot->pe[i] = i;
/* set scale parameters */
for ( i=0; i<m; i++ )
{
max1 = 0.0;
for ( j=0; j<n; j++ )
{
temp = fabs(A_v[i][j]);
max1 = max(max1,temp);
}
scale->ve[i] = max1;
}
/* main loop */
k_max = min(m,n)-1;
for ( k=0; k<k_max; k++ )
{
/* find best pivot row */
max1 = 0.0; i_max = -1;
for ( i=k; i<m; i++ )
if ( fabs(scale->ve[i]) >= tiny*fabs(A_v[i][k]) )
{
temp = fabs(A_v[i][k])/scale->ve[i];
if ( temp > max1 )
{ max1 = temp; i_max = i; }
}
/* if no pivot then ignore column k... */
if ( i_max == -1 )
{
/* set pivot entry A[k][k] exactly to zero,
rather than just "small" */
A_v[k][k] = 0.0;
continue;
}
/* do we pivot ? */
if ( i_max != k ) /* yes we do... */
{
px_transp(pivot,i_max,k);
for ( j=0; j<n; j++ )
{
temp = A_v[i_max][j];
A_v[i_max][j] = A_v[k][j];
A_v[k][j] = temp;
}
}
/* row operations */
for ( i=k+1; i<m; i++ ) /* for each row do... */
{ /* Note: divide by zero should never happen */
temp = A_v[i][k] = A_v[i][k]/A_v[k][k];
A_piv = &(A_v[k][k+1]);
A_row = &(A_v[i][k+1]);
if ( k+1 < n )
__mltadd__(A_row,A_piv,-temp,(int)(n-(k+1)));
/*********************************************
for ( j=k+1; j<n; j++ )
A_v[i][j] -= temp*A_v[k][j];
(*A_row++) -= temp*(*A_piv++);
*********************************************/
}
}
#ifdef THREADSAFE
V_FREE(scale);
#endif
return A;
}