source: Sophya/trunk/SophyaLib/NTools/matxop.c@ 975

#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>


#include "matxop.h"
#include "nbmath.h"

/*  Fonctions de manipulation de matrices et de vecteurs    */
/*  Resolution de systemes lineaires                        */

/*                              R. Ansari  Juillet 1993     */
/*                              LaSilla (Chili)             */


#define MXXFLOAT        1.e36



/* Nouvelle-Fonction */

int IVecxVec(int *v1, int *v2, int n)

/*  Produit scalaire de deux vecteurs entiers       */
/*  Retour (int) = v1(n) . v2(n)                    */

{
register int i,rc;
register int *ip1, *ip2;

ip1 = v1;  ip2 = v2;
rc = 0;
/* for(i=0; i<n; i++)  rc += *(ip1+i) * *(ip2+i); */
for(i=0; i<n; i++)  rc += *ip1++ * *ip2++;
return(rc);
}


/* Nouvelle-Fonction */

float RVecxVec(float *v1, float *v2, int n)

/*  Produit scalaire de deux vecteurs reels           */
/*  Retour (float) = v1(n) . v2(n)                    */

{
register int i;
register float rc;
register float *fp1, *fp2;

fp1 = v1;  fp2 = v2;
rc = 0.0;
/* for(i=0; i<n; i++)  rc += *(fp1+i) * *(fp2+i); */
for(i=0; i<n; i++)  rc += *fp1++ * *fp2++;
return(rc);
}


/* Nouvelle-Fonction */

double DVecxVec(double *v1, double *v2, int n)

/*  Produit scalaire de deux vecteurs double          */
/*  Retour (float) = v1(n) . v2(n)                    */

{
register int i;
register double rc;
register double *fp1, *fp2;

fp1 = v1;  fp2 = v2;
rc = 0.0;
/* for(i=0; i<n; i++)  rc += *(fp1+i) * *(fp2+i); */
for(i=0; i<n; i++)  rc += *fp1++ * *fp2++;
return(rc);
}


/* Nouvelle-Fonction */

void  IMatxVec(int *mx, int *vi, int *vo, int n)

/*  Matrice * Vecteur  (Entiers)     Integer           */
/*  Vecteur Vo(n) = Matrice Mx(n,n) . Vi(n)            */

{
register  int *vp, *mxp;
register int s;
register int j,i;

vp = vi;   mxp = mx;

for (i=0; i<n; i++)
  { s = 0;  
  for(j=0; j<n; j++)   s += *(mxp+j) * *(vp+j);
  *(vo+i) = s;  mxp += n; }

return;
}


/* Nouvelle-Fonction */

void  RMatxVec(float *mx, float *vi, float *vo, int n)

/*  Matrice * Vecteur  (Reels)     float               */
/*  Vecteur Vo(n) = Matrice Mx(n,n) . Vi(n)            */


/*  Matrice * Vecteur  (Reels)  */
{
register  float *vp, *mxp;
register float s;
register int j,i;

vp = vi;   mxp = mx;

for (i=0; i<n; i++)
  { s = 0;  
  for(j=0; j<n; j++)   s += *(mxp+j) * *(vp+j);
  *(vo+i) = s;  mxp += n; }

return;
}


/* Nouvelle-Fonction */

void  DMatxVec(double *mx, double *vi, double *vo, int n)

/*  Matrice * Vecteur  (double)     double             */
/*  Vecteur Vo(n) = Matrice Mx(n,n) . Vi(n)            */


/*  Matrice * Vecteur  (Reels)  */
{
register  double *vp, *mxp;
register double s;
register int j,i;

vp = vi;   mxp = mx;

for (i=0; i<n; i++)
  { s = 0;  
  for(j=0; j<n; j++)   s += *(mxp+j) * *(vp+j);
  *(vo+i) = s;  mxp += n; }

return;
}




/* Nouvelle-Fonction */

float SolveRLinSyst(float *mx, float *b, float *x, int n)

