JPRED-2 Add sources of all binaries (except alscript) to Git
[jpred.git] / sources / multicoil / matrix_operations.c
diff --git a/sources/multicoil/matrix_operations.c b/sources/multicoil/matrix_operations.c
new file mode 100644 (file)
index 0000000..06b0098
--- /dev/null
@@ -0,0 +1,347 @@
+/*** These are from numerical_recipes  in  C. */
+/*** PROBLEM: Thier counts went from 1 to N. **/
+/*** I Changed that. ***/
+#include <stdio.h>
+#include <math.h>
+#include "scconst.h"
+
+#define MAX_MATRIX_SIZE MAX_NUM_SCORE_DIM
+
+/*** gaussj replaces a by its inverse?? If b starts as identity matrix?? ***/
+#define SWAP(a,b) {double temp=(a);(a)=(b);(b)=temp;}
+
+void gaussj(a,n,b,m)
+double a[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],b[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
+int n,m;
+{
+       int *indxc,*indxr,*ipiv;
+       int i,icol,irow,j,k,l,ll,*ivector();
+       double big,dum,pivinv;
+       void nrerror(),free_ivector();
+
+       indxc=ivector(1,n);
+       indxr=ivector(1,n);
+       ipiv=ivector(1,n);
+       for (j=0;j<n;j++) ipiv[j]=0;
+       for (i=0;i<n;i++) {
+               big=0.0;
+               for (j=0;j<n;j++)
+                       if (ipiv[j] != 1)
+                               for (k=0;k<n;k++) {
+                                       if (ipiv[k] == 0) {
+                                               if (fabs(a[j][k]) >= big) {
+                                                       big=fabs(a[j][k]);
+                                                       irow=j;
+                                                       icol=k;
+                                               }
+                                       } else if (ipiv[k] > 1) nrerror("GAUSSJ: Singular Matrix-1");
+                               }
+               ++(ipiv[icol]);
+               if (irow != icol) {
+                       for (l=0;l<n;l++) SWAP(a[irow][l],a[icol][l])
+                       for (l=0;l<m;l++) SWAP(b[irow][l],b[icol][l])
+               }
+               indxr[i]=irow;
+               indxc[i]=icol;
+               if (a[icol][icol] == 0.0) nrerror("GAUSSJ: Singular Matrix-2");
+               pivinv=1.0/a[icol][icol];
+               a[icol][icol]=1.0;
+               for (l=0;l<n;l++) a[icol][l] *= pivinv;
+               for (l=0;l<m;l++) b[icol][l] *= pivinv;
+               for (ll=0;ll<n;ll++)
+                       if (ll != icol) {
+                               dum=a[ll][icol];
+                               a[ll][icol]=0.0;
+                               for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
+                               for (l=0;l<m;l++) b[ll][l] -= b[icol][l]*dum;
+                       }
+       }
+       for (l=n-1;l>=0;l--) {
+               if (indxr[l] != indxc[l])
+                       for (k=0;k<n;k++)
+                               SWAP(a[k][indxr[l]],a[k][indxc[l]]);
+       }
+       free_ivector(ipiv,1,n);
+       free_ivector(indxr,1,n);
+       free_ivector(indxc,1,n);
+}
+
+#undef SWAP
+
+
+
+/***************************************************************/
+
+
+#define TINY 1.0e-20;
+
+void ludcmp(a,n,indx,d)
+int n,*indx;
+double a[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],*d;
+{
+       int i,imax,j,k;
+       double big,dum,sum,temp;
+       double *vv,*vector();
+       void nrerror(),free_vector();
+
+/*     fprintf(stderr,"\n Declaring space for vv"); */
+/*     vv=vector(1,n); */
+       vv=vector(0,n-1);
+/*     fprintf(stderr,"\nSuccessfully declared vv"); */
+
+       *d=1.0;
+       for (i=0;i<n;i++) {
+               big=0.0;
+               for (j=0;j<n;j++) {
+                       if ((temp=fabs(a[i][j])) > big) big=temp;
+                     }
+               if (big == 0.0) nrerror("Singular matrix in routine LUDCMP");
+               vv[i]=1.0/big;
+       }
+
+       for (j=0;j<n;j++) {
+               for (i=0;i<j;i++) {
+                       sum=a[i][j];
+                       for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
+                       a[i][j]=sum;
+               }
+               big=0.0;
+               for (i=j;i<n;i++) {
+                       sum=a[i][j];
+                       for (k=0;k<j;k++)
+                               sum -= a[i][k]*a[k][j];
+                       a[i][j]=sum;
+                       if ( (dum=vv[i]*fabs(sum)) >= big) {
+                               big=dum;
+                               imax=i;
+                       }
+               }
+               if (j != imax) {
+                       for (k=0;k<n;k++) {
+                               dum=a[imax][k];
+                               a[imax][k]=a[j][k];
+                               a[j][k]=dum;
+                       }
+                       *d = -(*d);
+                       vv[imax]=vv[j];
+               }
+               indx[j]=imax;
+               if (a[j][j] == 0.0) a[j][j]=TINY;
+               if (j != n-1) {
+                       dum=1.0/(a[j][j]);
+                       for (i=j+1;i<n;i++) a[i][j] *= dum;
+               }
+       }
+
+/*     free_vector(vv,1,n);  */
+       free_vector(vv,0,n-1);
+
+}
+
+#undef TINY
+
+
+
+
+/***********************************/
+
+void lubksb(a,n,indx,b)
+double a[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],b[MAX_MATRIX_SIZE];
+int n,indx[MAX_MATRIX_SIZE];
+{
+       int i,ii=-1,ip,j;
+       double sum;
+
+       for (i=0;i<n;i++) {
+               ip=indx[i];
+               sum=b[ip];
+               b[ip]=b[i];
+               if (ii != -1)
+                       for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
+               else if (sum) ii=i;
+               b[i]=sum;
+       }
+       for (i=n-1;i>=0;i--) {
+               sum=b[i];
+               for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
+               b[i]=sum/a[i][i];
+       }
+}
+
+
+
+/*****************************************************/
+
+
+/*** 
+  matrix and inverse should be n X n arrays.
+  indx should be an array of n
+  col should be an array of n
+****/
+/*** If just want det then set inverse to NULL ***/
+void matrix_inverse_and_det (double matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],
+                            double inverse[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],
+                            double *det,int n)
+{
+  int i,j;
+  double  temp_matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
+  double col[MAX_MATRIX_SIZE];
+  int indx[MAX_MATRIX_SIZE];
+
+
+  for (i=0; i<n; i++) 
+    for (j=0; j<n; j++)      /** So matrix doesn't get destroyed. **/
+      temp_matrix[i][j] = matrix[i][j];  
+
+/**  fprintf(stderr,"\n Matrix to invert + find det of is:\n");
+  print_matrix(matrix,n,n,stderr);
+***/
+
+/*  fprintf(stderr,"\n Into ludcmp"); */
+  
+  ludcmp(temp_matrix,n,indx,det);
+
+
+  for (j=0; j<n; j++)
+    *det *= temp_matrix[j][j];
+  
+/*  fprintf(stderr,"\n did det=%lf",*det); */
+
+/*** Now convert current form of "inverse" into the real matrix inverse.  */
+/** only if want to compute inverse though. ****/
+  if (inverse)
+    for (j=0; j<n; j++) {
+      for (i=0; i<n; i++)
+       col[i] = 0.0;
+      col[j]=1.0;
+
+      lubksb(temp_matrix,n,indx,col);
+      for (i=0; i<n; i++)
+       inverse[i][j] = col[i];
+    }
+
+}
+
+
+
+/*************************************************************************/
+
+
+double determ(double matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], int n)
+{
+  double det;
+  matrix_inverse_and_det (matrix, NULL,   &det,n);
+
+  return (det);
+}
+    
+
+
+void  matrix_mult(int num_rows1, int num_columns1_rows2, int num_columns2,
+                 double matrix1[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], 
+                 double matrix2[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], 
+                 double matrix_product[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE])
+{
+
+  int i, j, k;
+
+  for (i=0; i<num_rows1; i++)
+    for(k=0; k<num_columns2; k++) {
+      matrix_product[i][k]=0;
+      for (j=0; j<num_columns1_rows2; j++)
+       matrix_product[i][k] += matrix1[i][j] * matrix2[j][k];
+    }
+
+}
+
+
+
+
+
+
+/** Return 0 if x had some -HUGE_VAL values in it, since that means **/
+/** the normalized matrix is grabage. ***?
+/*** compute x(transpose)  * M * x ******/
+int normalize (int num_M_row_column, 
+                int num_x_column, int num_x_row, 
+                double out_matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],
+                double M[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], 
+                double x[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE])
+
+{
+  double x_trans[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE]; 
+  int i,k;
+  double product1[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
+  int all_x_non_infinity =1;
+
+  for(i=0; i<num_x_column; i++)
+    for (k=0; k < num_x_row; k++) {
+      x_trans[i][k] = x[k][i]; 
+      if (x[k][i] <= -HUGE_VAL) 
+       all_x_non_infinity=0;
+    }
+
+
+  if (num_x_row != num_M_row_column) {   /** num_x_row = num_xtrans_column. **/
+    fprintf(stderr, "\n Error:  Can't multiply transpose by matrix because number_x_row = %d, number_M_row_column=%d.", num_x_row, num_M_row_column);
+    exit(-1);
+  }
+
+  matrix_mult(num_x_column, num_M_row_column, num_M_row_column, x_trans, M, 
+              product1);
+  matrix_mult(num_x_column, num_M_row_column, num_x_column, product1, x, 
+              out_matrix);
+
+/***********
+  if (all_x_non_infinity) {
+    fprintf(stderr,"\n x_trans=");
+    print_matrix(x_trans, num_x_column, num_M_row_column,stderr);
+    fprintf(stderr,"\n M=");
+    print_matrix(M, num_M_row_column, num_M_row_column,stderr);
+    fprintf(stderr,"\n x_trans*M=");
+    print_matrix(product1, num_x_column, num_M_row_column,stderr);
+    fprintf(stderr,"\n x_trans*M*x=");
+    print_matrix(out_matrix, num_x_column,num_x_column,stderr);
+  }
+*************/
+
+  if (all_x_non_infinity) return 1;
+  else return 0;
+
+}
+
+
+
+
+
+
+print_matrix(double M[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], int num_rows, 
+                 int num_columns, FILE *f_out)
+{
+  int i,j;
+
+  for (i=0; i<num_rows; i++){ 
+    fprintf(f_out,"\n");
+    for (j=0;j<num_columns;j++)
+      fprintf(f_out," %lf",M[i][j]);
+  }
+  fprintf(f_out,"\n");
+}
+
+
+/*** For printing a matrix of size used for computing ALL dimensions. ***/
+print_matrix2(double M[NUM_DIM_IN_ORIG_MATRIX][NUM_DIM_IN_ORIG_MATRIX], 
+             int num_rows, int num_columns, FILE *f_out)
+{
+  int i,j;
+
+  for (i=0; i<num_rows; i++){ 
+    fprintf(f_out,"\n");
+    for (j=0;j<num_columns;j++)
+      fprintf(f_out," %lf",M[i][j]);
+  }
+  fprintf(f_out,"\n");
+}