void mbs_close_kin(MBSdataStruct *s, LocalDataStruct *lds)
{
int i,j,k;
int nL,nC,nk;
double term;
nL = s->nhu;
nC = lds->iquc[0];
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->Juct[j][i] = lds->Jac[s->hu[i]][lds->iquc[j]];
// lds->Juc[i][j] = lds->Jac[s->hu[i]][lds->iquc[j]];
}
}
// calcul de la matrice de couplage des vitesses
for (j=1;j<=nC;j++)
{
lubksb(lds->mJv,s->nqv,lds->ind_mJv,lds->Juct[j]);
}
// gaussj(lds->mJv, s->nqv, lds->Juc, nC);
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->Bvuc[i][j] = lds->Juct[j][i];
// lds->Bvuc[i][j] = lds->Juc[i][j];
}
}
// calcul des vitesses dépendantes (qdv = Bvuc * qduc)
nL = s->nqv;
nk = lds->iquc[0];
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[i][k]*s->qd[lds->iquc[k]];
}
s->qd[s->qv[i]] = term;
}
// bp = (-Jv)\jdqd
cons_jdqd(lds->jdqd,s,s->tsim);
for(i=1;i<=s->nhu;i++)
{
lds->bp[i] = lds->jdqd[s->hu[i]];
// lds->bp[i][1] = lds->jdqd[s->hu[i]];
}
lubksb(lds->mJv,s->nqv,lds->ind_mJv,lds->bp);
// gaussj(lds->mJv, s->nqv, lds->bp, 1);
}
void
regress(float *b, float **x, float *y, int n, int dim)
{
float d;
float **xt, **xx;
int *indx;
xt = matrix(1,dim,1,n);
transpose(xt,x,n,dim); // X'
xx = matrix(1,dim,1,dim);
multiply(xx,xt,x,dim,n,dim); // X'*X
// b = vector(1,dim);
multiply(b,xt,y,dim,n); // X'*Y
// From NR:
// "To summarize, this is the preferred way to solve the linear set of equations A . x = b:
// float **a,*b,d;
// int n,*indx;
// ...
// ludcmp(a,n,indx,&d);
// lubksb(a,n,indx,b);
//
// The answer x will be given back in b. Your original matrix A will have been destroyed."
//
// In our case, we have (X'X) b = (X'Y)
indx=ivector(1,n);
ludcmp(xx,dim,indx,&d);
lubksb(xx,dim,indx,b);
}
int ns1__luinv(struct soap *soap, matrix *a, matrix *b)
{ vector col(soap);
ivector indx(soap);
double d;
int i, j, k, n;
if (ludcmp(soap, *a, indx, d))
return soap_sender_fault(soap, "Singular matrix in routine ludcmp", NULL);
n = a->size();
col.resize(n);
b->resize(n, n);
for (j = 1; j <= n; j++)
{ for (i = 1; i <= n; i++)
col[i] = 0.0;
col[j] = 1.0;
lubksb(*a, indx, col);
for (i = 1; i <= n; i++)
(*b)[i][j] = col[i];
}
for (i = 1; i <= n; i++)
{ for (j = 1; j <= n; j++)
if (fabs((*b)[i][j]) > 1.0e-15)
break;
for (k = n; k > j; k--)
if (fabs((*b)[i][k]) > 1.0e-15)
break;
(*b)[i].resize(j, k);
}
return SOAP_OK;
}
int
regress(float *b, float **x, float *y, float *w, int n, int dim)
{
float d;
float **xt, **xx;
int *indx,i,j;
xt = matrix(1,dim,1,n);
transpose(xt,x,n,dim); // X'
for(i=1;i<=dim;i++)
for(j=1;j<=n;j++)
xt[i][j] *= w[j]; // X'*W
xx = matrix(1,dim,1,dim);
multiply(xx,xt,x,dim,n,dim); // X'*W*X
// b = vector(1,dim);
multiply(b,xt,y,dim,n); // X'*W*Y
// Here we have (X'*W*X) b = (X'*W*Y)
// where W is diagonal matrix with diagonal elements w
indx=ivector(1,n);
if( ludcmp(xx,dim,indx,&d) ) return 1;
lubksb(xx,dim,indx,b);
free_matrix(xt,1,dim,1,n);
free_matrix(xx,1,dim,1,dim);
free_ivector(indx,1,n);
return 0;
}
void savgol(float c[], int np, int nl, int nr, int ld, int m)
{
void lubksb(float **a, int n, int *indx, float b[]);
void ludcmp(float **a, int n, int *indx, float *d);
int imj,ipj,j,k,kk,mm,*indx;
float d,fac,sum,**a,*b;
if (np < nl+nr+1 || nl < 0 || nr < 0 || ld > m || nl+nr < m)
nrerror("bad args in savgol");
indx=ivector(1,m+1);
a=matrix(1,m+1,1,m+1);
b=vector(1,m+1);
for (ipj=0;ipj<=(m << 1);ipj++) {
sum=(ipj ? 0.0 : 1.0);
for (k=1;k<=nr;k++) sum += pow((double)k,(double)ipj);
for (k=1;k<=nl;k++) sum += pow((double)-k,(double)ipj);
mm=FMIN(ipj,2*m-ipj);
for (imj = -mm;imj<=mm;imj+=2) a[1+(ipj+imj)/2][1+(ipj-imj)/2]=sum;
}
ludcmp(a,m+1,indx,&d);
for (j=1;j<=m+1;j++) b[j]=0.0;
b[ld+1]=1.0;
lubksb(a,m+1,indx,b);
for (kk=1;kk<=np;kk++) c[kk]=0.0;
for (k = -nl;k<=nr;k++) {
sum=b[1];
fac=1.0;
for (mm=1;mm<=m;mm++) sum += b[mm+1]*(fac *= k);
kk=((np-k) % np)+1;
c[kk]=sum;
}
free_vector(b,1,m+1);
free_matrix(a,1,m+1,1,m+1);
free_ivector(indx,1,m+1);
}
void
avtMatrix::Inverse()
{
avtMatrix n, y;
int i, j, indx[4];
double d, col[4];
n=*this;
if (ludcmp(&n, indx, &d)) {
MakeIdentity();
return;
}
for (j=0; j<4; j++) {
for (i=0; i<4; i++) {
col[i] = 0.0f;
}
col[j] = 1.0f;
lubksb(&n, indx, col);
for (i=0; i<4; i++) {
y.m[i][j] = col[i];
}
}
*this = y;
return;
}
void Newton_Solver( double * x1, double ** A, double * b, int vn)
{
int i, j;
double ger;
int *p = ivector( 1, vn);
double **LU = dmatrix( 1, vn, 1, vn);
for( i=0; i<=vn-1; i++)
for( j=0; j<=vn-1; j++)
LU[i+1][j+1] = A[i][j];
double *X = dvector( 1, vn);
for( i=0; i<=vn-1; i++)
X[i+1] = b[i];
ludcmp( LU, vn, p, &ger);
lubksb( LU, vn, p, X );
for( i=0; i<=vn-1; i++)
x1[i] = X[i+1];
}
int invm (const double *a, int N, double *y) {
/* Inverse of a real matrix a[0..N-1][0..N-1].
Input:
a[0..N-1][0..N-1] - given matrix (saved on exit)
N - number of rows and columns of a
Output:
y[0..N-1][0..N-1] - inverse of a
*/
double d, *col, *al;
int i, j, *indx;
al = (double *) calloc (sqr(N), sizeof (double));
indx = (int *) calloc (N, sizeof (int));
col = (double *) calloc (N, sizeof (double));
for (i=0; i<sqr(N); i++)
al[i] = a[i];
if (ludcmp (al, N, indx, &d))
return 1;
for (j=0; j<N; j++) {
for (i=0; i<N; i++)
col[i] = 0.0;
col[j] = 1.0;
lubksb (al, N, indx, col);
for (i=0; i<N; i++)
y[N*i+j] = col[i];
}
free (col);
free (indx);
free (al);
return 0;
} /* invm() */
//---------------------------------------------------------------------------
Matrix solveLinearSystem(const Matrix& A, const Matrix& b)
{
Matrix x;
// Make sure A is square!
if (A.numRows() != A.numCols())
{
std::cerr << "ERROR! A must be square matrix!\n";
return x;
}
// Make sure b is a column vector with the same dimensions
// as A
if (b.numRows() != A.numRows())
{
std::cerr << "ERROR! b must be a column vector with the same dimensions as square matrix A!\n";
return x;
}
// Make a copy of A since it gets modified
Matrix Acopy(A);
const int N = Acopy.numRows();
Matrix indx(N,1);
double d;
ludcmp(Acopy,indx,d);
x = b; // x will contain solution
lubksb(Acopy,indx,x);
// Return solution column vector
return x;
}
/////////////// MATRIX INVERSE!!
int
inverse(float **ainv, float **a, int n)
{
float d;
int j,i,*indx;
float *colum;
colum=vector(1,n);
indx=ivector(1,n);
// ainv=matrix(1,n,1,n);
if( ludcmp(a,n,indx,&d) ) return 1;
for(j=1;j<=n;j++)
{
for(i=1;i<=n;i++) colum[i]=0.0;
colum[j]=1.0;
lubksb(a,n,indx,colum);
for(i=1;i<=n;i++) ainv[i][j]=colum[i];
}
free_vector(colum,1,n);
free_ivector(indx,1,n);
return 0;
}
NUMERICS_EXPORT BOOL inverse(double **a, int n)
{
double d;
int i, j;
BOOL ret = FALSE;
double** ai = dmatrix(0, n - 1, 0, n - 1);
double* col = dvector(0, n - 1);
int* indx = ivector(0, n - 1);
if(ludcmp(a, n, indx, &d)){
for(j = 0; j < n; j++){
for(i = 0; i < n; i++) col[i] = 0.0;
col[j] = 1.0;
lubksb(a, n, indx, col);
for(i = 0; i < n; i++) ai[i][j] = col[i];
}
for(i = 0; i < n; i++){
for(j = 0; j < n; j++){
a[i][j] = ai[i][j];
}
}
ret = TRUE;
}
free_dmatrix(ai, 0, n - 1, 0);
free_dvector(col, 0);
free_ivector(indx, 0);
return ret;
}
for_all_active_elements(e, mesh)
{
mode = e->get_mode();
quad->set_mode(mode);
o = elem_orders[e->id];
int np = quad->get_num_points(o);
AsmList al;
space->get_element_assembly_list(e, &al);
pss->set_active_element(e);
for (int l = 0; l < num_components; l++)
{
// obtain solution values for the current element
scalar* val = mono;
elem_coefs[l][e->id] = (int) (mono - mono_coefs);
memset(val, 0, sizeof(scalar)*np);
for (int k = 0; k < al.cnt; k++)
{
pss->set_active_shape(al.idx[k]);
pss->set_quad_order(o, H2D_FN_VAL);
int dof = al.dof[k];
scalar coef = al.coef[k] * (dof >= 0 ? vec[dof] : dir);
double* shape = pss->get_fn_values(l);
for (int i = 0; i < np; i++)
val[i] += shape[i] * coef;
}
mono += np;
// solve for the monomial coefficients
if (mono_lu.mat[mode][o] == NULL)
mono_lu.mat[mode][o] = calc_mono_matrix(o, mono_lu.perm[mode][o]);
lubksb(mono_lu.mat[mode][o], np, mono_lu.perm[mode][o], val);
}
}
void Matrix_Inverse( double **invMat, double **Mat, int nn)
{
int i, j;
double ger;
int *p= ivector( 1, nn);
double **LU = dmatrix( 1, nn, 1, nn);
for( i=0; i<=nn-1; i++)
for( j=0; j<=nn-1; j++)
LU[i+1][j+1] = Mat[i][j];
double *col = dvector( 1, nn);
ludcmp( LU, nn, p, &ger);
for( j=1; j<=nn; j++) {
for( i=1; i<=nn; i++) col[i] = 0.0;
col[j] = 1.0;
lubksb( LU, nn, p, col );
for( i=1; i<=nn; i++) invMat[i-1][j-1] = (double) col[i];
};
free_dvector( col, 1, nn);
free_ivector( p, 1, nn);
free_dmatrix( LU, 1, nn, 1, nn);
}
//---------------------------------------------------------------------------
Matrix invert(const Matrix& A)
{
Matrix y(A.numRows(), A.numRows(), false);
// Make sure A is square!
if (A.numRows() != A.numCols())
{
std::cerr << "ERROR! A must be square matrix!\n";
return y;
}
const int N = A.numRows();
// Make a copy of A since it gets modified
Matrix Acopy(A);
// y.setSize(N,N);
Matrix col(N,1);
Matrix indx(N,1);
double d;
int i,j;
ludcmp(Acopy,indx,d);
for (j = 0; j < N; j++)
{
for (i = 0; i < N; i++) col(i,0) = 0.0;
col(j,0) = 1.0;
lubksb(Acopy,indx,col);
for (i = 0; i < N; i++) y(i,j) = col(i,0);
}
std::cout.flush();
// Return result
return y;
}
/* Invert a double matrix, 1-indexed of size dim
* from Numerical Recipes. Input matrix is destroyed.
*/
int
invert_matrix(double **in, double **out,
int dim)
{
extern void ludcmp(double **a, int n, int *indx, double *d);
extern void ludcmp(double **a, int n, int *indx, double *d);
int *indx,i,j;
double *tvec,det;
tvec = dvector(1,dim);
indx = ivector(1,dim);
ludcmp(in,dim,indx,&det);
for (j=1; j<=dim; j++) {
for (i=1; i<=dim; i++) tvec[i]=0.;
tvec[j] = 1.0;
lubksb(in,dim,indx,tvec);
for (i=1; i<=dim; i++) out[i][j]=tvec[i];
}
free_ivector(indx,1,6);
free_dvector(tvec,1,6);
return(0);
}
void NR::voltra(const DP t0, const DP h, Vec_O_DP &t, Mat_O_DP &f,
DP g(const int, const DP),
DP ak(const int, const int, const DP, const DP))
{
int i,j,k,l;
DP d,sum;
int m=f.nrows();
int n=f.ncols();
Vec_INT indx(m);
Vec_DP b(m);
Mat_DP a(m,m);
t[0]=t0;
for (k=0;k<m;k++) f[k][0]=g(k,t[0]);
for (i=1;i<n;i++) {
t[i]=t[i-1]+h;
for (k=0;k<m;k++) {
sum=g(k,t[i]);
for (l=0;l<m;l++) {
sum += 0.5*h*ak(k,l,t[i],t[0])*f[l][0];
for (j=1;j<i;j++)
sum += h*ak(k,l,t[i],t[j])*f[l][j];
if (k == l)
a[k][l]=1.0-0.5*h*ak(k,l,t[i],t[i]);
else
a[k][l] = -0.5*h*ak(k,l,t[i],t[i]);
}
b[k]=sum;
}
ludcmp(a,indx,d);
lubksb(a,indx,b);
for (k=0;k<m;k++) f[k][i]=b[k];
}
}
void savgol(double *c, int np, int nl, int nr, int ld, int m) {
/*-------------------------------------------------------------------------------------------
USES lubksb,ludcmp given below.
Returns in c(np), in wrap-around order (see reference) consistent with the argument respns
in routine convlv, a set of Savitzky-Golay filter coefficients. nl is the number of leftward
(past) data points used, while nr is the number of rightward (future) data points, making
the total number of data points used nl+nr+1. ld is the order of the derivative desired
(e.g., ld = 0 for smoothed function). m is the order of the smoothing polynomial, also
equal to the highest conserved moment; usual values are m = 2 or m = 4.
-------------------------------------------------------------------------------------------*/
int imj,ipj,j,k,kk,mm;
double d,fac,sum,**a,*b;
if (np < nl+nr+1 || nl < 0 || nr < 0 || ld > m || nl+nr < m)
nrerror("bad args in savgol");
int *indx= intvector(1,m+1);
a=matrix(1,m+1,1,m+1);
b=vector(1,m+1);
for (ipj=0;ipj<=(m << 1);ipj++)
{//Set up the normal equations of the desired least-squares fit
sum=(ipj ? 0.0 : 1.0);
for (k=1;k<=nr;k++)
sum += pow((double)k,(double)ipj);
for (k=1;k<=nl;k++)
sum += pow((double)-k,(double)ipj);
mm=IMIN(ipj,2*m-ipj);
for (imj = -mm;imj<=mm;imj+=2)
a[1+(ipj+imj)/2][1+(ipj-imj)/2]=sum;
}
ludcmp(a, m+1, indx, &d); //Solve them: LU decomposition.
for (j=1;j<=m+1;j++)
b[j]=0.0;
b[ld+1]=1.0; //Right-hand side vector is unit vector, depending on which derivative we want.
lubksb(a,m+1,indx,b); //Get one row of the inverse matrix.
for (kk=1;kk<=np;kk++)
c[kk]=0.0; //Zero the output array (it may be bigger than number of coefficients).
for (k = -nl;k<=nr;k++)
{
sum=b[1]; //Each Savitzky-Golay coefficient is the dot product
//of powers of an integer with the inverse matrix row.
fac=1.0;
for (mm=1;mm<=m;mm++)
sum += b[mm+1]*(fac *= k);
kk=((np-k) % np)+1; //Store in wrap-around order.
c[kk]=sum;
}
free_vector(b,1,m+1);
free_matrix(a,1,m+1,1,m+1);
free_intvector(indx,1,m+1);
}
double lik(POPROT *pop)
{/* Function to compute the inverse of the Fisher information matrix of the
population protocol pop. Returns the determinant of the matrix, also stored in
pop->det. The Fisher information matrix of the population protocol is stored
in pop->fisher and the inverse is stored in pop->finv
*/
int ndim = pop->ndim;
int ncase = (int)(ndim*(ndim+1)/2);
int i,j,jj,ifail;
int *indx;
double *col;
matrix *xa;
double cri;
/* POPROT_print(pop,1);
PROTOC_print(&prot[7]);
*/
indx = (int *)calloc(ndim,sizeof(int));
col = (double *)calloc(ndim,sizeof(double));
xa = matrix_create(ndim,ndim);
for(i = 0;i<ncase;i++)
{
pop->fisher[i] = 0;
for(j = 0;j<pop->np;j++)
{
pop->fisher[i] = pop->fisher[i]+pop->freq[j]*pop->pind[j].fisher[i];
}
}
jj = 0;
for(i = 0;i<ndim;i++)
{
for(j = 0;j<=i;j++)
{
xa->m[i][j] = pop->fisher[jj];
xa->m[j][i] = pop->fisher[jj];
jj++;
}
}
ifail = ludcmp(xa->m,ndim,indx,&cri);
if(ifail==1) return CRI_MAX;
for(i = 0;i<ndim;i++) cri = cri*xa->m[i][i];
pop->det = cri;
for(i = 0;i<ndim;i++)
{
for(j = 0;j<ndim;j++) col[j] = 0.0;
col[i] = 1.0;
lubksb(xa->m,ndim,indx,col);
for(j = 0;j<ndim;j++)
{
jj = i*(i+1)/2+j;
pop->finv[jj] = col[j]; /*=xb[j][i] using another matrix*/
}
}
/* desallocation */
matrix_destroy(xa);
free(indx);
return cri;
}
void simpr(float y[], float dydx[], float dfdx[], float **dfdy, int n,
float xs, float htot, int nstep, float yout[],
void (*derivs)(float, float [], float []))
{
void lubksb(float **a, int n, int *indx, float b[]);
void ludcmp(float **a, int n, int *indx, float *d);
int i,j,nn,*indx;
float d,h,x,**a,*del,*ytemp;
indx=ivector(1,n);
a=matrix(1,n,1,n);
del=vector(1,n);
ytemp=vector(1,n);
h=htot/nstep;
for (i=1;i<=n;i++) {
for (j=1;j<=n;j++) a[i][j] = -h*dfdy[i][j];
++a[i][i];
}
ludcmp(a,n,indx,&d);
for (i=1;i<=n;i++)
yout[i]=h*(dydx[i]+h*dfdx[i]);
lubksb(a,n,indx,yout);
for (i=1;i<=n;i++)
ytemp[i]=y[i]+(del[i]=yout[i]);
x=xs+h;
(*derivs)(x,ytemp,yout);
for (nn=2;nn<=nstep;nn++) {
for (i=1;i<=n;i++)
yout[i]=h*yout[i]-del[i];
lubksb(a,n,indx,yout);
for (i=1;i<=n;i++)
ytemp[i] += (del[i] += 2.0*yout[i]);
x += h;
(*derivs)(x,ytemp,yout);
}
for (i=1;i<=n;i++)
yout[i]=h*yout[i]-del[i];
lubksb(a,n,indx,yout);
for (i=1;i<=n;i++)
yout[i] += ytemp[i];
free_vector(ytemp,1,n);
free_vector(del,1,n);
free_matrix(a,1,n,1,n);
free_ivector(indx,1,n);
}
void CroutMatrix::Solver(MatrixColX& mcout, const MatrixColX& mcin)
{
REPORT
int i = mcin.skip; Real* el = mcin.data-i; Real* el1 = el;
while (i--) *el++ = 0.0;
el += mcin.storage; i = nrows - mcin.skip - mcin.storage;
while (i--) *el++ = 0.0;
lubksb(el1, mcout.skip);
}
int ns1__lusol(struct soap *soap, matrix *a, vector *b, vector *x)
{ ivector indx(soap);
double d;
if (ludcmp(soap, *a, indx, d))
return soap_sender_fault(soap, "Singular matrix in routine ludcmp", NULL);
lubksb(*a, indx, *b);
*x = *b;
return SOAP_OK;
}
double
cmat_invert(double**a, int sym, int dim)
{
int i,j;
double det=0;
double **y;
double *b;
b = (double*) malloc(sizeof(double)*dim);
y = cmat_new_square_matrix(dim);
if (sym) {
symm_lu_decomp(a,dim,&det);
if (fabs(det) < 1.0e-16) return 0;
for (i=0; i < dim; i++) det *= a[i][i];
if (fabs(det) < 1.0e-16) return 0;
for (i=0; i < dim; i++) {
for (j=0; j < dim; j++) b[j]=0;
b[i]=1;
symm_lu_back_sub(a,dim,b);
for (j=0; j < dim; j++) y[j][i]=b[j];
}
for (i=0; i < dim; i++)
for (j=0; j <= i; j++)
a[i][j] = y[i][j];
} else {
int *indx= (int*) malloc(sizeof(int)*dim);
ludcmp(a,dim,indx,&det);
if (fabs(det) < 1.0e-16) return 0;
for (i=0; i < dim; i++) det *= a[i][i];
if (fabs(det) < 1.0e-16) return 0;
for (i=0; i < dim; i++) {
memset(b,0,sizeof(double)*dim);
b[i]=1;
lubksb(a,dim,indx,b);
for (j=0; j < dim; j++) y[j][i]=b[j];
}
for (i=0; i < dim; i++)
for (j=0; j < dim; j++)
a[i][j] = y[i][j];
free(indx);
}
free(b);
cmat_delete_matrix(y);
return det;
}
int ns1__lusols(struct soap *soap, matrix *a, matrix *b, matrix *x)
{ ivector indx(soap);
double d;
if (ludcmp(soap, *a, indx, d))
return soap_sender_fault(soap, "Singular matrix in routine ludcmp", NULL);
for (int i = 1; i <= b->size(); i++)
lubksb(*a, indx, (*b)[i]);
*x = *b;
return SOAP_OK;
}
log_value<dcomplex>
TransposeInverseMatrix(const Array2 < complex <doublevar> > & a,
Array2 < complex <doublevar> > & a1,
const int n)
{
Array2 <complex <doublevar> > temp(n,n);
Array1 <int> indx(n);
doublevar d;
// a(i,j) first index i is row index (convention)
// elements of column vectors are stored contiguous in memory in C style arrays
// a(i) refers to a column vector
// calculate the inverse of the transposed matrix because this
// allows to pass a column vector to lubksb() instead of a row
// put the transposed matrix in temp
//cout << "temp " << endl;
for(int i=0;i<n;++i)
{
for(int j=0;j<n;++j)
{
temp(i,j)=a(i,j);
a1(i,j)=complex <doublevar> (0.0,0.0);
}
a1(i,i)=complex <doublevar> (1.0,0.0);
}
//cout << "ludcmp" << endl;
//if the matrix is singular, the determinant is zero.
if(ludcmp(temp,n,indx,d)==0) {
#ifdef SUPERDEBUG
cout << "log_value<dcomplex>TransposeInverseMatrix:zero determinant " << endl;
#endif
return dcomplex(0.0,0.0);
}
//cout << "lubksb" << endl;
for(int j=0;j<n;++j)
{
// get column vector
Array1 <complex <doublevar> > yy;//(a1(j));
yy.refer(a1(j));
lubksb(temp,n,indx,yy);
}
//complex <doublevar> det(d,0);
log_value<dcomplex> det=dcomplex(d,0);
for(int j=0;j<n;++j) {
det *= temp(j,j);
}
return det;
}
int invert_matrix(covdata **mat, covdata **inv, int dim)
{
double *col, **input;
int ii, jj, *index, err_code;
#if DEBUG
double sum;
int kk;
#endif
ASSERT(mat);
ASSERT(inv);
index = (int *) CALLOC(dim, sizeof(int), "clib.index_imatrix");
col = (double *) CALLOC(dim, sizeof(double), "clib.col");
input = (double **) CALLOC(dim, sizeof(double *), "clib.input_imatrix");
for (ii = 0; ii < dim; ii++)
{
input[ii] = (double *) CALLOC(dim, sizeof(double), "clib.input_imatrix[]");
for (jj = 0; jj < dim; jj++)
input[ii][jj] = mat[ii][jj];
}
if ((err_code = ludcmp(input, dim, index)) > 0) return(err_code);
for (jj = 0; jj < dim; jj++)
{
for (ii = 0; ii < dim; ii++)
col[ii] = 0;
col[jj] = 1;
lubksb(input, dim, index, col);
for (ii = 0; ii < dim; ii++)
inv[ii][jj] = col[ii];
}
for (ii = 0; ii < dim; ii++)
FREE((char *)input[ii]);
FREE((char *)input);
FREE((char *)col);
FREE((char *)index);
#if DEBUG
printf("Testing the inverse:\n");
for (ii = 0; ii < dim; ii++)
{
for (jj = 0; jj < dim; jj++)
{
sum = 0;
for (kk = 0; kk < dim; kk++)
sum += mat[ii][kk] * inv[kk][jj];
printf("%.2f ", sum);
}
printf("\n");
}
#endif
return (0);
}
int mbs_close_geo(MBSdataStruct *s, LocalDataStruct *lds)
{
int i,j;
int iter=0;
int nL,nC;
double d;
iter = 0;
lds->norm_h=1.0;
while((lds->norm_h > lds->NRerr) && (iter++ <= lds->MAX_NR_ITER))
{
// Calcul des contraintes et de la Jacobienne
mbs_calc_hJ(lds,s,s->tsim);
// Norme des contraintes (en supposant que toutes les contraintes indépendantes sont au début ???)
lds->norm_h = norminf_vector(lds->h,s->nhu);
// -Jv
nL = s->nhu;
nC = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->mJv[i][j] = -lds->Jac[s->hu[i]][s->qv[j]];
}
}
// Décomposition LU de la matrice -Jv
ludcmp(lds->mJv,s->nqv,lds->ind_mJv,&d);
if(lds->norm_h > lds->NRerr)
{
// err
for(i=1;i<=s->nhu;i++)
{
lds->mJv_h[i] = lds->h[s->hu[i]];
// lds->mJv_h[i][1] = lds->h[s->hu[i]];
}
lubksb(lds->mJv,s->nqv,lds->ind_mJv,lds->mJv_h);
// gaussj(lds->mJv, s->nqv, lds->mJv_h, 1);
// Correction des qv
for(i=1;i<=s->nhu;i++)
{
s->q[s->qv[i]] += lds->mJv_h[i];
// s->q[s->qv[i]] += lds->mJv_h[i][1];
}
}
}
return iter;
}
void solve_LES (double **a, double *b, int n)
{
double d;
int *indx, *ivector();
void ludcmp(), lubksb();
indx = ivector(1,n);
ludcmp(a,n,indx,&d);
lubksb(a,n,indx,b);
}
void NR::simpr(Vec_I_DP &y, Vec_I_DP &dydx, Vec_I_DP &dfdx, Mat_I_DP &dfdy,
const DP xs, const DP htot, const int nstep, Vec_O_DP &yout,
void derivs(const DP, Vec_I_DP &, Vec_O_DP &))
{
int i,j,nn;
DP d,h,x;
int n=y.size();
Mat_DP a(n,n);
Vec_INT indx(n);
Vec_DP del(n),ytemp(n);
h=htot/nstep;
for (i=0;i<n;i++) {
for (j=0;j<n;j++) a[i][j] = -h*dfdy[i][j];
++a[i][i];
}
ludcmp(a,indx,d);
for (i=0;i<n;i++)
yout[i]=h*(dydx[i]+h*dfdx[i]);
lubksb(a,indx,yout);
for (i=0;i<n;i++)
ytemp[i]=y[i]+(del[i]=yout[i]);
x=xs+h;
derivs(x,ytemp,yout);
for (nn=2;nn<=nstep;nn++) {
for (i=0;i<n;i++)
yout[i]=h*yout[i]-del[i];
lubksb(a,indx,yout);
for (i=0;i<n;i++) ytemp[i] += (del[i] += 2.0*yout[i]);
x += h;
derivs(x,ytemp,yout);
}
for (i=0;i<n;i++)
yout[i]=h*yout[i]-del[i];
lubksb(a,indx,yout);
for (i=0;i<n;i++)
yout[i] += ytemp[i];
}
void pade(double cof[], int n, float *resid)
{
void lubksb(float **a, int n, int *indx, float b[]);
void ludcmp(float **a, int n, int *indx, float *d);
void mprove(float **a, float **alud, int n, int indx[], float b[],
float x[]);
int j,k,*indx;
float d,rr,rrold,sum,**q,**qlu,*x,*y,*z;
indx=ivector(1,n);
q=matrix(1,n,1,n);
qlu=matrix(1,n,1,n);
x=vector(1,n);
y=vector(1,n);
z=vector(1,n);
for (j=1;j<=n;j++) {
y[j]=x[j]=cof[n+j];
for (k=1;k<=n;k++) {
q[j][k]=cof[j-k+n];
qlu[j][k]=q[j][k];
}
}
ludcmp(qlu,n,indx,&d);
lubksb(qlu,n,indx,x);
rr=BIG;
do {
rrold=rr;
for (j=1;j<=n;j++) z[j]=x[j];
mprove(q,qlu,n,indx,y,x);
for (rr=0.0,j=1;j<=n;j++)
rr += SQR(z[j]-x[j]);
} while (rr < rrold);
*resid=sqrt(rrold);
for (k=1;k<=n;k++) {
for (sum=cof[k],j=1;j<=k;j++) sum -= z[j]*cof[k-j];
y[k]=sum;
}
for (j=1;j<=n;j++) {
cof[j]=y[j];
cof[j+n] = -z[j];
}
free_vector(z,1,n);
free_vector(y,1,n);
free_vector(x,1,n);
free_matrix(qlu,1,n,1,n);
free_matrix(q,1,n,1,n);
free_ivector(indx,1,n);
}
int clust_invert(
double **a, /* input/output matrix */
int n, /* dimension */
double *det_man, /* determinant mantisa */
int *det_exp, /* determinant exponent */
/* scratch space */
int *indx, /* indx = G_alloc_ivector(n); */
double **y, /* y = G_alloc_matrix(n,n); */
double *col /* col = G_alloc_vector(n); */
)
{
int i,j;
double d_man;
int d_exp;
d_exp = 0;
if(ludcmp(a,n,indx,&d_man)) {
for(j=0; j<n; j++) {
d_man *= a[j][j];
while( double_abs(d_man)>10 ) {
d_man = d_man/10;
d_exp++;
}
while( (double_abs(d_man)<0.1)&&(double_abs(d_man)>0) ) {
d_man = d_man*10;
d_exp--;
}
}
*det_man = d_man;
*det_exp = d_exp;
for(j=0; j<n; j++) {
for(i=0; i<n; i++) col[i]=0.0;
col[j]=1.0;
lubksb(a,n,indx,col);
for(i=0; i<n; i++) y[i][j]=col[i];
}
for(i=0; i<n; i++)
for(j=0; j<n; j++) a[i][j]=y[i][j];
return(1);
}
else {
*det_man = 0.0;
*det_exp = 0;
return(0);
}
}