/*  Resolution de systeme lineaire                           */
/*  Matrice Mx(n,n) * Vecteur X(n) = Vecteur B(n)            */
/*  Inconnu : vecteur X                                      */
/*  Retour = Determinant du systeme (=0 Pb)  et X[]          */
/*  Note : Au retour le vecteur B(n) et la matrice Mx(n,n)   */
/*  sont modifies. Mx(n,n) contient une matrice triangulaire */
/*  superieure                                               */                          

{
int i,k;
register int j;
register float *fp1, *fp2;
double det;
float vt1;
register float vt2;
register float six;
int pivok;


#define  MINPIVOT   1.e-15
#define  MINDETER   1.e-15



for (i=0; i<n-1; i++)   /*  Boucle sur les pivots */
  {

/*  printf("\n Iteration %d : \n",i);
  for (k=0; k<n; k++)
    {
    for (j=0; j<n; j++)  printf(" %10g ",*(mx+k*n+j));
    printf("    b= %10g \n",*(b+k));
    }  */

  fp1 = fp2 = mx+i*n;  
  vt1 = *(fp1+i) ;

  if ((vt1 < MINPIVOT) && (vt1 > -MINPIVOT) )   /* Pivot trop petit  */
    {
    pivok = 0;
    for (k=i+1; k<n; k++)
      {
      vt1 = *(mx+k*n+i) ;
      if ((vt1 > MINPIVOT) || (vt1 < -MINPIVOT) )
        {
        fp2 = mx+k*n;  
        for (j=i; j<n; j++)
          {
          *(fp1+j) += *(fp2+j);
          }
        *(b+i) += *(b+k);
        vt1 = *(fp1+i) ;  
        fp2 = fp1;
        pivok = 1;
        break;
        }
      }
    if (!pivok)  return(0.0);
    }    

  for (k=i+1; k<n; k++)
    {
    fp1 += n;
    vt2 = *(fp1+i) / vt1 ; 
    for (j=i+1; j<n; j++)  
      *(fp1+j) -= (vt2 * *(fp2+j));

    *(fp1+i) = 0.0;  
    *(b+k) -= (vt2 * *(b+i)) ;
    }

  }

/*   Calcul du determinant  */
det = 1.0;
for (i=0; i<n; i++)  det *= *(mx+i*n+i);
if ((det < MINDETER) && (det > -MINDETER) )  return(0.0);
if (det > MXXFLOAT)  det = MXXFLOAT;
if (det < -MXXFLOAT)  det = -MXXFLOAT;


for(i=n-1; i>=0; i--)
  {
  fp1 = mx+i*n;
  six = *(b+i);
  for (j=i+1; j<n; j++)  six -= (x[j] * *(fp1+j) );
  x[i] = six / (*(fp1+i));

/*  printf("  Solution X[%2d] = %g \n",i,x[i]);  */
  }

return(det);
}





/* Nouvelle-Fonction */

double SolveDLinSyst(double *mx, double *b, double *x, int n)

/*  Resolution de systeme lineaire                           */
/*  Matrice Mx(n,n) * Vecteur X(n) = Vecteur B(n)            */
/*  Inconnu : vecteur X                                      */
/*  Retour = Determinant du systeme (=0 Pb)  et X[]          */
/*  Note : Au retour le vecteur B(n) et la matrice Mx(n,n)   */
/*  sont modifies. Mx(n,n) contient une matrice triangulaire */
/*  superieure                                               */                          

