source: Sophya/trunk/SophyaLib/NTools/cspline.cc@ 3440

#include "sopnamsp.h"
#include "machdefs.h"

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


#include "nbtri.h"
#include "cspline.h"

/*!
  \class SOPHYA::CSpline
  \ingroup NTools
  Spline 3 smoother (interpolator) for 1-D data points (y = f(x)) 
  \sa CSpline2
*/

/*!
  Createur pour spline 3 sur "x[0->n],y[0->n]" avec "yp1,ypn" derivees
  au premier et dernier points et "natural" indiquant les types de
  contraintes sur les derivees 2sd au premier et dernier point.
  "order" doit etre mis a "true" si le tableau de "x[]" n'est pas ordonne
  dans l'ordre des "x" croissants ("x[i]<x[i+1]"): cette option
  realloue la place pour les tableaux "x,y" autrement seule une
  connection aux tableaux "x,y" externes est realisee.
*/
CSpline::CSpline(int n,double* x,double* y,double yp1,double ypn
                ,int natural,bool order)
  : Nel(0), corrupt_Y2(true), XY_Created(false), Natural(natural)
  , YP1(yp1), YPn(ypn), X(NULL), Y(NULL), Y2(NULL), tmp(NULL)
{
SetNewTab(n,x,y,order,true);
if( x != NULL && y != NULL) ComputeCSpline();

}

//!     Createur par defaut.
CSpline::CSpline(double yp1,double ypn,int natural)
  : Nel(0), corrupt_Y2(true), XY_Created(false), Natural(natural)
  , YP1(yp1), YPn(ypn), X(NULL), Y(NULL), Y2(NULL), tmp(NULL)
{
}

//////////////////////////////////////////////////////////////////////////////
//! destructeur
CSpline::~CSpline()
{
DelTab();
}

/*!
  Pour changer les tableaux sans recreer la classe,
  memes arguments que dans le createur.
  Pour connecter les tableaux "x[n],y[n]" aux pointeurs internes "X,Y"
  Si "order=true", on considere que x n'est pas range par ordre
  des "x" croissants. La methode alloue de la place pour des tableaux
  internes "X,Y" qu'elle re-ordonne par "x" croissant.
  "force=true" impose la reallocation des divers buffers, sinon
  la reallocation n'a lieu que si le nombre de points augmente.
*/
void CSpline::SetNewTab(int n,double* x,double* y,bool order,bool force)
{
ASSERT( n>3 );

// allocation des buffers Y2 et tmp
if( n>Nel || force ) {
  if( Y2  != NULL ) delete [] Y2;
  if( tmp != NULL ) delete [] tmp;
  Y2   = new double[n];
  tmp  = new double[n];
}
// des-allocation eventuelle de X,Y
if( XY_Created ) {
  if( !order || n>Nel || force ) {
    if( X != NULL ) delete [] X;  X = NULL;
    if( Y != NULL ) delete [] Y;  Y = NULL;
    XY_Created = false;
  }
}
// allocation eventuelle de X,Y
if( order ) {
  if( !XY_Created || n>Nel || force ) {
    X = new double[n];
    Y = new double[n];
    XY_Created = true;
  }
}
Nel = n;
corrupt_Y2 = true;

if( x==NULL || y==NULL ) return;

// Classement eventuel par ordre des x croissants
if( order ) {
  if( tmp == NULL ) tmp = new double[n];
  ind = (int_4 *) tmp;
  tri_double(x,ind,(int_4) Nel);
  for(int i=0;i<Nel;i++) {
    X[i] = x[ind[i]];
    Y[i] = y[ind[i]];
    if( i>0 ) if( X[i-1]>= X[i] ) {
      printf("CSpline::SetNewTab_Erreur: X[%d]>=X[%d] (%g>=%g)\n"
            ,i-1,i,X[i-1],X[i]);
      throw ParmError(PExcLongMessage(""));
    }
  }
} else { X = x; Y = y; }

}

