00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #ifdef HAVE_CONFIG_H
00034 # include <config.h>
00035 #endif
00036
00037
00038
00039 #include "sinfo_matrix.h"
00048
00049
00050
00051 #define dtiny(a) ((a)<0?(a)> -1.e-30:(a)<1.e-30)
00052
00053
00054
00055 static int gauss_pivot(double *ptra, double *ptrc, int n);
00056
00057
00058
00059
00071 Matrix
00072 sinfo_create_mx(int nr, int nc)
00073 {
00074 Matrix b;
00075 b = (Matrix)cpl_calloc(1,sizeof(sinfo_eclipse_matrix));
00076 b->m = (double*)cpl_calloc(nr*nc,sizeof(double));
00077 b->nr= nr;
00078 b->nc= nc;
00079 return b;
00080 }
00081
00091 Matrix
00092 sinfo_copy_mx(Matrix a)
00093 {
00094 Matrix b = sinfo_create_mx(a->nr,a->nc);
00095 if (b!=NULL) {
00096 register int s = a->nr*a->nc;
00097 register double *mm = b->m+s;
00098 register double *am = a->m+s;
00099 while (s--) *--mm = *--am;
00100 }
00101 return b;
00102 }
00103
00113 void
00114 sinfo_close_mx(Matrix a)
00115 {
00116 if (a==NULL) return ;
00117 if (a->m != NULL)
00118 cpl_free(a->m);
00119 cpl_free(a);
00120 return;
00121 }
00122
00133 Matrix
00134 sinfo_mul_mx(Matrix a, Matrix b)
00135 {
00136 Matrix c, d;
00137 int n1=a->nr, n2=a->nc, n3=b->nc;
00138 register double *a0;
00139 register double *c0;
00140 register double *d0;
00141 register int i,j,k;
00142
00143 if(n2!=b->nr) return NULL;
00144 c = sinfo_create_mx(n1,n3);
00145 d = sinfo_transp_mx(b);
00146
00147 for (i=0,c0=c->m;i<n1;i++)
00148 for (j=0,d0=d->m;j<n3;j++,c0++)
00149 for (k=0,*c0=0,a0=a->m+i*n2;k<n2;k++)
00150 *c0 += *a0++ * *d0++;
00151 sinfo_close_mx(d);
00152 return c;
00153 }
00154
00166 Matrix
00167 sinfo_invert_mx(Matrix aa)
00168 {
00169 Matrix bb;
00170 int test=1;
00171
00172 if(aa->nr!=aa->nc) return NULL;
00173 bb = sinfo_create_mx(aa->nr,aa->nc);
00174
00175 if(aa->nr==1)
00176 {
00177 double det;
00178 register double ted;
00179 det= *(aa->m);
00180 if(dtiny(det)) test=0;
00181 ted=1./det;
00182 *(bb->m)=ted;
00183 }
00184 else if(aa->nr==2)
00185 {
00186 double det;
00187 register double ted;
00188 register double *mm=aa->m;
00189 double a= *(mm++),b= *(mm++);
00190 double c= *(mm++),d= *(mm);
00191 det=a*d-b*c;
00192 if(dtiny(det)) test=0;
00193 ted=1./det;
00194 mm=bb->m;
00195 *(mm++)= d*ted,*(mm++)= -b*ted;
00196 *(mm++)= -c*ted,*(mm)= a*ted;
00197 }
00198 else if(aa->nr==3)
00199 {
00200 double det;
00201 register double ted;
00202 register double *mm=aa->m;
00203 double a= *(mm++),b= *(mm++),c= *(mm++);
00204 double d= *(mm++),e= *(mm++),f= *(mm++);
00205 double g= *(mm++),h= *(mm++),i= *(mm);
00206 det=a*e*i-a*h*f-b*d*i+b*g*f+c*d*h-c*g*e;
00207 if(dtiny(det)) test=0;
00208 ted=1./det;
00209 mm=bb->m;
00210 *(mm++)=(e*i-f*h)*ted,
00211 *(mm++)=(c*h-b*i)*ted,
00212 *(mm++)=(b*f-e*c)*ted;
00213
00214 *(mm++)=(f*g-d*i)*ted,
00215 *(mm++)=(a*i-g*c)*ted,
00216 *(mm++)=(d*c-a*f)*ted;
00217
00218 *(mm++)=(d*h-g*e)*ted,
00219 *(mm++)=(g*b-a*h)*ted,
00220 *(mm)=(a*e-d*b)*ted;
00221 }
00222 else
00223 {
00224 Matrix temp=sinfo_copy_mx(aa);
00225 if(gauss_pivot(temp->m,bb->m,aa->nr)==0) test=0;
00226 sinfo_close_mx(temp);
00227 }
00228 if(test==0)
00229 {
00230 sinfo_msg_error("not invertible, aborting inversion");
00231 return NULL;
00232 }
00233 return bb;
00234 }
00235
00245 Matrix
00246 sinfo_transp_mx(Matrix a)
00247 {
00248 register int nc=a->nc, nr=a->nr;
00249 register double *a0;
00250 register double *b0;
00251 register int i,j;
00252 Matrix b = sinfo_create_mx(nc,nr);
00253
00254 if (b == (Matrix)NULL) return b ;
00255 for (i=0,b0=b->m;i<nc;i++)
00256 for (j=0,a0=a->m+i;j<nr;j++,a0+=nc,b0++)
00257 *b0 = *a0;
00258 return b;
00259 }
00260
00273 static int
00274 gauss_pivot(double *ptra, double *ptrc, int n)
00275
00276 {
00277 #define SINFO_ABS(a) (((a) > 0) ? (a) : -(a))
00278
00279 register int i,j,k,l;
00280 int maj;
00281 double max,r,t;
00282 double *ptrb;
00283
00284 ptrb=(double *)cpl_calloc(n*n,sizeof(double));
00285 for(i=0;i<n;i++)
00286 ptrb[i*n+i]= 1.0;
00287
00288 for (i=1;i <= n;i++)
00289 {
00290
00291 max = SINFO_ABS(*(ptra + n*i-n));
00292 maj = i;
00293 for (j = i;j <= n;j++)
00294 if (SINFO_ABS(*(ptra+n*j+i-n-1)) > max)
00295 {
00296 maj = j;
00297 max = SINFO_ABS(*(ptra+n*j+i-n-1));
00298 }
00299
00300
00301 if (maj != i)
00302 {
00303 for (j = i;j <= n;j++)
00304 {
00305 r = *(ptra+n*maj+j-n-1);
00306 *(ptra+n*maj+j-n-1) = *(ptra+n*i+j-n-1);
00307 *(ptra+n*i+j-n-1) = r;
00308 }
00309 for(l=0;l<n;l++)
00310 {
00311 r = *(ptrb+l*n+maj-1);
00312 *(ptrb+l*n+maj-1) = *(ptrb+l*n+i-1);
00313 *(ptrb+l*n+i-1) = r;
00314 }
00315 }
00316
00317
00318 for (j = i + 1;j <= n;j++)
00319 {
00320 t = (*(ptra+(n+1)*i-n-1));
00321 if(dtiny(t)) return(0);
00322 r = (*(ptra+n*j+i-n-1)) / t;
00323 for(l=0;l<n;l++)
00324 *(ptrb+l*n+j-1) -= r * (*(ptrb+l*n+i-1));
00325 for (k = i;k <= n;k++)
00326 *(ptra+n*j+k-n-1) -= r * (*(ptra+n*i+k-n-1));
00327 }
00328 }
00329
00330
00331 for(l=0;l<n;l++)
00332 for (i = n;i >= 1;i--)
00333 {
00334 t = (*(ptra+(n+1)*i-n-1));
00335 if(dtiny(t)) return(0);
00336 *(ptrc+l+(i-1)*n) = (*(ptrb+l*n+i-1)) / t;
00337 if (i > 1)
00338 for (j = i - 1;j > 0;j--)
00339 *(ptrb+l*n+j-1) -= (*(ptra+n*j+i-n-1)) *
00340 (*(ptrc+l+(i-1)*n));
00341 }
00342 cpl_free(ptrb);
00343 return(1);
00344 }
00345
00365 Matrix
00366 sinfo_least_sq_mx(
00367 Matrix A,
00368 Matrix B
00369 )
00370 {
00371 Matrix m1,
00372 m2,
00373 m3,
00374 m4,
00375 m5 ;
00376
00377
00378
00379 m1 = sinfo_transp_mx(A) ;
00380 m2 = sinfo_mul_mx(A, m1) ;
00381 m3 = sinfo_invert_mx(m2) ;
00382 m4 = sinfo_mul_mx(B, m1) ;
00383 m5 = sinfo_mul_mx(m4, m3) ;
00384
00385 sinfo_close_mx(m1) ;
00386 sinfo_close_mx(m2) ;
00387 sinfo_close_mx(m3) ;
00388 sinfo_close_mx(m4) ;
00389
00390 return m5 ;
00391 }
00392
00405 void
00406 sinfo_print_mx(
00407 Matrix M,
00408 const char * name
00409 )
00410 {
00411 int i, j ;
00412
00413 fprintf(stdout, "# sinfo_eclipse_matrix %s is [%d x %d]\n",
00414 name, M->nr, M->nc) ;
00415 for (j=0 ; j<M->nr ; j++) {
00416 for (i=0 ; i<M->nc ; i++) {
00417 fprintf(stdout, "%g\t", M->m[i+j*M->nc]) ;
00418 }
00419 fprintf(stdout, "\n") ;
00420 }
00421 fprintf(stdout, "\n") ;
00422 }
00423