{
int i,k;
register int j;
register double *fp1, *fp2;
double det;
double vt1;
register double vt2;
register double six;
int pivok;


#define  DMINPIVOT   1.e-25
#define  DMINDETER   1.e-30



for (i=0; i<n-1; i++)   /*  Boucle sur les pivots */
  {

/*  printf("\n Iteration %d : \n",i);
  for (k=0; k<n; k++)
    {
    for (j=0; j<n; j++)  printf(" %10g ",*(mx+k*n+j));
    printf("    b= %10g \n",*(b+k));
    }  */

  fp1 = fp2 = mx+i*n;  
  vt1 = *(fp1+i) ;

  if ((vt1 < DMINPIVOT) && (vt1 > -DMINPIVOT) )   /* Pivot trop petit  */
    {
    pivok = 0;
    for (k=i+1; k<n; k++)
      {
      vt1 = *(mx+k*n+i) ;
      if ((vt1 > DMINPIVOT) || (vt1 < -DMINPIVOT) )
        {
        fp2 = mx+k*n;  
        for (j=i; j<n; j++)
          {
          *(fp1+j) += *(fp2+j);
          }
        *(b+i) += *(b+k);
        vt1 = *(fp1+i) ;  
        fp2 = fp1;
        pivok = 1;
        break;
        }
      }
    if (!pivok)  return(0.0);
    }    

  for (k=i+1; k<n; k++)
    {
    fp1 += n;
    vt2 = *(fp1+i) / vt1 ; 
    for (j=i+1; j<n; j++)  
      *(fp1+j) -= (vt2 * *(fp2+j));

    *(fp1+i) = 0.0;  
    *(b+k) -= (vt2 * *(b+i)) ;
    }

  }

/*   Calcul du determinant  */
det = 1.0;
for (i=0; i<n; i++)  det *= *(mx+i*n+i);
if ((det < DMINDETER) && (det > -DMINDETER) )  return(0.0);
if (det > MXXFLOAT)  det = MXXFLOAT;
if (det < -MXXFLOAT)  det = -MXXFLOAT;


for(i=n-1; i>=0; i--)
  {
  fp1 = mx+i*n;
  six = *(b+i);
  for (j=i+1; j<n; j++)  six -= (x[j] * *(fp1+j) );
  x[i] = six / (*(fp1+i));

/*  printf("  Solution X[%2d] = %g \n",i,x[i]);  */
  }

return(det);
}




/*    ==============================================================  */
/*   Fonctions de Fit lineaire de Xi2                                 */
/*    ==============================================================  */

/*   .......... Fit avec vecteurs depart reels ...............      */

static int FFBusy = -1;                /*  Flag Deja Busy (GetFitVect effectue)  */
static int FNVarMax = 0;               /*  Nb Maxi de variables a ajuster        */
static int FVLenMax = 0;               /*  Longueur maxi des vecteurs            */
static float **FVeci = NULL;           /*  Vecteurs en entree  *Vecf = Y         */
                                       /*                      *(Vecf+i) = X[i]  */
static float **FVecf = NULL;           /*  Vecteurs *FVecf = *FVecf+2 = B = M.X  */                                           
                                       /*           *(FVecf+1) = La sortie = Ai  */
static float *FVSpace = NULL;          /*  Espace pour les *FVeci                */  
static float *FVSpacef = NULL;         /*    "   "    "    *FVecf                */
static double  *FFMtx = NULL;          /*  Matrice a inverser = M                */
static double  *FFMtx2 = NULL;         /*   Copie de M                           */
static double  *FFSort = NULL;         /*  RFitLinErr() pour Sortie GausPiv()    */
                                       /*  FNVarMax Lignes * (FNVarMax+1) Col.   */
                                       /*  Col1= Solution, Col2..N+1 : MatInv    */        


int InitRFitLin(int nvarmx, int szv)

/*  Initialisation du fit de Xi2 Entier             */
/*  Allocation d'espace memoire                     */
/*  nvarmx = Nb maxi de variable                    */
/*  szv = Taille maxi des vecteurs de points        */