//////////////////////////////////////////////////////////////////////////////
//! destruction des divers tableaux en tenant compte des allocations/connections
void CSpline::DelTab()
{
if( X   != NULL && XY_Created ) delete [] X;    X   = NULL;
if( Y   != NULL && XY_Created ) delete [] Y;    Y   = NULL;
if( Y2  != NULL ) delete [] Y2;   Y2  = NULL;
if( tmp != NULL ) delete [] tmp;  tmp = NULL;
}

//////////////////////////////////////////////////////////////////////////////
/*!
  Pour changer les valeurs des derivees 1ere au 1er et dernier points
  Valeurs imposees des derivees 1ere au points "X[0]" et "X[Nel-1]".
*/
void CSpline::SetBound1er(double yp1,double ypn)
{
if( yp1 == YP1 && ypn == YPn ) return;

YP1 = yp1;
YPn = ypn;

corrupt_Y2 = true;
}

//! Pour calculer les tableaux de coeff permettant le calcul des interpolations spline.
void CSpline::ComputeCSpline()
{
// on ne fait rien si les tableaux ne sont pas connectes
if( X == NULL || Y == NULL ) {
  printf("CSpline::ComputeCSpline()_Erreur: tableaux non connectes X=%p Y=%p\n"
        ,X,Y);
  return;
}
// On ne fait rien si rien n'a change!
if( ! corrupt_Y2 ) return;
// protection si tmp a ete desalloue pour gain de place (ex: CSpline2)
if( tmp == NULL ) tmp = new double[Nel];

double p,qn,sig,un;

if (Natural & Natural1)
  Y2[0] = tmp[0] = 0.0;
else {
  Y2[0] = -0.5;
  tmp[0] = (3.0/(X[1]-X[0]))*((Y[1]-Y[0])/(X[1]-X[0])-YP1);
}

for (int i=1;i<Nel-1;i++) {
  sig = (X[i]-X[i-1])/(X[i+1]-X[i-1]);
  p = sig * Y2[i-1] + 2.0;
  Y2[i] = (sig-1.0)/p;
  tmp[i]= (Y[i+1]-Y[i])/(X[i+1]-X[i]) - (Y[i]-Y[i-1])/(X[i]-X[i-1]);
  tmp[i]= (6.0*tmp[i]/(X[i+1]-X[i-1])-sig*tmp[i-1])/p;
}

if (Natural & NaturalN)
   qn = un = 0.0;
else {
   qn = 0.5;
   un = (3.0/(X[Nel-1]-X[Nel-2]))
       *(YPn-(Y[Nel-1]-Y[Nel-2])/(X[Nel-1]-X[Nel-2]));
}
Y2[Nel-1] = (un-qn*tmp[Nel-2])/(qn*Y2[Nel-2]+1.0);
for (int k=Nel-2;k>=0;k--) Y2[k] = Y2[k]*Y2[k+1] + tmp[k];

corrupt_Y2 = false;
}

//!     Interpolation spline en \b x
double CSpline::CSplineInt(double x)
{
int klo,khi,k;
double h,b,a,y = 0.;

if( corrupt_Y2 ) {
  cout<<"CSpline::CSplineInt: calcul des coef du spline corrupted"<<endl;
  throw ParmError(PExcLongMessage(""));
}

klo = 0;
khi = Nel-1;
while (khi-klo > 1) {
  k = (khi+klo) >> 1;
  if (X[k] > x) khi=k;
    else klo=k;
}
h=X[khi]-X[klo];

if (h == 0.0) {
  cout<<"CSpline::CSplineInt: pout khi="<<khi<<" klo="<<klo
      <<" memes valeurs de X[]: "<<X[khi]<<endl;
  throw ParmError(PExcLongMessage(""));
}

a = (X[khi]-x)/h;
b = (x-X[klo])/h;
y = a*Y[klo]+b*Y[khi]+((a*a*a-a)*Y2[klo]+(b*b*b-b)*Y2[khi])*(h*h)/6.0;

return y;
}


