lsq.sav.txt

射线追踪程序

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <unistd.h>
#include "sparse.h"

#define OK fprintf(stderr,"FILE %s, LINE %d\n",__FILE__,__LINE__);
#define EPS 1.0e-9

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

FILE *flog;

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);
                  fprintf(flog,"[%.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");
     fprintf(flog,"[inv_cholesky]: malloc error (z)\n");
     fclose(flog);
     exit(1);
  }
  if ((L=(float**)calloc(N,sizeof(float*)))==NULL) {
     fprintf(stderr,"[inv_cholesky]: malloc error (L)\n");
     fprintf(flog,"[inv_cholesky]: malloc error (L)\n");
     fclose(flog);
     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);
         fprintf(flog,"[inv_cholesky] malloc error (L:%d)\n",i);
         fclose(flog);
         exit(1);
      }
  if ((Linv=(float**)calloc(N,sizeof(float*)))==NULL) {
     fprintf(stderr,"[inv_cholesky]: malloc error (Linv)\n");
     fprintf(flog,"[inv_cholesky]: malloc error (Linv)\n");
     fclose(flog);
     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);
         fprintf(flog,"[inv_cholesky] malloc error (Linv:%d)\n",i);
         fclose(flog);
         exit(1);
      }
  /* A est sym閠rique d閒inie positive,
   * On cherche L triangulaire inf閞ieure telle que A=L*transp(L)
   * */
  fprintf(stderr,"<L>");
  fprintf(flog,"<L>");
  ptime(3,1);
  /* colonne par colonne (k);*/
  for (k=0; k<N; k++) {
      v=L[k];
      /* 閘閙ent diagonal */
      sum=A[k][k];
      for (i=0; i<k; i++) sum-=(v[i]*v[i]);
      if (sum<0.) { fprintf(stderr,"\n[inv_cholesky] sqrt( %f<0. )\n",sum);
                    fprintf(flog,"\n[inv_cholesky] sqrt( %f<0. )\n",sum);
                    fclose(flog);
                    exit(1);
      }
      v[k]=sqrt((double)sum);
      /* 閘閙ents non diagonaux */
      x=L[k][k];
      if (fabs(x)<EPS) {
         fprintf(stderr,"[inv_cholesky] 1/x, x<%.0e\n",EPS);
         fprintf(flog,"[inv_cholesky] 1/x, x<%.0e\n",EPS);
         fclose(flog);
         exit(1);
      }
      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>");
  fprintf(flog," <sysT>");
  ptime(3,1);
  /* tres long... */
  /* On resoud L.Linv=I */
  /* k=indice de colonne <-> kieme vecteur de base */
  for (i=0; i<N; i++) {
      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];
      }
  }

  for (i=0; i<N; i++) free(L[i]);
  free(L);
  free(z);

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

  for (i=0; i<N; i++) {
      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];
      }
  }

  ptime(3,2);
  for (i=0; i<N; i++) free(Linv[i]);
  free(Linv);
  return(0);
}

float inverse(struct matrix_t *G, float **Ginv, float **R, float *cov,
              float* D, int N, int M)
{ register int i,j,k;
  float *v,*w,x,y;
  float **GtG,**inv;
  ptime(1,1);
  GtG=(float**)calloc(M,sizeof(float*));
  inv=(float**)calloc(M,sizeof(float*));
  for (i=0; i<M; i++) {
      if ((inv[i]=(float*)calloc(i+1,sizeof(float)))==NULL) {
         fprintf(stderr,"malloc error (2:%d)\n",i);
         fprintf(flog,"malloc error (2:%d)\n",i);
         fclose(flog);
         exit(1);
      }
      if ((GtG[i]=(float*)calloc(i+1,sizeof(float)))==NULL) {
         fprintf(stderr,"malloc error (2:%d)\n",i);
         fprintf(flog,"malloc error (2:%d)\n",i);
         fclose(flog);
         exit(1);
      }
  }
  for (i=0; i<M; i++) {
      for (j=0; j<M; j++) R[i][j]=0.;
      for (j=0; j<i+1; j++) inv[i][j]=GtG[i][j]=0.0;
      for (j=0; j<N; j++)   Ginv[i][j]=0.0;
  }

  fprintf(stderr,"\n   GtG... ");
  fprintf(flog,"\n   GtG... ");
  ptime(0,1);

  for (k=0; k<N; k++) {
      for (i=0; i<M; i++) {
          w=GtG[i];
          if (sparsegetval(G,k,i,&x))
             for (j=0; j<=i; j++)
                 if (sparsegetval(G,k,j,&y))
                    w[j]+=x*y;
      }
  }

  ptime(0,2);

  for (i=0; i<M; i++) memcpy(inv[i],GtG[i],(i+1)*sizeof(float));

  fprintf(stderr,"\n   inv(GtG+D*Id)... ");
  fprintf(flog,"\n   inv(GtG+D*Id)... ");
  for (i=0; i<M; i++) inv[i][i]+=D[i];
  /*dumpdiag("sys.bin",inv,M);*/

  fprintf(stderr,"inv_cholesky... ");
  fprintf(flog,"inv_cholesky... ");

  ptime(0,1);
  inv_cholesky(inv,M);
  ptime(0,2);

  fprintf(stderr,"\n   (inv(GtG)+D*Id)*Gt... ");
  fprintf(flog,"\n   (inv(GtG)+D*Id)*Gt... ");
  ptime(0,1);
  for (k=0; k<M; k++) {
      v=inv[k];
      for (j=0; j<N; j++) {
          if (sparsegetval(G,j,k,&x)) {
             for (i=0; i<k; i++) Ginv[i][j]+=x*v[i];
             for (i=k; i<M; i++) Ginv[i][j]+=x*inv[i][k];
          }
      }
  }
  ptime(0,2);

  ptime(0,1);
  fprintf(stderr,"\n   (inv(GtG)+D*Id)*GtG... ");
  fprintf(flog,"\n   (inv(GtG)+D*Id)*GtG... ");
  for (k=0; k<M; k++) {
      v=inv[k];
      for (j=0; j<M; j++) {
          x=((j<k)?GtG[k][j]:GtG[j][k]);
          if (x>EPS) for (i=0; i<M; i++) R[i][j]+=((k<i)?inv[i][k]:v[i])*x;
      }
  }
/*
  for (k=0; k<M; k++) {
      v=GtG[k];
      for (i=0; i<M; i++) {
          x=(k<i)?inv[i][k]:inv[k][i];
          w=R[i];
          if (x>EPS) for (j=0; j<M; j++) w[j]+=x*((j<k)?v[j]:GtG[j][k]);
      }
  }
*/
  ptime(0,2);

  if (cov) {
     fprintf(stderr,"\n   model diag cov... ");
     fprintf(flog,"\n   model diag cov... ");
     ptime(0,1);
     for (i=0; i<M; i++) cov[i]=0.;
     for (i=0; i<M; i++)
         for (k=0; k<M; k++)
             cov[i]+=((k<i)?inv[i][k]:inv[k][i])*R[i][k];
     ptime(0,2);
  }
  
  fprintf(stderr,"\nDone ");
  fprintf(flog,"\nDone ");
  ptime(1,2);
  for (i=0; i<M; i++) free(inv[i]);
  free(inv);
  for (i=0; i<M; i++) free(GtG[i]);
  free(GtG);
  return(0);
}