{
int i;

FFBusy = -1;
nvarmx ++;
if (nvarmx < 7)  nvarmx = 7;
if (szv < 25) szv = 25;

if ( (FVeci = (float**) malloc(nvarmx*sizeof(float *))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(Veci)) \n");
  return(1);
  }

if ( (FVSpace = (float*)  malloc(nvarmx*szv*sizeof(float))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(VSpace)) \n");
  free(FVeci);
  return(2);
  }

for(i=0; i<nvarmx; i++)  *(FVeci+i) = FVSpace+i*szv;


if ( (FVecf = (float**) malloc(3*sizeof(float *))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(Vecf)) \n");
  free(FVeci);
  free(FVSpace);
  return(3);
  }

if ( (FVSpacef = (float*) malloc(3*szv*sizeof(float))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(VSpacef)) \n");
  free(FVeci);
  free(FVSpace);
  free(FVecf);
  return(4);
  }

for(i=0; i<3; i++)  *(FVecf+i) = FVSpacef+i*szv;


if ( (FFMtx = (double*) malloc(nvarmx*nvarmx*sizeof(double))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(FMtx)) \n");
  free(FVeci);
  free(FVSpace);
  free(FVecf);
  free(FVSpacef);
  return(5);
  }

if ( (FFMtx2 = (double*) malloc(nvarmx*nvarmx*sizeof(double))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(FMtx2)) \n");
  free(FVeci);
  free(FVSpace);
  free(FVecf);
  free(FVSpacef);
  free(FFMtx);
  return(6);
  }

if ( (FFSort = (double*) malloc((nvarmx+1)*nvarmx*sizeof(double))) == NULL )
  { printf("InitRFitLin_Erreur: (Pb malloc(FFSort)) \n");
  free(FVeci);
  free(FVSpace);
  free(FVecf);
  free(FVSpacef);
  free(FFMtx);
  free(FFMtx2);
  return(7);
  }

FFBusy = 0;
FNVarMax = nvarmx-1;
FVLenMax = szv;

return(0);
}


/* Nouvelle-Fonction */
void EndRFitLin()
{

if (FVeci != NULL)  free(FVeci);
if (FVSpace != NULL)  free(FVSpace);
if (FVecf != NULL)  free(FVecf);
if (FVSpacef != NULL)  free(FVSpacef);
if (FFMtx != NULL)  free(FFMtx);
if (FFMtx2 != NULL)  free(FFMtx2);
if (FFSort != NULL)  free(FFSort);

FVeci = NULL;
FVSpace = NULL;
FVecf = NULL;
FVSpacef = NULL;
FFMtx = NULL;
FFMtx2 = NULL;

FFBusy = -1;
FNVarMax = 0;
FVLenMax = 0;
return;
}


/* Nouvelle-Fonction */

float **GetRFitVect(int nvar, int vsz)
/*  reservation de l'espace des vecteurs pour le fit  */
{
int nvmx, vszmx;

if (FFBusy > 0)  
  { printf("GetRFitVect_Erreur:  FFBusy= %d \n",FFBusy);
  return(NULL);  }
if ((nvar < 1) || (vsz < 1) )  
  { printf("GetRFitVect_Erreur:  NVar,VSz= %d %d \n",nvar,vsz);
  return(NULL);  }
if ((nvar > FNVarMax) || (vsz > FVLenMax) || (FFBusy < 0))
  {
  EndRFitLin();
  if (nvar > FNVarMax)   nvmx = nvar;
  else  nvmx = FNVarMax;
  if (vsz > FVLenMax)  vszmx = vsz;
  else vszmx = FVLenMax;
  if (InitRFitLin(nvmx, vszmx) != 0)  return(NULL);
  }

FFBusy = 1;
return(FVeci);
}


/* Nouvelle-Fonction */
void FreeRFitVect()
{
FFBusy = 0;
return;
}

/* Nouvelle-Fonction */

float RFitLin(int nd, int vsz, float *XVar)

/*  Effectue le fit de Xi2 pour nd inconnu, Taille vecteurs vsz */
/*  Les vecteurs doivent etre remplis auparavant                */
/*  Retourne la valeur du Xi2 du fit, Negatif si Pb.            */