//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

/*!
  \class SOPHYA::CSpline2
  \ingroup NTools
  Spline smoother (interpolator) for 2-D data points (y = f(x1,x2)) 
  \sa CSpline
*/

/*!
  Contructeur - Meme commentaire que pour CSpline avec:
  \verbatim
  x1[n1]: liste des coordonnees selon l axe 1
  x2[n2]: liste des coordonnees selon l axe 2
  y[n1*n2]: liste des valeurs avec le rangement suivant
  x1[0]......x1[n1-1]  x1[0]......x1[n1-1]  ... x1[0]......x1[n1-1]
  |    0<=i<n1      |  |    0<=i<n1      |  ... |    0<=i<n1      |
  |      j=0 X2[0]            j=1 X2[1]              j=n2-1 X2[n2-1]
  \endverbatim
*/
CSpline2::CSpline2(int n1,double* x1,int n2,double* x2,double* y
                  ,int natural,bool order)
  : Nel1(0), Nel2(0), corrupt_Y2(true), XY_Created(false), Natural(natural)
  , X1(NULL), X2(NULL), Y(NULL), Y2(NULL)
  , Nel_S(0), S(NULL), Sint(NULL), tmp(NULL)
{
SetNewTab(n1,x1,n2,x2,y,order,true);
if( x1 != NULL && x2 != NULL && y != NULL) ComputeCSpline();

}

//!     Createur par defaut.
CSpline2::CSpline2(int natural)
  : Nel1(0), Nel2(0), corrupt_Y2(true), XY_Created(false), Natural(natural)
  , X1(NULL), X2(NULL), Y(NULL), Y2(NULL)
  , Nel_S(0), S(NULL), Sint(NULL), tmp(NULL)
{
}

//////////////////////////////////////////////////////////////////////////////
CSpline2::~CSpline2()
{
DelTab();
}

//////////////////////////////////////////////////////////////////////////////
//!     Voir commentaire meme methode de CSpline
void CSpline2::SetNewTab(int n1,double* x1,int n2,double* x2,double* y
                        ,bool order,bool force)
{
ASSERT( n1>3 && n2>3 );

int n = ( n1 < n2 ) ? n2 : n1;

// allocation des buffers Y2 et tmp et des CSpline 1D
if( n1>Nel1 || n2>Nel2 || force ) {
  if( Y2  != NULL ) delete [] Y2;
  if( tmp != NULL ) delete [] tmp;
  Y2   = new double[n1*n2];
  tmp  = new double[n];

  // et les CSpline[n1] pour memoriser les interpolations sur x1(0->n1)
  if( S != NULL ) {
    for(int i=0;i<Nel_S;i++) if(S[i] != NULL) { delete S[i]; S[i]=NULL;}
    delete S; S = NULL;
  }
  S = new CSpline * [n2];
  for(int j=0;j<n2;j++) {
    S[j] = new CSpline(n1,NULL,NULL,0.,0.,Natural);
    S[j]->Free_Tmp();
  }
  Nel_S = n2;

  if( S != NULL ) { delete Sint; Sint = NULL;}
  Sint = new CSpline(n2,NULL,NULL,0.,0.,Natural);

}
// des-allocation eventuelle de X1,X2,Y
if( XY_Created ) {
  if( !order || n1>Nel1 || n2>Nel2 || force ) {
    if( X1 != NULL ) delete [] X1; X1 = NULL;
    if( X2 != NULL ) delete [] X2; X2 = NULL;
    if( Y != NULL )  delete [] Y;  Y  = NULL;
    XY_Created = false;
  }
}
// allocation eventuelle de X1,X2,Y
if( order ) {
  if( !XY_Created || n1>Nel1 || n2>Nel1 || force ) {
    X1 = new double[n1];
    X2 = new double[n2];
    Y  = new double[n1*n2];
    XY_Created = true;
  }
}
Nel1 = n1;  Nel2 = n2;
corrupt_Y2 = true;

if( x1==NULL || x2==NULL || y==NULL ) return;

// Classement eventuel par ordre des x1 et x2 croissants
if( order ) {

  if( tmp == NULL ) tmp = new double[n];
  ind = (int_4 *) tmp;
  double* Ytmp = new double[n1*n2];

  // tri par valeur croissantes de x1
  tri_double(x1,ind,(int_4) Nel1);
  for(int i=0;i<Nel1;i++) {
    X1[i] = x1[ind[i]];
    if( i>0 ) if( X1[i-1] >= X1[i] )
      { printf("CSpline::SetNewTab_Erreur: X1[%d]>=X1[%d] (%g>=%g)\n"
              ,i-1,i,X1[i-1],X1[i]);
        throw ParmError(PExcLongMessage("")); }
    for(int j=0;j<Nel2;j++) Ytmp[j*Nel1+i]  = y[j*Nel1+ind[i]];
  }

  // tri par valeur croissantes de x2
  tri_double(x2,ind,(int_4) Nel2);
  for(int j=0;j<Nel2;j++) {
    X2[j] = x2[ind[j]];
    if( j>0 ) if( X2[j-1] >= X2[j] )
      { printf("CSpline::SetNewTab_Erreur: X2[%d]>=X2[%d] (%g>=%g)\n"
              ,j-1,j,X2[j-1],X2[j]);
        throw ParmError(PExcLongMessage("")); }
    for(int i=0;i<Nel1;i++) Y[j*Nel1+i] = Ytmp[j*Nel1+ind[i]];
  }
  delete [] Ytmp;

} else {

  X1 = x1;
  X2 = x2;
  Y  = y;

}

}

