793 lines
18 KiB
C++
793 lines
18 KiB
C++
|
/********************************************************
|
||
|
|
* ██████╗ ██████╗████████╗██╗
|
||
|
|
* ██╔════╝ ██╔════╝╚══██╔══╝██║
|
||
|
|
* ██║ ███╗██║ ██║ ██║
|
||
|
|
* ██║ ██║██║ ██║ ██║
|
||
|
|
* ╚██████╔╝╚██████╗ ██║ ███████╗
|
||
|
|
* ╚═════╝ ╚═════╝ ╚═╝ ╚══════╝
|
||
|
|
* Geophysical Computational Tools & Library (GCTL)
|
||
|
|
*
|
||
|
|
* Copyright (c) 2023 Yi Zhang (yizhang-geo@zju.edu.cn)
|
||
|
|
*
|
||
|
|
* GCTL is distributed under a dual licensing scheme. You can redistribute
|
||
|
|
* it and/or modify it under the terms of the GNU Lesser General Public
|
||
|
|
* License as published by the Free Software Foundation, either version 2
|
||
|
|
* of the License, or (at your option) any later version. You should have
|
||
|
|
* received a copy of the GNU Lesser General Public License along with this
|
||
|
|
* program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*
|
||
|
|
* If the terms and conditions of the LGPL v.2. would prevent you from using
|
||
|
|
* the GCTL, please consider the option to obtain a commercial license for a
|
||
|
|
* fee. These licenses are offered by the GCTL's original author. As a rule,
|
||
|
|
* licenses are provided "as-is", unlimited in time for a one time fee. Please
|
||
|
|
* send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget
|
||
|
|
* to include some description of your company and the realm of its activities.
|
||
|
|
* Also add information on how to contact you by electronic and paper mail.
|
||
|
|
******************************************************/
|
||
|
|
|
||
|
|
#include "../core/macro.h"
|
||
|
|
#include "mathfunc_ext.h"
|
||
|
|
#include "cmath"
|
||
|
|
|
||
|
|
void gctl::DTfilter(double **a,int m,int n,double A,double B,double Q,int sw)
|
||
|
|
{
|
||
|
|
int i,j;
|
||
|
|
for(i=0;i<m;i++)
|
||
|
|
{
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
{
|
||
|
|
switch(sw)
|
||
|
|
{
|
||
|
|
case 0:
|
||
|
|
{
|
||
|
|
if(a[i][j]>=A)
|
||
|
|
a[i][j]=Q;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case 1:
|
||
|
|
{
|
||
|
|
if(a[i][j]<=A)
|
||
|
|
a[i][j]=Q;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case 2:
|
||
|
|
{
|
||
|
|
if((a[i][j]<=A)||(a[i][j]>=B))
|
||
|
|
a[i][j]=Q;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case 3:
|
||
|
|
{
|
||
|
|
if((a[i][j]>=A)&&(a[i][j]<=B))
|
||
|
|
a[i][j]=Q;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void gctl::DTari(double **a,int m,int n,double A,int sw)
|
||
|
|
{
|
||
|
|
int i,j;
|
||
|
|
for(i=0;i<m;i++)
|
||
|
|
{
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
{
|
||
|
|
switch(sw)
|
||
|
|
{
|
||
|
|
case 0: { a[i][j]+=A; break; }
|
||
|
|
case 1: { a[i][j]*=A; break; }
|
||
|
|
case 2: { a[i][j]=a[i][j]/(double)(A+1e-40); break; }
|
||
|
|
case 3: { a[i][j]*=a[i][j]; break; }
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void gctl::DTfunc(double **a,int m,int n,double A,double B,int sw)
|
||
|
|
{
|
||
|
|
int i,j;
|
||
|
|
double min,p;
|
||
|
|
if(sw==2)
|
||
|
|
{
|
||
|
|
min=(double)fabs(a[0][0]);
|
||
|
|
for(i=0;i<m;i++)
|
||
|
|
{
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
{
|
||
|
|
if(fabs(a[i][j])<min) min=(double)fabs(a[i][j]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for(i=0;i<m;i++)
|
||
|
|
{
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
{
|
||
|
|
switch(sw)
|
||
|
|
{
|
||
|
|
case 0: {a[i][j]=A+B*a[i][j];break;}
|
||
|
|
case 1: {a[i][j]=A*(double)exp((double)(B*a[i][j]));break;}
|
||
|
|
case 2:
|
||
|
|
{
|
||
|
|
p=(double)fabs(a[i][j])/(double)(a[i][j]+1e-80);
|
||
|
|
a[i][j]=A*p*(double)log((double)(B*(fabs(a[i][j])+1e-80)/(min+1e-80)));
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case 3: {a[i][j]=A*(double)pow((double)(a[i][j]+1e-80),(double)B);break;}
|
||
|
|
case 4: {a[i][j]=A*(double)atan((double)(B*a[i][j]));break;}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////
|
||
|
|
// Preparation & Extending Data
|
||
|
|
//
|
||
|
|
// zr/zi...first address of real/image data array
|
||
|
|
// m/n...number of line/point; m1/n1...extended m/n
|
||
|
|
// me/ne...{m1/n1=pow(2,me/ne)}; np/nq...{m1/n1=2*(np/nq)+m/n}
|
||
|
|
double gctl::prep(double **zr,double **zi,int m,int n,int m1,int n1,int np,int nq)
|
||
|
|
{
|
||
|
|
int i,j; double pjg;
|
||
|
|
pjg=(double)0;
|
||
|
|
if(m==1)
|
||
|
|
{ pjg=(zr[0][0]+zr[0][n-1])/(double)2.;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{ for(j=0;j<n;j++)
|
||
|
|
{ pjg+=(zr[0][j]+zr[m-1][j]);
|
||
|
|
}
|
||
|
|
for(i=1;i<(m-1);i++)
|
||
|
|
{ pjg+=(zr[i][0]+zr[i][n-1]);
|
||
|
|
}
|
||
|
|
pjg=pjg/(double)(2*m+2*n-4);
|
||
|
|
}
|
||
|
|
for(i=(m-1);i>=0;i--)
|
||
|
|
{ for(j=(n-1);j>=0;j--)
|
||
|
|
{ zr[i+np][j+nq]=zr[i][j]-pjg;
|
||
|
|
} }
|
||
|
|
//---------------------------------------------
|
||
|
|
// extending data
|
||
|
|
//---------------------------------------------
|
||
|
|
|
||
|
|
if(m>1)
|
||
|
|
{ for(i=np;i<(m+np);i++)
|
||
|
|
{ zr[i][n1-1]=(double)0;
|
||
|
|
for(j=0;j<nq;j++)
|
||
|
|
{ zr[i][j]=zr[i][nq]*(double)(1-cos(((double)GCTL_Pi*(double)j/(double)nq)))/(double)2;
|
||
|
|
zr[i][j+n+nq]=zr[i][n+nq-1]*(double)(1+cos(((double)GCTL_Pi*(double)(j+1)/(double)(n1-n-nq))))/2;
|
||
|
|
} }
|
||
|
|
for(j=0;j<n1;j++)
|
||
|
|
{ zr[m1-1][j]=(double)0;
|
||
|
|
for(i=0;i<np;i++)
|
||
|
|
{ zr[i][j]=zr[np][j]*(double)(1-cos(((double)GCTL_Pi*(double)i/(double)np)))/(double)2;
|
||
|
|
zr[i+m+np][j]=zr[m+np-1][j]*(double)(1+cos(((double)GCTL_Pi*(double)(i+1)/(double)(m1-m-np))))/(double)2;
|
||
|
|
} }
|
||
|
|
for(i=0;i<m1;i++)
|
||
|
|
{ for(j=0;j<n1;j++)
|
||
|
|
{ zi[i][j]=(double)0;
|
||
|
|
} }
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{ for(j=0;j<nq;j++)
|
||
|
|
{ zr[0][j]=zr[0][nq]*(double)(1-cos(((double)GCTL_Pi*(double)j/(double)nq)))/(double)2;
|
||
|
|
zr[0][j+n+nq]=zr[0][n+nq-1]*(double)(1+cos(((double)GCTL_Pi*(double)(j+1)/(double)(n1-n-nq))))/(double)2;
|
||
|
|
}
|
||
|
|
for(j=0;j<n1;j++)
|
||
|
|
{ zi[0][j]=(double)0; }
|
||
|
|
}
|
||
|
|
return pjg;
|
||
|
|
}
|
||
|
|
///////////////////////////////////////////
|
||
|
|
////To extend the data in array a[][] from
|
||
|
|
//// a[m][n] to a[m+2*mx][n+2*nx]
|
||
|
|
void gctl::ext(double **a,int m,int n,int mx,int nx)
|
||
|
|
{ int i,j,i1,j1;
|
||
|
|
|
||
|
|
for(i=(m+mx-1);i>=mx;i--)
|
||
|
|
for(j=(n+nx-1);j>=nx;j--)
|
||
|
|
a[i][j]=a[i-mx][j-nx];
|
||
|
|
for(i=mx;i<(m+mx);i++)
|
||
|
|
{ for(j=0;j<nx;j++)
|
||
|
|
{ j1=n+nx+j;
|
||
|
|
a[i][j]=a[i][2*nx-j]; a[i][j1]=a[i][n+nx-2-j];
|
||
|
|
} }
|
||
|
|
for(j=0;j<n+2*nx;j++)
|
||
|
|
{ for(i=0;i<mx;i++)
|
||
|
|
{ i1=m+mx+i;
|
||
|
|
a[i][j]=a[2*mx-i][j]; a[i1][j]=a[m+mx-2-i][j];
|
||
|
|
} }
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
////////////////////////////////////////////
|
||
|
|
//To find the Maximun and Minimun of present array data
|
||
|
|
void gctl::MaxMin(double **a,int m,int n,double *MxMn)
|
||
|
|
{
|
||
|
|
double p;
|
||
|
|
MxMn[0]=MxMn[1]=a[0][0]; p=(double)0;
|
||
|
|
for(int i=0;i<m;i++)
|
||
|
|
{
|
||
|
|
for(int j=0;j<n;j++)
|
||
|
|
{
|
||
|
|
p+=a[i][j];
|
||
|
|
if(MxMn[1]<a[i][j])
|
||
|
|
MxMn[1]=a[i][j];
|
||
|
|
else if(MxMn[0]>a[i][j])
|
||
|
|
MxMn[0]=a[i][j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
MxMn[2]=p/(double)(m*n+1e-40);
|
||
|
|
}
|
||
|
|
|
||
|
|
double gctl::int4p(double x,double d,double y1,double y2,double y3,double y4)
|
||
|
|
{ double z,t;
|
||
|
|
t=y1*x*(x-d)/(2*d*d)-y2*(x+d)*(x-d)/(d*d)+y3*(x+d)*x/(2*d*d);
|
||
|
|
z=y2*(x-d)*(x-2*d)/(2*d*d)-y3*x*(x-2*d)/(d*d)+y4*x*(x-d)/(2*d*d);
|
||
|
|
z=(z+t)/2;
|
||
|
|
return(z);
|
||
|
|
}
|
||
|
|
|
||
|
|
void gctl::intp(double **a,int m,int n,int mi,int ni)
|
||
|
|
{ int i,j,k; double d,x;
|
||
|
|
// mi=(int)(dx0/dx+.0001)
|
||
|
|
// ---number of inserted rows + 1
|
||
|
|
// ni=(int)(dy0/dy+.0001)
|
||
|
|
// ---number of inserted columns + 1
|
||
|
|
ext(a,m,n,1,1);
|
||
|
|
for(i=m+1;i>=0;i--)
|
||
|
|
for(j=n+1;j>=0;j--)
|
||
|
|
a[i*mi][j*ni]=a[i][j];
|
||
|
|
d=(double)ni; //(0, d, 2d, 3d,) are y_values knawn
|
||
|
|
for(i=0;i<=(m+1)*mi;i+=mi)
|
||
|
|
{ for(j=0;j<=(n-2)*ni;j+=ni)
|
||
|
|
{ for(k=1;k<ni;k++)
|
||
|
|
{ x=(double)k; // y=f(x) is the value required
|
||
|
|
a[i][j+ni+k]=int4p(x,d,a[i][j],a[i][j+ni],a[i][j+2*ni],a[i][j+3*ni]);
|
||
|
|
}
|
||
|
|
} }
|
||
|
|
d=(double)mi; //(0, d, 2d, 3d,) are y_values knawn
|
||
|
|
for(j=ni;j<=n*ni;j++)
|
||
|
|
{ for(i=0;i<=(m-2)*mi;i+=mi)
|
||
|
|
{ for(k=1;k<mi;k++)
|
||
|
|
{ x=(double)k; // y=f(x) is the value required
|
||
|
|
a[i+mi+k][j]=int4p(x,d,a[i][j],a[i+mi][j],a[i+2*mi][j],a[i+3*mi][j]);
|
||
|
|
} } }
|
||
|
|
for(i=0;i<(m-1)*mi+1;i++)
|
||
|
|
for(j=0;j<(n-1)*ni+1;j++)
|
||
|
|
a[i][j]=a[i+mi][j+ni];
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////
|
||
|
|
/* Fast-Fourier_Transform */
|
||
|
|
|
||
|
|
/* zr/zi...first address of real/image data array */
|
||
|
|
/* m/n...number of line/point; m1/n1...extended m/n */
|
||
|
|
/* me/ne...{m1/n1=pow(2,me/ne)}; */
|
||
|
|
/* key... 0 for +FFT, 1 for -FFT */
|
||
|
|
/* 1-Dimension Fast Fourier Transform (FFT) */
|
||
|
|
void gctl::fft1(double *ar,double *ai,int m,int n,int key)
|
||
|
|
{ int i,j,k,k1,l1,l2,ip;
|
||
|
|
double *ir,*ii,*i1,*i2;
|
||
|
|
double f1,t1,t2,v,u,w1,w2,d,vu,pi1;
|
||
|
|
f1=(double)-1;
|
||
|
|
if(key==0) f1=(double)1;
|
||
|
|
j=0;
|
||
|
|
for(i=0,ir=ar,ii=ai;i<n;i++,ir++,ii++)
|
||
|
|
{ if(i<j)
|
||
|
|
{ i1=ar+j; i2=ai+j;
|
||
|
|
t1=*i1; t2=*i2; *i1=*ir; *i2=*ii; *ir=t1; *ii=t2;
|
||
|
|
}
|
||
|
|
k1=n/2;
|
||
|
|
while((j>=k1)&&(k1!=0))
|
||
|
|
{ j=j-k1; k1=k1/2;
|
||
|
|
}
|
||
|
|
j=j+k1;
|
||
|
|
}
|
||
|
|
for(j=1;j<m+1;j++)
|
||
|
|
{ l1=2;
|
||
|
|
for (i=1;i<j;i++) l1=2*l1;
|
||
|
|
l2=l1/2; pi1=(double)GCTL_Pi/(double)l2; v=(double)1; u=(double)0;
|
||
|
|
w1=(double)cos(pi1); w2=(double)sin(pi1);
|
||
|
|
for(k=1;k<l2+1;k++)
|
||
|
|
{ d=f1*u;
|
||
|
|
for(i=k-1;i<n-l1+k;i+=l1)
|
||
|
|
{ ip=i+l2; i1=ar+ip; i2=ai+ip; ir=ar+i; ii=ai+i;
|
||
|
|
t1=*i1*v+*i2*d; t2=*i2*v-*i1*d;
|
||
|
|
*i1=*ir-t1; *i2=*ii-t2; *ir=*ir+t1; *ii=*ii+t2;
|
||
|
|
}
|
||
|
|
vu=v*w1-u*w2; u=v*w2+u*w1; v=vu;
|
||
|
|
} }
|
||
|
|
if(key==0)
|
||
|
|
for(i=0,ir=ar,ii=ai;i<n;i++,ir++,ii++)
|
||
|
|
{ *ir/=n; *ii/=n;
|
||
|
|
}
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
void gctl::fft(double **zr,double **zi,int m1,int n1,int me,int ne,int key)
|
||
|
|
{
|
||
|
|
int i,j;
|
||
|
|
double *ir0,*ii0,*ir1,*ii1;
|
||
|
|
|
||
|
|
if(m1==1)
|
||
|
|
{
|
||
|
|
ir0=*(zr+0)+0; ii0=*(zi+0)+0;
|
||
|
|
fft1(ir0,ii0,ne,n1,key);
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
double *cr = new double [m1];
|
||
|
|
double *ci = new double [m1];
|
||
|
|
|
||
|
|
for(i=0;i<m1;i++)
|
||
|
|
{
|
||
|
|
ir0=*(zr+i)+0; ii0=*(zi+i)+0; ir1=*(zr+0)+i+8;
|
||
|
|
fft1(ir0,ii0,ne,n1,key);
|
||
|
|
}
|
||
|
|
|
||
|
|
for(j=0;j<n1;j++)
|
||
|
|
{
|
||
|
|
for(i=0;i<m1;i++)
|
||
|
|
{
|
||
|
|
cr[i]=*(*(zr+i)+j); ci[i]=*(*(zi+i)+j);
|
||
|
|
}
|
||
|
|
fft1(cr,ci,me,m1,key);
|
||
|
|
for(i=0;i<m1;i++)
|
||
|
|
{
|
||
|
|
ir1=*(zr+i)+j; ii1=*(zi+i)+j;
|
||
|
|
*ir1=cr[i]; *ii1=ci[i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
delete[] cr;
|
||
|
|
delete[] ci;
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////
|
||
|
|
// To form the metrix of factors /////////////////////////////
|
||
|
|
// x,y sould are positive
|
||
|
|
// n is order of f(x,y)
|
||
|
|
void gctl::fact(double **s,double *x,double *y,double *g,double *b,int mt,int n)
|
||
|
|
{ int i,j,l,l1,l2,lx,ly,k,k1,k2,kx,ky,kk;
|
||
|
|
double p,f1,f2,f3,f4;
|
||
|
|
for(i=0;i<=n;i++)
|
||
|
|
{ l1=(i+1)*i/2+1; l2=l1+i; lx=i; ly=0;
|
||
|
|
for(l=l1;l<=l2;l++)
|
||
|
|
{ for(j=0;j<=n;j++)
|
||
|
|
{ k1=(j+1)*j/2+1; k2=k1+j; kx=j; ky=0;
|
||
|
|
for(k=k1;k<=k2;k++)
|
||
|
|
{ p=0.;
|
||
|
|
for(kk=0;kk<mt;kk++)
|
||
|
|
{ f1=pow(x[kk],lx);
|
||
|
|
f2=pow(y[kk],ly);
|
||
|
|
f3=pow(x[kk],kx);
|
||
|
|
f4=pow(y[kk],ky);
|
||
|
|
p+=f1*f2*f3*f4;
|
||
|
|
}
|
||
|
|
s[l-1][k-1]=p;
|
||
|
|
kx=kx-1; ky=ky+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
lx=lx-1; ly=ly+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for(j=0;j<=n;j++)
|
||
|
|
{ k1=(j+1)*j/2+1; k2=k1+j; kx=j; ky=0;
|
||
|
|
for(k=k1;k<=k2;k++)
|
||
|
|
{ p=0.;
|
||
|
|
for(kk=0;kk<mt;kk++)
|
||
|
|
{
|
||
|
|
f1=pow(x[kk],kx);
|
||
|
|
f2=pow(y[kk],ky);
|
||
|
|
p+=f1*f2*g[kk];
|
||
|
|
}
|
||
|
|
b[k-1]=p;
|
||
|
|
kx=kx-1; ky=ky+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////
|
||
|
|
// To form the metrix of factors /////////////////////////////
|
||
|
|
// x,y sould are positive
|
||
|
|
// m is heigher order of f(x,y) along between x and y,
|
||
|
|
void gctl::fact1(double **s,double *x,double *y,double *g,double *b,int mt,int n,int xLt,int yLt)
|
||
|
|
{ int i,j,l,l1,l2,lx,ly,k,k1,k2,kx,ky,kk,ks,ls;
|
||
|
|
double p,f1,f2,f3,f4;
|
||
|
|
ls=0;
|
||
|
|
for(i=0;i<=n;i++)
|
||
|
|
{
|
||
|
|
l1=(i+1)*i/2+1; l2=l1+i; lx=i; ly=0;
|
||
|
|
for(l=l1;l<=l2;l++)
|
||
|
|
{
|
||
|
|
if((lx>xLt)||(ly>yLt))
|
||
|
|
{
|
||
|
|
ls+=1; lx-=1; ly+=1; continue;
|
||
|
|
}
|
||
|
|
for(j=0;j<=n;j++)
|
||
|
|
{
|
||
|
|
k1=(j+1)*j/2+1; k2=k1+j; kx=j; ky=0;
|
||
|
|
ks=0;
|
||
|
|
for(k=k1;k<=k2;k++)
|
||
|
|
{
|
||
|
|
if((kx>xLt)||(ky>yLt))
|
||
|
|
{
|
||
|
|
ks+=1; kx-=1; ky+=1; continue;
|
||
|
|
}
|
||
|
|
p=0.;
|
||
|
|
for(kk=0;kk<mt;kk++)
|
||
|
|
{
|
||
|
|
f1=pow(x[kk],lx);
|
||
|
|
f2=pow(y[kk],ly);
|
||
|
|
f3=pow(x[kk],kx);
|
||
|
|
f4=pow(y[kk],ky);
|
||
|
|
p+=f1*f2*f3*f4;
|
||
|
|
}
|
||
|
|
s[l-1-ls][k-1-ks]=p;
|
||
|
|
kx=kx-1; ky=ky+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
lx=lx-1; ly=ly+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
ks=0;
|
||
|
|
for(j=0;j<=n;j++)
|
||
|
|
{
|
||
|
|
k1=(j+1)*j/2+1; k2=k1+j; kx=j; ky=0;
|
||
|
|
for(k=k1;k<=k2;k++)
|
||
|
|
{
|
||
|
|
if((kx>xLt)||(ky>yLt))
|
||
|
|
{
|
||
|
|
ks+=1; kx-=1; ky+=1; continue;
|
||
|
|
}
|
||
|
|
p=0.;
|
||
|
|
for(kk=0;kk<mt;kk++)
|
||
|
|
{
|
||
|
|
f1=pow(x[kk],kx);
|
||
|
|
f2=pow(y[kk],ky);
|
||
|
|
p+=f1*f2*g[kk];
|
||
|
|
}
|
||
|
|
b[k-1-ks]=p;
|
||
|
|
kx=kx-1; ky=ky+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////
|
||
|
|
//To calculate the multiply values of x and y/////////////
|
||
|
|
// x,y sould are positive
|
||
|
|
void gctl::tsv(double *s,double x,double y,int n)
|
||
|
|
//////////// n is order of f(x,y) in y,
|
||
|
|
{ int j,k,k1,k2,kx,ky;
|
||
|
|
s[0]=(double)1;
|
||
|
|
for(j=1;j<=n;j++)
|
||
|
|
{ k1=(j+1)*j/2+1; k2=k1+j; kx=j; ky=0;
|
||
|
|
for(k=k1;k<=k2;k++)
|
||
|
|
{ s[k-1]=pow(x,kx)*pow(y,ky);
|
||
|
|
kx=kx-1; ky=ky+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////
|
||
|
|
//To calculate the multiply values of x and y/////////////
|
||
|
|
// x,y sould are positive
|
||
|
|
void gctl::tsv1(double *s,double x,double y,int n,int xLt,int yLt)
|
||
|
|
//////////// n is higher order of f(x,y) along that between x and y
|
||
|
|
{ int j,k,k1,k2,kx,ky,ks;
|
||
|
|
ks=0;
|
||
|
|
s[0]=(double)1;
|
||
|
|
for(j=1;j<=n;j++)
|
||
|
|
{ k1=(j+1)*j/2+1; k2=k1+j; kx=j; ky=0;
|
||
|
|
for(k=k1;k<=k2;k++)
|
||
|
|
{ if((kx>xLt)||(ky>yLt))
|
||
|
|
{ ks+=1; kx-=1; ky+=1; continue; }
|
||
|
|
s[k-1-ks]=pow(x,kx)*pow(y,ky);
|
||
|
|
kx=kx-1; ky=ky+1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////
|
||
|
|
void gctl::tran(double **a,int n)
|
||
|
|
{ int i,j; double t;
|
||
|
|
for(i=0;i<n-1;i++)
|
||
|
|
{ for(j=i+1;j<n;j++)
|
||
|
|
{ t=a[i][j]; a[i][j]=a[j][i]; a[j][i]=t; }
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
void gctl::metrimult(double **a,double **b,int m,int n,int l,double **c)
|
||
|
|
{
|
||
|
|
int i,j,k; double p;
|
||
|
|
for(i=0;i<m;i++)
|
||
|
|
{ for(j=0;j<l;j++)
|
||
|
|
{ p=0.;
|
||
|
|
for(k=0;k<n;k++)
|
||
|
|
p+=a[i][k]*b[k][j];
|
||
|
|
c[i][j]=p;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
///////////////////////////
|
||
|
|
|
||
|
|
//C*********************************************************************C
|
||
|
|
//C C
|
||
|
|
//C Calculation for Singular Values and Vecters C
|
||
|
|
//C C
|
||
|
|
//C*********************************************************************C
|
||
|
|
//C [ SVD.V2 ]
|
||
|
|
//C
|
||
|
|
//C******************************************************************C
|
||
|
|
//C
|
||
|
|
// SUBROUTINE SVD(MD,ND,M,N,A,U,V,Q,INDEX)
|
||
|
|
//C
|
||
|
|
//C CALLS NO OTHER ROUTINES
|
||
|
|
//C ( SINGULAR VALUE DECOMPOSITION )
|
||
|
|
//C FOR ALGO PROGRAM SEE WILKINSON+REINSC HANDBOOK
|
||
|
|
//C FOR AUTOMATIC COMPUTATION VOL 2 - LINEAR ALGEBRA, PG140-144,
|
||
|
|
//C TRANSLATED FROM ALGOL BY R.L.PARKER.
|
||
|
|
//C THE MATRIX A(M,N) IS DECOMPOSED. SINGULAR VALUES IN Q(N),
|
||
|
|
//C PRE-MATRIX IN U(M,N), POST-MATRIX IN V(N,N).
|
||
|
|
//C INDEX MAY BE 1,2,3 OR 4.
|
||
|
|
//C IF 1, FIND U,V;
|
||
|
|
//C IF 2, FIND ONLY U;
|
||
|
|
//C IF 3, FIND ONLY V;
|
||
|
|
//C IF 4, FIND NEITHER.
|
||
|
|
//C Translated from FORTRAN to C by Chen Chao in 1997
|
||
|
|
void gctl::svd_standalone(double **a,int m,int n,double **u,double **v,double *q,int Index)
|
||
|
|
{
|
||
|
|
int i,j,k,l;
|
||
|
|
double c,f,g,h,s,x,y,z;
|
||
|
|
double *e=new double [n];
|
||
|
|
double eps=(double)(1e-40);
|
||
|
|
double tol=(double)(1e-60);
|
||
|
|
for(i=0;i<m;i++)
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
u[i][j]=a[i][j];
|
||
|
|
|
||
|
|
//C HOUSEHOLDER REDUCTION TO BI-DIAGONAL FORM
|
||
|
|
g=(double)0; x=(double)0;
|
||
|
|
for(i=0;i<n;i++)
|
||
|
|
{
|
||
|
|
l=i+1; e[i]=g; g=(double)0;
|
||
|
|
if(i<m)
|
||
|
|
{
|
||
|
|
s=(double)0;
|
||
|
|
for(j=i;j<m;j++)
|
||
|
|
s+=u[j][i]*u[j][i];
|
||
|
|
if(s>=tol)
|
||
|
|
{
|
||
|
|
f=u[i][i];
|
||
|
|
g=-sqrt(s)*f/(fabs(f)+(double)1e-40);
|
||
|
|
h=f*g-s;
|
||
|
|
u[i][i]=f-g;
|
||
|
|
if(l<n)
|
||
|
|
{
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
{
|
||
|
|
s=(double)0;
|
||
|
|
for(k=i;k<m;k++)
|
||
|
|
s+=u[k][i]*u[k][j];
|
||
|
|
f=s/h;
|
||
|
|
for(k=i;k<m;k++)
|
||
|
|
u[k][j]+=f*u[k][i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//C/////////////////////////
|
||
|
|
q[i]=g; g=(double)0;
|
||
|
|
if(l<n)
|
||
|
|
{
|
||
|
|
s=(double)0;
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
s+=u[i][j]*u[i][j];
|
||
|
|
if(s>=tol)
|
||
|
|
{
|
||
|
|
f=u[i][i+1];
|
||
|
|
g=-sqrt(s)*f/(fabs(f)+(double)1e-40);
|
||
|
|
h=f*g-s;
|
||
|
|
u[i][i+1]=f-g;
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
e[j]=u[i][j]/h;
|
||
|
|
if(l<m)
|
||
|
|
{
|
||
|
|
for(j=l;j<m;j++)
|
||
|
|
{
|
||
|
|
s=(double)0;
|
||
|
|
for(k=l;k<n;k++)
|
||
|
|
s+=u[j][k]*u[i][k];
|
||
|
|
for(k=l;k<n;k++)
|
||
|
|
u[j][k]+=s*e[k];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
y=fabs(q[i])+fabs(e[i]);
|
||
|
|
if(y>x)
|
||
|
|
x=y;
|
||
|
|
}
|
||
|
|
//C
|
||
|
|
//C ACCUMULATION OF RIGHT-HAND TRANSFORMS (V)
|
||
|
|
//C
|
||
|
|
if((Index==1)||(Index==3))
|
||
|
|
{
|
||
|
|
v[n-1][n-1]=(double)1;
|
||
|
|
g=e[n-1];
|
||
|
|
for(i=n-2;i>=0;i--)
|
||
|
|
{
|
||
|
|
l=i+1;
|
||
|
|
if(g!=(double)0)
|
||
|
|
{
|
||
|
|
h=u[i][i+1]*g;
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
v[j][i]=u[i][j]/h;
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
{
|
||
|
|
s=(double)0;
|
||
|
|
for(k=l;k<n;k++)
|
||
|
|
s+=u[i][k]*v[k][j];
|
||
|
|
for(k=l;k<n;k++)
|
||
|
|
v[k][j]+=s*v[k][i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
{
|
||
|
|
v[j][i]=(double)0;
|
||
|
|
v[i][j]=(double)0;
|
||
|
|
}
|
||
|
|
v[i][i]=(double)1; g=e[i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//C
|
||
|
|
//C ACCUMULATION OF LEFT-HAND TRANSFORMS ( U )
|
||
|
|
//C
|
||
|
|
if((Index==1)||(Index==2))
|
||
|
|
{
|
||
|
|
for(i=(n<m)?(n-1):(m-1);i>=0;i--)
|
||
|
|
{
|
||
|
|
l=i+1; g=q[i];
|
||
|
|
if(l<n)
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
u[i][j]=(double)0;
|
||
|
|
if(g!=(double)0)
|
||
|
|
{
|
||
|
|
h=u[i][i]*g;
|
||
|
|
if(l<n)
|
||
|
|
{
|
||
|
|
for(j=l;j<n;j++)
|
||
|
|
{
|
||
|
|
s=(double)0;
|
||
|
|
for(k=l;k<m;k++)
|
||
|
|
s+=u[k][i]*u[k][j];
|
||
|
|
f=s/h;
|
||
|
|
for(k=i;k<m;k++)
|
||
|
|
u[k][j]+=f*u[k][i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for(j=i;j<m;j++)
|
||
|
|
u[j][i]=u[j][i]/g;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
for(j=i;j<m;j++)
|
||
|
|
u[j][i]=(double)0;
|
||
|
|
}
|
||
|
|
u[i][i]+=(double)1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//C
|
||
|
|
//C DIAGONALIZATION OF BI-DIAGONAL FORM
|
||
|
|
//C
|
||
|
|
eps*=x;
|
||
|
|
for(k=n-1;k>=0;k--)
|
||
|
|
{
|
||
|
|
label_380: /////////////C TEST F-SPLITTING
|
||
|
|
for(l=k;l>=0;l--)
|
||
|
|
{
|
||
|
|
if(fabs(e[l])<=eps)
|
||
|
|
goto label_440;
|
||
|
|
if(l==0)
|
||
|
|
continue;
|
||
|
|
if(fabs(q[l-1])<=eps)
|
||
|
|
goto label_410;
|
||
|
|
}
|
||
|
|
|
||
|
|
label_410: ///// CANCELLATION OF E(L), IF L .GT. 1
|
||
|
|
c=(double)0;
|
||
|
|
s=(double)1;
|
||
|
|
for(i=l;i<=k;i++)
|
||
|
|
{
|
||
|
|
f=s*e[i]; e[i]*=c;
|
||
|
|
if(fabs(f)<=eps)
|
||
|
|
goto label_440;
|
||
|
|
g=q[i]; q[i]=sqrt(f*f+g*g);
|
||
|
|
h=q[i]; c=g/h; s=-f/h;
|
||
|
|
if((Index==1)||(Index==2))
|
||
|
|
{
|
||
|
|
for(j=0;j<m;j++)
|
||
|
|
{
|
||
|
|
y=u[j][l-1]; z=u[j][i];
|
||
|
|
u[j][l-1]=y*c+z*s;
|
||
|
|
u[j][i]=-y*s+z*c;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
label_440: //// TEST F-CONVERGENCE
|
||
|
|
z=q[k];
|
||
|
|
if(l==k)
|
||
|
|
goto label_480;
|
||
|
|
/////////// SHIFT FROM BOTTOM 2 X 2 MINOR
|
||
|
|
x=q[l];
|
||
|
|
y=q[k-1];
|
||
|
|
g=e[k-1];
|
||
|
|
h=e[k];
|
||
|
|
f=(double)0.5*((y-z)*(y+z)+(g-h)*(g+h))/h/y;
|
||
|
|
g=sqrt(f*f+(double)1);
|
||
|
|
f=((x-z)*(x+z)+h*(y/(f+g*f/(fabs(f)+(double)1e-40))-h))/x;
|
||
|
|
///////////// NEXT Q-R TRANSFORMATION
|
||
|
|
c=(double)1;
|
||
|
|
s=(double)1;
|
||
|
|
for(i=l+1;i<=k;i++)
|
||
|
|
{
|
||
|
|
g=e[i]; y=q[i]; h=s*g; g=c*g;
|
||
|
|
z=sqrt(f*f+h*h);
|
||
|
|
e[i-1]=z; c=f/z; s=h/z;
|
||
|
|
f=x*c+g*s; g=-x*s+g*c;
|
||
|
|
h=y*s; y=y*c;
|
||
|
|
if((Index==1)||(Index==3))
|
||
|
|
{
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
{
|
||
|
|
x=v[j][i-1]; z=v[j][i];
|
||
|
|
v[j][i-1]=x*c+z*s;
|
||
|
|
v[j][i]=-x*s+z*c;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
z=sqrt(f*f+h*h);
|
||
|
|
q[i-1]=z; c=f/z; s=h/z;
|
||
|
|
f=c*g+s*y; x=-s*g+c*y;
|
||
|
|
if((Index==1)||(Index==2))
|
||
|
|
{
|
||
|
|
for(j=0;j<m;j++)
|
||
|
|
{
|
||
|
|
y=u[j][i-1]; z=u[j][i];
|
||
|
|
u[j][i-1]=y*c+z*s;
|
||
|
|
u[j][i]=-y*s+z*c;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
e[l]=(double)0;
|
||
|
|
e[k]=f;
|
||
|
|
q[k]=x;
|
||
|
|
goto label_380;
|
||
|
|
|
||
|
|
label_480: ///////////C CONVERGENCE///////////////
|
||
|
|
if(z<(double)0)
|
||
|
|
{
|
||
|
|
q[k]=-z;
|
||
|
|
if((Index==1)||(Index==3))
|
||
|
|
{
|
||
|
|
for(j=0;j<n;j++)
|
||
|
|
v[j][k]=-v[j][k];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delete[] e;
|
||
|
|
return ;
|
||
|
|
}
|