{
int i,j;
register float *ip;
register float *fp, *fp2;
register double x;
register float y;
register float *ffmtx, *ffmtx2;
float det, rc;


if ((nd > FNVarMax) || (vsz > FVLenMax) )   {rc = -1000.0;  goto Fin; }

/*  Fabrication matrice du syteme lineaire a resoudre  */
ffmtx = (float *)FFMtx;
ffmtx2 = (float *)FFMtx2;
for(i=0; i<nd; i++)
  {
  fp = ffmtx+i*nd;   fp2 = ffmtx2+i*nd;
  ip = *(FVeci+i+1);

/*  Elements diagonals  */
  *(fp2+i) = *(fp+i) = RVecxVec( ip, ip, vsz) ;  
 
  for(j=i+1; j<nd; j++)  
    {                /* Les autres elements  */
    *(fp2+j) = *(fp+j) = RVecxVec( ip, *(FVeci+j+1), vsz) ;
  /* Matrice symetrique */ 
    *(ffmtx2+j*nd+i) = *(ffmtx+j*nd+i) = *(fp+j);  
    }

/*  Second membre  M.X = B   */
  fp = *FVecf;  fp2 = *(FVecf+2);
  *(fp2+i) = *(fp+i) = RVecxVec( ip, *FVeci, vsz) ;
  
  }

/*  On resoud le systeme lineaire : */
det = SolveRLinSyst(ffmtx, *FVecf, *(FVecf+1), nd);
if (det == 0.0)   {rc = -500.0;  goto Fin; }

/*  On calcule le Xi2  */
x = RVecxVec( *FVeci, *FVeci, vsz);
fp = *(FVecf+2);  
for(i=0; i<nd; i++)
  {
  XVar[i] = y = *(*(FVecf+1)+i);  
  fp2 = ffmtx2+i*nd+i;   
  x += (y * y * *fp2);
  x -= ( 2.0 * ( y * *(fp+i) ) );
  for (j=i+1; j<nd; j++)  
    { fp2++;  
    x += 2.0 * y * *(*(FVecf+1)+j) * *fp2 ;  }    
  }

rc = (float)x;

Fin:
FreeRFitVect();
return(rc);
}



/* Nouvelle-Fonction */

float RFitLinErr(int nd, int vsz, float *XVar, float *Err)

/*  Effectue le fit de Xi2 pour nd inconnu, Taille vecteurs vsz */
/*  Les vecteurs doivent etre remplis auparavant                */
/*  Retourne la valeur du Xi2 du fit, Negatif si Pb.            */

{
int i,j,md;
register float *ip;
register double *dp, *dp2;
register float *fp;
register double x,y;
float rc;
double det;


if ((nd > FNVarMax) || (vsz > FVLenMax) )   {rc = -1000.0;  goto Fin; }

md = nd+1;  
/*  Mise a zero de la matrice de sortie et creation matrice identite */
for(i=0; i<nd*md; i++)  FFSort[i] = 0.;
for(i=1; i<nd*md; i += md+1) FFSort[i] = 1.;  

/*  Fabrication matrice du syteme lineaire a resoudre  */

for(i=0; i<nd; i++)
  {
  dp = FFMtx+i*nd;   dp2 = FFMtx2+i*nd;
  ip = *(FVeci+i+1);

/*  Elements diagonals  */
  *(dp+i) = *(dp2+i) = RVecxVec( ip, ip, vsz) ;  
 
  for(j=i+1; j<nd; j++)  
    {                /* Les autres elements  */
    *(dp+j) = *(dp2+j) = RVecxVec( ip, *(FVeci+j+1), vsz) ;
  /* Matrice symetrique */ 
    *(FFMtx+j*nd+i) = *(FFMtx2+j*nd+i) = *(dp+j);  
    }

/*  Second membre  M.X = B   */
  dp = FFSort+i*md;  fp = *(FVecf+2);  
  *dp = *(fp+i) = RVecxVec( ip, *FVeci, vsz) ;
  
  }


/*  Appel GausPiv pour resoudre le systeme et inverser la matrice */
det = GausPiv(FFMtx, nd, nd, FFSort, md, md, 0);
/* printf("Det= %g Resul= %g %g %g \n",det,FFSort[0], FFSort[md], FFSort[2*md]); */
if (det == 0.0)  {rc = -500.0;  goto Fin; } 
 
/*  On calcule le Xi2   Et remplissage vecteurs sortie */

x = RVecxVec( *FVeci, *FVeci, vsz);
fp = *(FVecf+2);  
for(i=0; i<nd; i++)
  {
  y = FFSort[1+i*(md+1)]; 
  Err[i] = (y >= 0.) ? (float)sqrt(y) : -(float)sqrt(-y);
  XVar[i]= y =FFSort[i*md];
 
  dp2 = FFMtx2+i*nd+i;  
  x += (y * y * *dp2);
  x -= ( 2.0 * ( y * (double)*(fp+i) ) );
  for (j=i+1; j<nd; j++)  
    { dp2++;  
    x += 2.0 *  y * FFSort[j*md] * *dp2 ;  }    
  }
rc = (float)x;

Fin:
FreeRFitVect();
return(rc);
}




