sinfo_matrix.c

00001 /*
00002  * This file is part of the ESO SINFONI Pipeline
00003  * Copyright (C) 2004,2005 European Southern Observatory
00004  *
00005  * This program is free software; you can redistribute it and/or modify
00006  * it under the terms of the GNU General Public License as published by
00007  * the Free Software Foundation; either version 2 of the License, or
00008  * (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
00018  */
00019 /*----------------------------------------------------------------------------
00020    
00021    File name     :    sinfo_matrix.c
00022    Author         :    Nicolas Devillard
00023    Created on    :    1994
00024    Description    :    basic 2d sinfo_eclipse_matrix handling routines
00025 
00026  ---------------------------------------------------------------------------*/
00027 /*
00028  $Id: sinfo_matrix.c,v 1.3 2007/06/06 07:10:45 amodigli Exp $
00029  $Author: amodigli $
00030  $Date: 2007/06/06 07:10:45 $
00031  $Revision: 1.3 $
00032  */
00033 #ifdef HAVE_CONFIG_H
00034 #  include <config.h>
00035 #endif
00036 /*----------------------------------------------------------------------------
00037                                    Includes
00038  ---------------------------------------------------------------------------*/
00039 #include "sinfo_matrix.h"
00048 /*----------------------------------------------------------------------------
00049                                    Macros
00050  ---------------------------------------------------------------------------*/
00051 #define dtiny(a) ((a)<0?(a)> -1.e-30:(a)<1.e-30)
00052 /*----------------------------------------------------------------------------
00053                         Private function prototypes
00054  ---------------------------------------------------------------------------*/
00055 static int gauss_pivot(double *ptra, double *ptrc, int n);
00056 /*----------------------------------------------------------------------------
00057                               Function codes
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 /* c(n,n) = a(n,n)^-1 */
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         /* Search max in current column  */
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         /* swap lines i and maj */
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         /* Subtract line by line */
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     /* Triangular system resolution    */
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 

Generated on 8 Mar 2011 for SINFONI Pipeline Reference Manual by  doxygen 1.6.1