lsq.c

射线追踪程序

/*
 * This file is part of tomo3d
 *
 * Copyright (C) 2002, 2003, Sebastien Judenherc <[e-mail]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
 * USA
 *
 */
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <unistd.h>
#include <assert.h>

clock_t __lclock=0;
#define PROGRESS(k,N) { if (clock()>__lclock+CLOCKS_PER_SEC) { fprintf(stderr,"%6d/%6d",k,N); __lclock=clock(); } }

#ifdef DISK
#include "sparse_disk.h"
#else
#include "sparse.h"
#endif

#define EPS 1.0e-9

#define __abs(a)  ((a>0.0)?a:-a)

clock_t CC[10];
int Ncc=0;

void dump(char *fn, float **mat, int M,int N)
{
        FILE *f;
        float titi;
        int i,j;
        f=fopen(fn,"wb");
        if (!N) {
                for (i=0; i<M; i++)
                        for (j=0; j<=i; j++) {
                                titi=(float)i;
                                fwrite(&titi,sizeof(float),1,f);
                                titi=(float)j;
                                fwrite(&titi,sizeof(float),1,f);
                                fwrite(&(mat[i][j]),sizeof(float),1,f);
                                if (i!=j) {
                                        titi=(float)j;
                                        fwrite(&titi,sizeof(float),1,f);
                                        titi=(float)i;
                                        fwrite(&titi,sizeof(float),1,f);
                                        fwrite(&(mat[i][j]),sizeof(float),1,f);
                                }
                        }
        } else {
                for (i=0; i<M; i++)
                        for (j=0; j<N; j++) {
                                titi=(float)i;
                                fwrite(&titi,sizeof(float),1,f);
                                titi=(float)j;
                                fwrite(&titi,sizeof(float),1,f);
                                fwrite(&(mat[i][j]),sizeof(float),1,f);
                        }
        }
        fclose(f);
}

void ptime(int ncc, int comm)
{ switch (comm) {
        case -1:
                for (ncc=0; ncc<10; ncc++) CC[ncc]=0;
                break;
        case  1:
                CC[ncc]=clock();
                break;
        case  2:
                fprintf(stderr,"[%.2f sec]          ",
                                (float)(clock()-CC[ncc])/(float)CLOCKS_PER_SEC);
                break;
                }
}


/* inversion de A par resolution de Ax=b ou x est une
 * colonne de inv(A) et b un vecteur de base
 * A triangulaire inf閞ieure
 * A est d閠ruite et continet inv(A) est sortie */
float ** inv_cholesky(float **A, int N)
{
        float **L;
        float **Linv;
        float *z,x,*v,*w;
        register int i,j,k;
        double sum;
        if ((z=(float*)calloc(N,sizeof(float)))==NULL) {
                fprintf(stderr,"[inv_cholesky]: malloc error (z)\n");
                exit(1);
        }
        if ((L=(float**)calloc(N,sizeof(float*)))==NULL) {
                fprintf(stderr,"[inv_cholesky]: malloc error (L)\n");
                exit(1);
        }
        for (i=0; i<N; i++) {
                if ((L[i]=(float*)calloc(i+1,sizeof(float)))==NULL) {
                        fprintf(stderr,"[inv_cholesky] malloc error (L:%d)\n",i);
                        exit(1);
                }
        }
        if ((Linv=(float**)calloc(N,sizeof(float*)))==NULL) {
                fprintf(stderr,"[inv_cholesky]: malloc error (Linv)\n");
                exit(1);
        }
        for (i=0; i<N; i++) {
                if ((Linv[i]=(float*)calloc(i+1,sizeof(float)))==NULL) {
                        fprintf(stderr,"[inv_cholesky] malloc error (Linv:%d)\n",i);
                        exit(1);
                }
        }
        /* A est sym閠rique d閒inie positive,
         * On cherche L triangulaire inf閞ieure telle que A=L*transp(L)
         */
        fprintf(stderr,"<L>");
        ptime(3,1);
        /* colonne par colonne (k);*/
        for (k=0; k<N; k++) {
                PROGRESS(k,N);
                v=L[k];
                /* 閘閙ent diagonal */
                sum=A[k][k];
                for (i=0; i<k; i++) {
                        sum-=(v[i]*v[i]);
                }
                assert(sum>=0.0);
                v[k]=sqrt((double)sum);
                /* 閘閙ents non diagonaux */
                x=L[k][k];
                assert(fabs(x)>EPS);
                for (j=k+1; j<N; j++) {
                        w=L[j];
                        sum=A[j][k];
                        for (i=0; i<k; i++) {
                                sum-=(w[i]*v[i]);
                        }
                        w[k]=sum/x;
                }
        }

        for (i=0; i<N; i++) memset(A[i],0,(i+1)*sizeof(float));

        ptime(3,2);
        fprintf(stderr," <sysT>");
        ptime(3,1);


        /* tres long... */
        /* On resoud L.Linv=I */
        /* k=indice de colonne <-> kieme vecteur de base */
        fprintf(stderr,"[1]");
        for (i=0; i<N; i++) {
                PROGRESS(i,N);
                v=L[i];
                w=Linv[i];
                for (k=0; k<=i; k++) {
                        if (i==k) {
                                sum=1.;
                        } else {
                                sum=0.;
                        }
                        for (j=k; j<i; j++) {
                                sum-=v[j]*Linv[j][k];
                        }
                        w[k]=sum/v[i];
                }
        }

        /* On resoud transp(L).A=Linv */
        /* A=transp(Linv)*Linv */

        fprintf(stderr,"[2]");
        for (i=0; i<N; i++) {
                PROGRESS(i,N);
                v=A[i];
                for (k=i; k<N; k++) {
                        x=Linv[k][i];
                        w=Linv[k];
                        for (j=0; j<=i; j++) {
                                v[j]+=x*w[j];
                        }
                }
        }
        fprintf(stderr,"");
        ptime(3,2);

        for (i=0; i<N; i++) {
                free(L[i]);
                free(Linv[i]);
        }
        free(L);
        free(z);
        free(Linv);
        return(0);
}

float cvar(float *x,int n)
{
        int i;
        double M=0.;
        double V=0.;
        for (i=0; i<n; i++) M+=x[i];
        M/=(double)n;
        for (i=0; i<n; i++) V+=(x[i]-M)*(x[i]-M);
        V/=(double)n;
        return((float)V);
}

int main(int argc,char **argv)
{
        struct matrix_t *G;
        float *w,y,*v;
        float **GtG,**inv;
        float *Gtd;
        float **R=NULL;
        float *d=NULL,*d2=NULL,*sd=NULL,*res=NULL;
        float *m=NULL,*sm=NULL;
        float *dampfact=NULL;
        float damp0=-1.;
        int N,M,i,j,k,normdamp=0;
        int WS=0;
        FILE *fmat,*f=NULL;
        char sysname[255],paraname[255],resolname[255],inmname[255],
             logname[255],varname[255],outname[255];
        char *basename=NULL;
        float trR;
        float var,x;
        int VAR=0;
        for (i=1; i<argc; i++) {
                if (argv[i][0]=='-') {
                        switch (argv[i][1]) {
                                case 'v' :
                                        VAR=1;
                                        break;
                                case 's' :
                                        WS=1;
                                        break;
                                case 'n' :
                                        if (argc<=i+1) goto error;