/*   .......... Fit avec vecteurs depart double ...............      */

/*   Voir description des variables ds IniRFitLin()   */
static int DFBusy = -1;
static int DNVarMax = 0;
static int DVLenMax = 0;
static double **DVeci = NULL;
static double **DVecf = NULL;
static double *DVSpace = NULL;
static double *DVSpacef = NULL;
static double  *DFMtx = NULL;
static double  *DFMtx2 = NULL;


int InitDFitLin(int nvarmx, int szv)

/*  Initialisation du fit de Xi2 Entier             */
/*  Allocation d'espace memoire                     */
/*  nvarmx = Nb maxi de variable                    */
/*  szv = Taille maxi des vecteurs de points        */

{
int i;

DFBusy = -1;
nvarmx ++;
if (nvarmx < 7)  nvarmx = 7;
if (szv < 25) szv = 25;

if ( (DVeci = (double**) malloc(nvarmx*sizeof(double *))) == NULL )
  { printf("InitDFitLin_Erreur: (Pb malloc(Veci)) \n");
  return(1);
  }

if ( (DVSpace = (double*) malloc(nvarmx*szv*sizeof(double))) == NULL )
  { printf("InitDFitLin_Erreur: (Pb malloc(VSpace)) \n");
  free(DVeci);
  return(2);
  }

for(i=0; i<nvarmx; i++)  *(DVeci+i) = DVSpace+i*szv;


if ( (DVecf = (double**) malloc(3*sizeof(double *))) == NULL )
  { printf("InitDFitLin_Erreur: (Pb malloc(Vecf)) \n");
  free(DVeci);
  free(DVSpace);
  return(3);
  }

if ( (DVSpacef = (double*) malloc(3*szv*sizeof(double))) == NULL )
  { printf("InitDFitLin_Erreur: (Pb malloc(VSpacef)) \n");
  free(DVeci);
  free(DVSpace);
  free(DVecf);
  return(4);
  }

for(i=0; i<3; i++)  *(DVecf+i) = DVSpacef+i*szv;


if ( (DFMtx = (double*) malloc(nvarmx*nvarmx*sizeof(double))) == NULL )
  { printf("InitDFitLin_Erreur: (Pb malloc(FMtx)) \n");
  free(DVeci);
  free(DVSpace);
  free(DVecf);
  free(DVSpacef);
  return(5);
  }

if ( (DFMtx2 = (double*) malloc(nvarmx*nvarmx*sizeof(double))) == NULL )
  { printf("InitDFitLin_Erreur: (Pb malloc(FMtx2)) \n");
  free(DVeci);
  free(DVSpace);
  free(DVecf);
  free(DVSpacef);
  free(DFMtx);
  return(6);
  }

DFBusy = 0;
DNVarMax = nvarmx-1;
DVLenMax = szv;

return(0);
}


/* Nouvelle-Fonction */
void EndDFitLin()
{

if (DVeci != NULL)  free(DVeci);
if (DVSpace != NULL)  free(DVSpace);
if (DVecf != NULL)  free(DVecf);
if (DVSpacef != NULL)  free(DVSpacef);
if (DFMtx != NULL)  free(DFMtx);
if (DFMtx2 != NULL)  free(DFMtx2);

DVeci = NULL;
DVSpace = NULL;
DVecf = NULL;
DVSpacef = NULL;
DFMtx = DFMtx2 = NULL;

DFBusy = -1;
DNVarMax = 0;
DVLenMax = 0;
return;
}


