realfft

#include "stdafx.h"

#include "math.h"

void SWAP(double & a, double & b)
{
 double c;
 c=a;
 a=b;
 b=c;
}

void four1(double * data, int nn2, const int isign)
{
 int n,mmax,m,j,istep,i;
 double wtemp, wr, wpr, wpi, wi, theta,tempr, tempi;

 int nn=nn2/2;
 n=nn<<1;
 j=1;
 for(i=1;i<n;i+=2) {
  if(j>i){
   SWAP(data[j-1],data[i-1]);
   SWAP(data[j],data[i]);
  }
  m=nn;
  while(m>=2 && j>m){
   j-=m;
   m>>=1;
  }
  j+=m;
 }
 mmax=2;
 while(n>mmax){
  istep=mmax<<1;
  theta=isign*(6.28318530717959/mmax);
  wtemp=sin(0.5*theta);
  wpr=-2.0*wtemp*wtemp;
  wpi=sin(theta);
  wr=1.0;
  wi=0.0;
  for(m=1;m<mmax;m+=2) {
   for(i=m;i<=n;i+=istep) {
    j=i+mmax;
    tempr=wr*data[j-1]-wi*data[j];
    tempi=wr*data[j]+wi*data[j-1];
    data[j-1]=data[i-1]-tempr;
    data[j]=data[i]-tempi;
    data[i-1]+=tempr;
    data[i]+=tempi;
   }
   wr=(wtemp=wr)*wpr-wi*wpi+wr;
   wi=wi*wpr+wtemp*wpi+wi;
  }
  mmax=istep;
 }
}

void realfft(double * data, int n, const int isign)
{
 int i,i1,i2,i3,i4;
 double c1=0.5, c2, h1r, h1i, h2r, h2i, wr, wi, wpr, wpi, wtemp, theta;

 theta=3.141592653589793238/(n>>1);
 if(isign == 1){
  c2=-0.5;
  four1(data,n,1);
 } else {
  c2=0.5;
  theta=-theta;
 }
 wtemp = sin(0.5*theta);
 wpr=-2.0*wtemp*wtemp;
 wpi=sin(theta);
 wr=1.0+wpr;
 wi=wpi;
 for(i=1;i<(n>>2);i++){
  i2=1+(i1=i+i);
  i4=1+(i3=n-i1);
  h1r=c1*(data[i1]+data[i3]);
  h1i=c1*(data[i2]-data[i4]);
  h2r=-c2*(data[i2]+data[i4]);
  h2i=c2*(data[i1]-data[i3]);
  data[i1]=h1r+wr*h2r-wi*h2i;
  data[i2]=h1i+wr*h2i+wi*h2r;
  data[i3]=h1r-wr*h2r+wi*h2i;
  data[i4]=-h1i+wr*h2i+wi*h2r;
  wr=(wtemp=wr)*wpr-wi*wpi+wr;
  wi=wi*wpr+wtemp*wpi+wi;
 }
 if(isign==1){
  data[0]=(h1r=data[0])+data[1];
  data[1]=h1r-data[1];
 } else {
  data[0]=c1*((h1r=data[0])+data[1]);
  data[1]=c1*(h1r-data[1]);
  four1(data,n,-1);
 }
 if(isign==-1){
  for(i=0;i<n;i++)
   data[i]*=2.0/n;
 }
}

void realfftm(double * data, int n)
{
 realfft(data,n,1);
 for(int i=1; i<n/2; i++){
  data[i]=sqrt(data[2*i]*data[2*i]+data[2*i+1]*data[2*i+1]);
 }
 data[0]=fabs(data[0]);
}

void realfftm2(double * data, int n)
{
 realfft(data,n,1);
 for(int i=1; i<n/2; i++){
  data[i]=data[2*i]*data[2*i]+data[2*i+1]*data[2*i+1];
 }
 data[0]=data[0]*data[0];
}

void correl(const double * data1, const double * data2, double * ans, int nn)
// 相关程序，计算data1与data2的相关值，在数组ans中返回，三个数组都已经准备好，
// 长度为nn，nn必须是2的整数幂，本程序的符号约定，如果data1滞后data2，则ans将在
// 正滞后呈现峰值。
{
 int no2,i;
 double tmp;

 int n=nn;
 double * temp = new double[n];
 for(i=0;i<n;i++){
  ans[i]=data1[i];
  temp[i]=data2[i];
 }
 realfft(ans,n,1);
 realfft(temp,n,1);
 no2=n>>1;
 for(i=2;i<n;i+=2){
  tmp = ans[i];
  ans[i]=(ans[i]*temp[i]+ans[i+1]*temp[i+1])/no2;
  ans[i+1]=(ans[i+1]*temp[i]-tmp*temp[i+1])/no2;
 }
 ans[0]=ans[0]*temp[0]/no2;
 ans[1]=ans[1]*temp[1]/no2;
 realfft(ans,n,-1);
 delete []temp;
}

double sinc(double x)
{
 if(x==0.0)
  return 1;
 return sin(x)/x;
}