//////////////////////////////////////////////////////////////////////////////
void CSpline2::DelTab()
{
if( X1  != NULL && XY_Created ) delete [] X1;  X1   = NULL;
if( X2  != NULL && XY_Created ) delete [] X2;  X2   = NULL;
if( Y   != NULL && XY_Created ) delete [] Y;   Y   = NULL;
if( Y2  != NULL ) delete [] Y2;   Y2  = NULL;
if( tmp != NULL ) delete [] tmp;  tmp = NULL;
if( S != NULL ) {
  for(int i=0;i<Nel_S;i++) if(S[i] != NULL) { delete S[i]; S[i]=NULL;}
  delete S; S = NULL;
}
if( Sint != NULL ) { delete Sint; Sint=NULL;}
}

//////////////////////////////////////////////////////////////////////////////
//!     Voir commentaire meme methode de CSpline
void CSpline2::ComputeCSpline()
{
// on ne fait rien si X1 ou X2 ou Y non connectes
if( X1 == NULL || X2 == NULL || Y == NULL ) return;
// On ne fait rien si rien n'a change
if( ! corrupt_Y2 ) return;

for(int j=0; j<Nel2; j++) {
  // on n'alloue pas de place nouvelle, on utilise CSpline2::tmp
  S[j]->tmp = tmp;
  // connection de X1,Y au spline 1D sans ordre demande
  S[j]->SetNewTab(Nel1,X1,&Y[j*Nel1],false,false);
  // calcul des coeff splien pour l'interpolation future
  S[j]->ComputeCSpline();
}

corrupt_Y2 = false;
}

//////////////////////////////////////////////////////////////////////////////
//! Calcule la valeur interpole (spline) pour le point \b (x1,x2)
double CSpline2::CSplineInt(double x1,double x2)
{
// calcul de la valeur Y pour x=x1 et remplissage du tampon tmp
for(int j=0;j<Nel2;j++) tmp[j] = S[j]->CSplineInt(x1);

// connection X2,tmp pour interpolation selon x=x2
Sint->SetNewTab(Nel2,X2,tmp,false,false);
// calcul des coeff pour interpolation selon X2
Sint->ComputeCSpline();
// Interpolation finale
return Sint->CSplineInt(x2);
}