/* Nouvelle-Fonction */

double **GetDFitVect(int nvar, int vsz)
/*  reservation de l'espace des vecteurs pour le fit  */
{
int nvmx, vszmx;

if (DFBusy > 0)  return(NULL);
if ( (nvar < 1) || (vsz < 1) ) return(NULL);
if ((nvar > DNVarMax) || (vsz > DVLenMax) || (DFBusy < 0))
  {
  EndDFitLin();
  if (nvar > DNVarMax)   nvmx = nvar;
  else  nvmx = DNVarMax;
  if (vsz > DVLenMax)  vszmx = vsz;
  else vszmx = DVLenMax;
  InitDFitLin(nvmx, vszmx);
  }

DFBusy = 1;
return(DVeci);
}


/* Nouvelle-Fonction */
void FreeDFitVect()
{
DFBusy = 0;
return;
}


/* Nouvelle-Fonction */

double DFitLin(int nd, int vsz, double *XVar)

/*  Effectue le fit de Xi2 pour nd inconnu, Taille vecteurs vsz */
/*  Les vecteurs doivent etre remplis auparavant                */
/*  Retourne la valeur du Xi2 du fit, Negatif si Pb.            */

{
int i,j;
register double *ip;
register double *fp, *fp2;
register double x,y;
double det,rc;


if ((nd > DNVarMax) || (vsz > DVLenMax) )   { rc = -1000.;  goto Fin; }

/*   Impression de debug  */
/* 
for(i=0; i<nd+1; i++)
  {
  ip = *(DVeci+i);
  printf("DFitLin_Debug Veci[%d][0..5]= %lg %lg %lg %lg %lg %lg \n",
       i,*ip, *(ip+1), *(ip+2), *(ip+3), *(ip+4), *(ip+5));  
  } 
*/


/*  Fabrication matrice du syteme lineaire a resoudre  */

for(i=0; i<nd; i++)
  {
  fp = DFMtx+i*nd;   fp2 = DFMtx2+i*nd;
  ip = *(DVeci+i+1);

/*  Elements diagonals  */
  *(fp2+i) = *(fp+i) =  DVecxVec( ip, ip, vsz) ;  
 
  for(j=i+1; j<nd; j++)  
    {                /* Les autres elements  */
    *(fp2+j) = *(fp+j) = DVecxVec( ip, *(DVeci+j+1), vsz) ;
  /* Matrice symetrique */ 
    *(DFMtx2+j*nd+i) = *(DFMtx+j*nd+i) = *(fp+j);  
    }

/*  Second membre  M.X = B   */
  fp = *DVecf;  fp2 = *(DVecf+2);
  *(fp2+i) = *(fp+i) = DVecxVec( ip, *DVeci, vsz) ;
  
  }

/* 
printf("DFitLin_Debug B[0..5]= %lg %lg %lg %lg %lg %lg \n",
       *fp2, *(fp2+1), *(fp2+2), *(fp2+3), *(fp2+4), *(fp2+5));
*/

/*  On resoud le systeme lineaire : */
fp = *(DVecf+1);
det = SolveDLinSyst(DFMtx, *DVecf, fp, nd);
if (det == 0.0)   { rc = -500.;  goto Fin; }

/*  On calcule le Xi2  */
x = DVecxVec( *DVeci, *DVeci, vsz);
for(i=0; i<nd; i++)   /* Et on remplit le vecteur de sortie */
  {
  fp2 = DFMtx2+i*nd+i;   y = XVar[i] = *(*(DVecf+1)+i);  
  x += (y * y * *fp2);
  x -= ( 2.0 * ( y * *(*(DVecf+2)+i) ) );
  for (j=i+1; j<nd; j++)  
    { fp2++;  
    x += ( 2.0 * ( y * *(*(DVecf+1)+j) * *fp2 ) );  }    
  }
rc = x;


Fin:
FreeDFitVect();
return(rc);
}