fftpack.c 21.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29
/*
 *  This file is part of libfftpack.
 *
 *  libfftpack is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  libfftpack is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with libfftpack; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

/*
 *  libfftpack is being developed at the Max-Planck-Institut fuer Astrophysik
 *  and financially supported by the Deutsches Zentrum fuer Luft- und Raumfahrt
 *  (DLR).
 */

/*
  fftpack.c : A set of FFT routines in C.
  Algorithmically based on Fortran-77 FFTPACK by Paul N. Swarztrauber
  (Version 4, 1985).

Leo Singer's avatar
Leo Singer committed
30
  C port by Martin Reinecke (2010-2017)
31 32 33 34 35 36
 */

#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "fftpack.h"
Leo Singer's avatar
Leo Singer committed
37
#include "trig_utils.h"
38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75

#define WA(x,i) wa[(i)+(x)*ido]
#define CH(a,b,c) ch[(a)+ido*((b)+l1*(c))]
#define CC(a,b,c) cc[(a)+ido*((b)+cdim*(c))]
#define PM(a,b,c,d) { a=c+d; b=c-d; }
#define PMC(a,b,c,d) { a.r=c.r+d.r; a.i=c.i+d.i; b.r=c.r-d.r; b.i=c.i-d.i; }
#define ADDC(a,b,c) { a.r=b.r+c.r; a.i=b.i+c.i; }
#define SCALEC(a,b) { a.r*=b; a.i*=b; }
#define CONJFLIPC(a) { double tmp_=a.r; a.r=-a.i; a.i=tmp_; }
/* (a+ib) = conj(c+id) * (e+if) */
#define MULPM(a,b,c,d,e,f) { a=c*e+d*f; b=c*f-d*e; }

typedef struct {
  double r,i;
} cmplx;

#define CONCAT(a,b) a ## b

#define X(arg) CONCAT(passb,arg)
#define BACKWARD
#include "fftpack_inc.c"
#undef BACKWARD
#undef X

#define X(arg) CONCAT(passf,arg)
#include "fftpack_inc.c"
#undef X

#undef CC
#undef CH
#define CC(a,b,c) cc[(a)+ido*((b)+l1*(c))]
#define CH(a,b,c) ch[(a)+ido*((b)+cdim*(c))]

static void radf2 (size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=2;

Leo Singer's avatar
Leo Singer committed
76
  for (size_t k=0; k<l1; k++)
77 78
    PM (CH(0,0,k),CH(ido-1,1,k),CC(0,k,0),CC(0,k,1))
  if ((ido&1)==0)
Leo Singer's avatar
Leo Singer committed
79
    for (size_t k=0; k<l1; k++)
80 81 82 83 84
      {
      CH(    0,1,k) = -CC(ido-1,k,1);
      CH(ido-1,0,k) =  CC(ido-1,k,0);
      }
  if (ido<=2) return;
Leo Singer's avatar
Leo Singer committed
85 86
  for (size_t k=0; k<l1; k++)
    for (size_t i=2; i<ido; i+=2)
87
      {
Leo Singer's avatar
Leo Singer committed
88 89
      size_t ic=ido-i;
      double ti2, tr2;
90 91 92 93 94 95 96 97 98 99 100 101
      MULPM (tr2,ti2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
      PM (CH(i-1,0,k),CH(ic-1,1,k),CC(i-1,k,0),tr2)
      PM (CH(i  ,0,k),CH(ic  ,1,k),ti2,CC(i  ,k,0))
      }
  }

static void radf3(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=3;
  static const double taur=-0.5, taui=0.86602540378443864676;

Leo Singer's avatar
Leo Singer committed
102
  for (size_t k=0; k<l1; k++)
103
    {
Leo Singer's avatar
Leo Singer committed
104
    double cr2=CC(0,k,1)+CC(0,k,2);
105 106 107 108 109
    CH(0,0,k) = CC(0,k,0)+cr2;
    CH(0,2,k) = taui*(CC(0,k,2)-CC(0,k,1));
    CH(ido-1,1,k) = CC(0,k,0)+taur*cr2;
    }
  if (ido==1) return;
Leo Singer's avatar
Leo Singer committed
110 111
  for (size_t k=0; k<l1; k++)
    for (size_t i=2; i<ido; i+=2)
112
      {
Leo Singer's avatar
Leo Singer committed
113 114
      size_t ic=ido-i;
      double di2, di3, dr2, dr3;
115 116
      MULPM (dr2,di2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
      MULPM (dr3,di3,WA(1,i-2),WA(1,i-1),CC(i-1,k,2),CC(i,k,2))
Leo Singer's avatar
Leo Singer committed
117 118
      double cr2=dr2+dr3;
      double ci2=di2+di3;
119 120
      CH(i-1,0,k) = CC(i-1,k,0)+cr2;
      CH(i  ,0,k) = CC(i  ,k,0)+ci2;
Leo Singer's avatar
Leo Singer committed
121 122 123 124
      double tr2 = CC(i-1,k,0)+taur*cr2;
      double ti2 = CC(i  ,k,0)+taur*ci2;
      double tr3 = taui*(di2-di3);
      double ti3 = taui*(dr3-dr2);
125 126 127 128 129 130 131 132 133 134 135 136
      PM(CH(i-1,2,k),CH(ic-1,1,k),tr2,tr3)
      PM(CH(i  ,2,k),CH(ic  ,1,k),ti3,ti2)
      }
  }

static void radf4(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=4;
  static const double hsqt2=0.70710678118654752440;
  double ci2, ci3, ci4, cr2, cr3, cr4, ti1, ti2, ti3, ti4, tr1, tr2, tr3, tr4;

Leo Singer's avatar
Leo Singer committed
137
  for (size_t k=0; k<l1; k++)
138 139 140 141 142 143
    {
    PM (tr1,CH(0,2,k),CC(0,k,3),CC(0,k,1))
    PM (tr2,CH(ido-1,1,k),CC(0,k,0),CC(0,k,2))
    PM (CH(0,0,k),CH(ido-1,3,k),tr2,tr1)
    }
  if ((ido&1)==0)
Leo Singer's avatar
Leo Singer committed
144
    for (size_t k=0; k<l1; k++)
145 146 147 148 149 150 151
      {
      ti1=-hsqt2*(CC(ido-1,k,1)+CC(ido-1,k,3));
      tr1= hsqt2*(CC(ido-1,k,1)-CC(ido-1,k,3));
      PM (CH(ido-1,0,k),CH(ido-1,2,k),CC(ido-1,k,0),tr1)
      PM (CH(    0,3,k),CH(    0,1,k),ti1,CC(ido-1,k,2))
      }
  if (ido<=2) return;
Leo Singer's avatar
Leo Singer committed
152 153
  for (size_t k=0; k<l1; k++)
    for (size_t i=2; i<ido; i+=2)
154
      {
Leo Singer's avatar
Leo Singer committed
155
      size_t ic=ido-i;
156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178
      MULPM(cr2,ci2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
      MULPM(cr3,ci3,WA(1,i-2),WA(1,i-1),CC(i-1,k,2),CC(i,k,2))
      MULPM(cr4,ci4,WA(2,i-2),WA(2,i-1),CC(i-1,k,3),CC(i,k,3))
      PM(tr1,tr4,cr4,cr2)
      PM(ti1,ti4,ci2,ci4)
      PM(tr2,tr3,CC(i-1,k,0),cr3)
      PM(ti2,ti3,CC(i  ,k,0),ci3)
      PM(CH(i-1,0,k),CH(ic-1,3,k),tr2,tr1)
      PM(CH(i  ,0,k),CH(ic  ,3,k),ti1,ti2)
      PM(CH(i-1,2,k),CH(ic-1,1,k),tr3,ti4)
      PM(CH(i  ,2,k),CH(ic  ,1,k),tr4,ti3)
      }
  }

static void radf5(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=5;
  static const double tr11= 0.3090169943749474241, ti11=0.95105651629515357212,
                      tr12=-0.8090169943749474241, ti12=0.58778525229247312917;
  double ci2, di2, ci4, ci5, di3, di4, di5, ci3, cr2, cr3, dr2, dr3,
         dr4, dr5, cr5, cr4, ti2, ti3, ti5, ti4, tr2, tr3, tr4, tr5;

Leo Singer's avatar
Leo Singer committed
179
  for (size_t k=0; k<l1; k++)
180 181 182 183 184 185 186 187 188 189
    {
    PM (cr2,ci5,CC(0,k,4),CC(0,k,1))
    PM (cr3,ci4,CC(0,k,3),CC(0,k,2))
    CH(0,0,k)=CC(0,k,0)+cr2+cr3;
    CH(ido-1,1,k)=CC(0,k,0)+tr11*cr2+tr12*cr3;
    CH(0,2,k)=ti11*ci5+ti12*ci4;
    CH(ido-1,3,k)=CC(0,k,0)+tr12*cr2+tr11*cr3;
    CH(0,4,k)=ti12*ci5-ti11*ci4;
    }
  if (ido==1) return;
Leo Singer's avatar
Leo Singer committed
190 191
  for (size_t k=0; k<l1;++k)
    for (size_t i=2; i<ido; i+=2)
192
      {
Leo Singer's avatar
Leo Singer committed
193
      size_t ic=ido-i;
194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228
      MULPM (dr2,di2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
      MULPM (dr3,di3,WA(1,i-2),WA(1,i-1),CC(i-1,k,2),CC(i,k,2))
      MULPM (dr4,di4,WA(2,i-2),WA(2,i-1),CC(i-1,k,3),CC(i,k,3))
      MULPM (dr5,di5,WA(3,i-2),WA(3,i-1),CC(i-1,k,4),CC(i,k,4))
      PM(cr2,ci5,dr5,dr2)
      PM(ci2,cr5,di2,di5)
      PM(cr3,ci4,dr4,dr3)
      PM(ci3,cr4,di3,di4)
      CH(i-1,0,k)=CC(i-1,k,0)+cr2+cr3;
      CH(i  ,0,k)=CC(i  ,k,0)+ci2+ci3;
      tr2=CC(i-1,k,0)+tr11*cr2+tr12*cr3;
      ti2=CC(i  ,k,0)+tr11*ci2+tr12*ci3;
      tr3=CC(i-1,k,0)+tr12*cr2+tr11*cr3;
      ti3=CC(i  ,k,0)+tr12*ci2+tr11*ci3;
      MULPM(tr5,tr4,cr5,cr4,ti11,ti12)
      MULPM(ti5,ti4,ci5,ci4,ti11,ti12)
      PM(CH(i-1,2,k),CH(ic-1,1,k),tr2,tr5)
      PM(CH(i  ,2,k),CH(ic  ,1,k),ti5,ti2)
      PM(CH(i-1,4,k),CH(ic-1,3,k),tr3,tr4)
      PM(CH(i  ,4,k),CH(ic  ,3,k),ti4,ti3)
      }
  }

#undef CH
#undef CC
#define CH(a,b,c) ch[(a)+ido*((b)+l1*(c))]
#define CC(a,b,c) cc[(a)+ido*((b)+cdim*(c))]
#define C1(a,b,c) cc[(a)+ido*((b)+l1*(c))]
#define C2(a,b) cc[(a)+idl1*(b)]
#define CH2(a,b) ch[(a)+idl1*(b)]
static void radfg(size_t ido, size_t ip, size_t l1, size_t idl1,
  double *cc, double *ch, const double *wa)
  {
  const size_t cdim=ip;

Leo Singer's avatar
Leo Singer committed
229
  size_t ipph=(ip+1)/ 2;
230 231 232 233
  if(ido!=1)
    {
    memcpy(ch,cc,idl1*sizeof(double));

Leo Singer's avatar
Leo Singer committed
234 235
    for(size_t j=1; j<ip; j++)
      for(size_t k=0; k<l1; k++)
236 237
        {
        CH(0,k,j)=C1(0,k,j);
Leo Singer's avatar
Leo Singer committed
238 239
        size_t idij=(j-1)*ido+1;
        for(size_t i=2; i<ido; i+=2,idij+=2)
240 241 242
          MULPM(CH(i-1,k,j),CH(i,k,j),wa[idij-1],wa[idij],C1(i-1,k,j),C1(i,k,j))
        }

Leo Singer's avatar
Leo Singer committed
243 244 245
    for(size_t j=1,jc=ip-1; j<ipph; j++,jc--)
      for(size_t k=0; k<l1; k++)
        for(size_t i=2; i<ido; i+=2)
246 247 248 249 250 251 252 253
          {
          PM(C1(i-1,k,j),C1(i  ,k,jc),CH(i-1,k,jc),CH(i-1,k,j ))
          PM(C1(i  ,k,j),C1(i-1,k,jc),CH(i  ,k,j ),CH(i  ,k,jc))
          }
    }
  else
    memcpy(cc,ch,idl1*sizeof(double));

Leo Singer's avatar
Leo Singer committed
254 255
  for(size_t j=1,jc=ip-1; j<ipph; j++,jc--)
    for(size_t k=0; k<l1; k++)
256 257
      PM(C1(0,k,j),C1(0,k,jc),CH(0,k,jc),CH(0,k,j))

Leo Singer's avatar
Leo Singer committed
258 259 260 261
  double *csarr=RALLOC(double,2*ip);
  sincos_2pibyn(ip, ip, &csarr[1], &csarr[0], 2);

  for(size_t l=1,lc=ip-1; l<ipph; l++,lc--)
262
    {
Leo Singer's avatar
Leo Singer committed
263 264 265
    double ar1=csarr[2*l];
    double ai1=csarr[2*l+1];
    for(size_t ik=0; ik<idl1; ik++)
266 267 268 269
      {
      CH2(ik,l)=C2(ik,0)+ar1*C2(ik,1);
      CH2(ik,lc)=ai1*C2(ik,ip-1);
      }
Leo Singer's avatar
Leo Singer committed
270 271
    size_t aidx=2*l;
    for(size_t j=2,jc=ip-2; j<ipph; j++,jc--)
272 273 274
      {
      aidx+=2*l;
      if (aidx>=2*ip) aidx-=2*ip;
Leo Singer's avatar
Leo Singer committed
275 276 277
      double ar2=csarr[aidx];
      double ai2=csarr[aidx+1];
      for(size_t ik=0; ik<idl1; ik++)
278 279 280 281 282 283 284 285
        {
        CH2(ik,l )+=ar2*C2(ik,j );
        CH2(ik,lc)+=ai2*C2(ik,jc);
        }
      }
    }
  DEALLOC(csarr);

Leo Singer's avatar
Leo Singer committed
286 287
  for(size_t j=1; j<ipph; j++)
    for(size_t ik=0; ik<idl1; ik++)
288 289
      CH2(ik,0)+=C2(ik,j);

Leo Singer's avatar
Leo Singer committed
290
  for(size_t k=0; k<l1; k++)
291
    memcpy(&CC(0,0,k),&CH(0,k,0),ido*sizeof(double));
Leo Singer's avatar
Leo Singer committed
292
  for(size_t j=1; j<ipph; j++)
293
    {
Leo Singer's avatar
Leo Singer committed
294 295 296
    size_t jc=ip-j;
    size_t j2=2*j;
    for(size_t k=0; k<l1; k++)
297 298 299 300 301 302 303
      {
      CC(ido-1,j2-1,k) = CH(0,k,j );
      CC(0    ,j2  ,k) = CH(0,k,jc);
      }
    }
  if(ido==1) return;

Leo Singer's avatar
Leo Singer committed
304
  for(size_t j=1; j<ipph; j++)
305
    {
Leo Singer's avatar
Leo Singer committed
306 307 308 309
    size_t jc=ip-j;
    size_t j2=2*j;
    for(size_t k=0; k<l1; k++)
      for(size_t i=2; i<ido; i+=2)
310
        {
Leo Singer's avatar
Leo Singer committed
311
        size_t ic=ido-i;
312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327
        PM (CC(i-1,j2,k),CC(ic-1,j2-1,k),CH(i-1,k,j ),CH(i-1,k,jc))
        PM (CC(i  ,j2,k),CC(ic  ,j2-1,k),CH(i  ,k,jc),CH(i  ,k,j ))
        }
    }
  }

#undef CC
#undef CH
#define CH(a,b,c) ch[(a)+ido*((b)+l1*(c))]
#define CC(a,b,c) cc[(a)+ido*((b)+cdim*(c))]

static void radb2(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=2;

Leo Singer's avatar
Leo Singer committed
328
  for (size_t k=0; k<l1; k++)
329 330
    PM (CH(0,k,0),CH(0,k,1),CC(0,0,k),CC(ido-1,1,k))
  if ((ido&1)==0)
Leo Singer's avatar
Leo Singer committed
331
    for (size_t k=0; k<l1; k++)
332 333 334 335 336
      {
      CH(ido-1,k,0) =  2*CC(ido-1,0,k);
      CH(ido-1,k,1) = -2*CC(0    ,1,k);
      }
  if (ido<=2) return;
Leo Singer's avatar
Leo Singer committed
337 338
  for (size_t k=0; k<l1;++k)
    for (size_t i=2; i<ido; i+=2)
339
      {
Leo Singer's avatar
Leo Singer committed
340 341
      size_t ic=ido-i;
      double ti2, tr2;
342 343 344 345 346 347 348 349 350 351 352 353 354
      PM (CH(i-1,k,0),tr2,CC(i-1,0,k),CC(ic-1,1,k))
      PM (ti2,CH(i  ,k,0),CC(i  ,0,k),CC(ic  ,1,k))
      MULPM (CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),ti2,tr2)
      }
  }

static void radb3(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=3;
  static const double taur=-0.5, taui=0.86602540378443864676;
  double ci2, ci3, di2, di3, cr2, cr3, dr2, dr3, ti2, tr2;

Leo Singer's avatar
Leo Singer committed
355
  for (size_t k=0; k<l1; k++)
356 357 358 359 360 361 362 363
    {
    tr2=2*CC(ido-1,1,k);
    cr2=CC(0,0,k)+taur*tr2;
    CH(0,k,0)=CC(0,0,k)+tr2;
    ci3=2*taui*CC(0,2,k);
    PM (CH(0,k,2),CH(0,k,1),cr2,ci3);
    }
  if (ido==1) return;
Leo Singer's avatar
Leo Singer committed
364 365
  for (size_t k=0; k<l1; k++)
    for (size_t i=2; i<ido; i+=2)
366
      {
Leo Singer's avatar
Leo Singer committed
367
      size_t ic=ido-i;
368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389
      tr2=CC(i-1,2,k)+CC(ic-1,1,k);
      ti2=CC(i  ,2,k)-CC(ic  ,1,k);
      cr2=CC(i-1,0,k)+taur*tr2;
      ci2=CC(i  ,0,k)+taur*ti2;
      CH(i-1,k,0)=CC(i-1,0,k)+tr2;
      CH(i  ,k,0)=CC(i  ,0,k)+ti2;
      cr3=taui*(CC(i-1,2,k)-CC(ic-1,1,k));
      ci3=taui*(CC(i  ,2,k)+CC(ic  ,1,k));
      PM(dr3,dr2,cr2,ci3)
      PM(di2,di3,ci2,cr3)
      MULPM(CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),di2,dr2)
      MULPM(CH(i,k,2),CH(i-1,k,2),WA(1,i-2),WA(1,i-1),di3,dr3)
      }
  }

static void radb4(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=4;
  static const double sqrt2=1.41421356237309504880;
  double ci2, ci3, ci4, cr2, cr3, cr4, ti1, ti2, ti3, ti4, tr1, tr2, tr3, tr4;

Leo Singer's avatar
Leo Singer committed
390
  for (size_t k=0; k<l1; k++)
391 392 393 394 395 396 397 398
    {
    PM (tr2,tr1,CC(0,0,k),CC(ido-1,3,k))
    tr3=2*CC(ido-1,1,k);
    tr4=2*CC(0,2,k);
    PM (CH(0,k,0),CH(0,k,2),tr2,tr3)
    PM (CH(0,k,3),CH(0,k,1),tr1,tr4)
    }
  if ((ido&1)==0)
Leo Singer's avatar
Leo Singer committed
399
    for (size_t k=0; k<l1; k++)
400 401 402 403 404 405 406 407 408
      {
      PM (ti1,ti2,CC(0    ,3,k),CC(0    ,1,k))
      PM (tr2,tr1,CC(ido-1,0,k),CC(ido-1,2,k))
      CH(ido-1,k,0)=tr2+tr2;
      CH(ido-1,k,1)=sqrt2*(tr1-ti1);
      CH(ido-1,k,2)=ti2+ti2;
      CH(ido-1,k,3)=-sqrt2*(tr1+ti1);
      }
  if (ido<=2) return;
Leo Singer's avatar
Leo Singer committed
409 410
  for (size_t k=0; k<l1;++k)
    for (size_t i=2; i<ido; i+=2)
411
      {
Leo Singer's avatar
Leo Singer committed
412
      size_t ic=ido-i;
413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435
      PM (tr2,tr1,CC(i-1,0,k),CC(ic-1,3,k))
      PM (ti1,ti2,CC(i  ,0,k),CC(ic  ,3,k))
      PM (tr4,ti3,CC(i  ,2,k),CC(ic  ,1,k))
      PM (tr3,ti4,CC(i-1,2,k),CC(ic-1,1,k))
      PM (CH(i-1,k,0),cr3,tr2,tr3)
      PM (CH(i  ,k,0),ci3,ti2,ti3)
      PM (cr4,cr2,tr1,tr4)
      PM (ci2,ci4,ti1,ti4)
      MULPM (CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),ci2,cr2)
      MULPM (CH(i,k,2),CH(i-1,k,2),WA(1,i-2),WA(1,i-1),ci3,cr3)
      MULPM (CH(i,k,3),CH(i-1,k,3),WA(2,i-2),WA(2,i-1),ci4,cr4)
      }
  }

static void radb5(size_t ido, size_t l1, const double *cc, double *ch,
  const double *wa)
  {
  const size_t cdim=5;
  static const double tr11= 0.3090169943749474241, ti11=0.95105651629515357212,
                      tr12=-0.8090169943749474241, ti12=0.58778525229247312917;
  double ci2, ci3, ci4, ci5, di3, di4, di5, di2, cr2, cr3, cr5, cr4,
         ti2, ti3, ti4, ti5, dr3, dr4, dr5, dr2, tr2, tr3, tr4, tr5;

Leo Singer's avatar
Leo Singer committed
436
  for (size_t k=0; k<l1; k++)
437 438 439 440 441 442 443 444 445 446 447 448 449
    {
    ti5=2*CC(0,2,k);
    ti4=2*CC(0,4,k);
    tr2=2*CC(ido-1,1,k);
    tr3=2*CC(ido-1,3,k);
    CH(0,k,0)=CC(0,0,k)+tr2+tr3;
    cr2=CC(0,0,k)+tr11*tr2+tr12*tr3;
    cr3=CC(0,0,k)+tr12*tr2+tr11*tr3;
    MULPM(ci5,ci4,ti5,ti4,ti11,ti12)
    PM(CH(0,k,4),CH(0,k,1),cr2,ci5)
    PM(CH(0,k,3),CH(0,k,2),cr3,ci4)
    }
  if (ido==1) return;
Leo Singer's avatar
Leo Singer committed
450 451
  for (size_t k=0; k<l1;++k)
    for (size_t i=2; i<ido; i+=2)
452
      {
Leo Singer's avatar
Leo Singer committed
453
      size_t ic=ido-i;
454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480
      PM(tr2,tr5,CC(i-1,2,k),CC(ic-1,1,k))
      PM(ti5,ti2,CC(i  ,2,k),CC(ic  ,1,k))
      PM(tr3,tr4,CC(i-1,4,k),CC(ic-1,3,k))
      PM(ti4,ti3,CC(i  ,4,k),CC(ic  ,3,k))
      CH(i-1,k,0)=CC(i-1,0,k)+tr2+tr3;
      CH(i  ,k,0)=CC(i  ,0,k)+ti2+ti3;
      cr2=CC(i-1,0,k)+tr11*tr2+tr12*tr3;
      ci2=CC(i  ,0,k)+tr11*ti2+tr12*ti3;
      cr3=CC(i-1,0,k)+tr12*tr2+tr11*tr3;
      ci3=CC(i  ,0,k)+tr12*ti2+tr11*ti3;
      MULPM(cr5,cr4,tr5,tr4,ti11,ti12)
      MULPM(ci5,ci4,ti5,ti4,ti11,ti12)
      PM(dr4,dr3,cr3,ci4)
      PM(di3,di4,ci3,cr4)
      PM(dr5,dr2,cr2,ci5)
      PM(di2,di5,ci2,cr5)
      MULPM(CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),di2,dr2)
      MULPM(CH(i,k,2),CH(i-1,k,2),WA(1,i-2),WA(1,i-1),di3,dr3)
      MULPM(CH(i,k,3),CH(i-1,k,3),WA(2,i-2),WA(2,i-1),di4,dr4)
      MULPM(CH(i,k,4),CH(i-1,k,4),WA(3,i-2),WA(3,i-1),di5,dr5)
      }
  }

static void radbg(size_t ido, size_t ip, size_t l1, size_t idl1,
  double *cc, double *ch, const double *wa)
  {
  const size_t cdim=ip;
Leo Singer's avatar
Leo Singer committed
481 482 483

  size_t ipph=(ip+1)/ 2;
  for(size_t k=0; k<l1; k++)
484
    memcpy(&CH(0,k,0),&CC(0,0,k),ido*sizeof(double));
Leo Singer's avatar
Leo Singer committed
485
  for(size_t j=1; j<ipph; j++)
486
    {
Leo Singer's avatar
Leo Singer committed
487 488 489
    size_t jc=ip-j;
    size_t j2=2*j;
    for(size_t k=0; k<l1; k++)
490 491 492 493 494 495 496
      {
      CH(0,k,j )=2*CC(ido-1,j2-1,k);
      CH(0,k,jc)=2*CC(0    ,j2  ,k);
      }
    }

  if(ido!=1)
Leo Singer's avatar
Leo Singer committed
497 498 499
    for(size_t j=1,jc=ip-1; j<ipph; j++,jc--)
      for(size_t k=0; k<l1; k++)
        for(size_t i=2; i<ido; i+=2)
500
          {
Leo Singer's avatar
Leo Singer committed
501
          size_t ic=ido-i;
502 503 504 505
          PM (CH(i-1,k,j ),CH(i-1,k,jc),CC(i-1,2*j,k),CC(ic-1,2*j-1,k))
          PM (CH(i  ,k,jc),CH(i  ,k,j ),CC(i  ,2*j,k),CC(ic  ,2*j-1,k))
          }

Leo Singer's avatar
Leo Singer committed
506 507 508 509
  double *csarr=RALLOC(double,2*ip);
  sincos_2pibyn(ip, ip, &csarr[1], &csarr[0], 2);

  for(size_t l=1; l<ipph; l++)
510
    {
Leo Singer's avatar
Leo Singer committed
511 512 513 514
    size_t lc=ip-l;
    double ar1=csarr[2*l];
    double ai1=csarr[2*l+1];
    for(size_t ik=0; ik<idl1; ik++)
515 516 517 518
      {
      C2(ik,l)=CH2(ik,0)+ar1*CH2(ik,1);
      C2(ik,lc)=ai1*CH2(ik,ip-1);
      }
Leo Singer's avatar
Leo Singer committed
519 520
    size_t aidx=2*l;
    for(size_t j=2; j<ipph; j++)
521
      {
Leo Singer's avatar
Leo Singer committed
522
      size_t jc=ip-j;
523 524
      aidx+=2*l;
      if (aidx>=2*ip) aidx-=2*ip;
Leo Singer's avatar
Leo Singer committed
525 526 527
      double ar2=csarr[aidx];
      double ai2=csarr[aidx+1];
      for(size_t ik=0; ik<idl1; ik++)
528 529 530 531 532 533 534 535
        {
        C2(ik,l )+=ar2*CH2(ik,j );
        C2(ik,lc)+=ai2*CH2(ik,jc);
        }
      }
    }
  DEALLOC(csarr);

Leo Singer's avatar
Leo Singer committed
536 537
  for(size_t j=1; j<ipph; j++)
    for(size_t ik=0; ik<idl1; ik++)
538 539
      CH2(ik,0)+=CH2(ik,j);

Leo Singer's avatar
Leo Singer committed
540 541
  for(size_t j=1,jc=ip-1; j<ipph; j++,jc--)
    for(size_t k=0; k<l1; k++)
542 543 544 545
      PM (CH(0,k,jc),CH(0,k,j),C1(0,k,j),C1(0,k,jc))

  if(ido==1)
    return;
Leo Singer's avatar
Leo Singer committed
546 547 548
  for(size_t j=1,jc=ip-1; j<ipph; j++,jc--)
    for(size_t k=0; k<l1; k++)
      for(size_t i=2; i<ido; i+=2)
549 550 551 552 553 554
        {
        PM (CH(i-1,k,jc),CH(i-1,k,j ),C1(i-1,k,j),C1(i  ,k,jc))
        PM (CH(i  ,k,j ),CH(i  ,k,jc),C1(i  ,k,j),C1(i-1,k,jc))
        }
  memcpy(cc,ch,idl1*sizeof(double));

Leo Singer's avatar
Leo Singer committed
555 556
  for(size_t j=1; j<ip; j++)
    for(size_t k=0; k<l1; k++)
557 558
      {
      C1(0,k,j)=CH(0,k,j);
Leo Singer's avatar
Leo Singer committed
559 560
      size_t idij=(j-1)*ido+1;
      for(size_t i=2; i<ido; i+=2,idij+=2)
561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577
        MULPM (C1(i,k,j),C1(i-1,k,j),wa[idij-1],wa[idij],CH(i,k,j),CH(i-1,k,j))
      }
  }

#undef CC
#undef CH
#undef PM
#undef MULPM


/*----------------------------------------------------------------------
   cfftf1, cfftb1, cfftf, cfftb, cffti1, cffti. Complex FFTs.
  ----------------------------------------------------------------------*/

static void cfft1(size_t n, cmplx c[], cmplx ch[], const cmplx wa[],
  const size_t ifac[], int isign)
  {
Leo Singer's avatar
Leo Singer committed
578
  size_t l1=1, nf=ifac[1], iw=0;
579 580
  cmplx *p1=c, *p2=ch;

Leo Singer's avatar
Leo Singer committed
581
  for(size_t k1=0; k1<nf; k1++)
582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627
    {
    size_t ip=ifac[k1+2];
    size_t l2=ip*l1;
    size_t ido = n/l2;
    if(ip==4)
      (isign>0) ? passb4(ido, l1, p1, p2, wa+iw)
                : passf4(ido, l1, p1, p2, wa+iw);
    else if(ip==2)
      (isign>0) ? passb2(ido, l1, p1, p2, wa+iw)
                : passf2(ido, l1, p1, p2, wa+iw);
    else if(ip==3)
      (isign>0) ? passb3(ido, l1, p1, p2, wa+iw)
                : passf3(ido, l1, p1, p2, wa+iw);
    else if(ip==5)
      (isign>0) ? passb5(ido, l1, p1, p2, wa+iw)
                : passf5(ido, l1, p1, p2, wa+iw);
    else if(ip==6)
      (isign>0) ? passb6(ido, l1, p1, p2, wa+iw)
                : passf6(ido, l1, p1, p2, wa+iw);
    else
      (isign>0) ? passbg(ido, ip, l1, p1, p2, wa+iw)
                : passfg(ido, ip, l1, p1, p2, wa+iw);
    SWAP(p1,p2,cmplx *);
    l1=l2;
    iw+=(ip-1)*ido;
    }
  if (p1!=c)
    memcpy (c,p1,n*sizeof(cmplx));
  }

void cfftf(size_t n, double c[], double wsave[])
  {
  if (n!=1)
    cfft1(n, (cmplx*)c, (cmplx*)wsave, (cmplx*)(wsave+2*n),
          (size_t*)(wsave+4*n),-1);
  }

void cfftb(size_t n, double c[], double wsave[])
  {
  if (n!=1)
    cfft1(n, (cmplx*)c, (cmplx*)wsave, (cmplx*)(wsave+2*n),
          (size_t*)(wsave+4*n),+1);
  }

static void factorize (size_t n, const size_t *pf, size_t npf, size_t *ifac)
  {
Leo Singer's avatar
Leo Singer committed
628
  size_t nl=n, nf=0, ntry=0, j=0;
629 630 631 632 633 634 635 636 637 638 639 640 641 642 643

startloop:
  j++;
  ntry = (j<=npf) ? pf[j-1] : ntry+2;
  do
    {
    size_t nq=nl / ntry;
    size_t nr=nl-ntry*nq;
    if (nr!=0)
      goto startloop;
    nf++;
    ifac[nf+1]=ntry;
    nl=nq;
    if ((ntry==2) && (nf!=1))
      {
Leo Singer's avatar
Leo Singer committed
644
      for (size_t i=nf+1; i>2; --i)
645 646 647 648 649 650 651 652 653 654 655 656 657 658 659
        ifac[i]=ifac[i-1];
      ifac[2]=2;
      }
    }
  while(nl!=1);
  ifac[0]=n;
  ifac[1]=nf;
  }

static void cffti1(size_t n, double wa[], size_t ifac[])
  {
  static const size_t ntryh[5]={4,6,3,2,5};

  size_t i=0, l1=1;
  factorize (n,ntryh,5,ifac);
Leo Singer's avatar
Leo Singer committed
660 661 662
  triggen tg;
  triggen_init (&tg,n);
  for(size_t k=1; k<=ifac[1]; k++)
663 664 665
    {
    size_t ip=ifac[k+1];
    size_t ido=n/(l1*ip);
Leo Singer's avatar
Leo Singer committed
666
    for(size_t j=1; j<ip; j++)
667
      {
Leo Singer's avatar
Leo Singer committed
668 669 670
      for (size_t l=0; l<ido+1; ++l)
        triggen_get (&tg,j*l1*l,&wa[i+2*l+1],&wa[i+2*l]);

671 672
      if(ip>6)
        {
Leo Singer's avatar
Leo Singer committed
673 674
        wa[i  ]=wa[i+2*ido  ];
        wa[i+1]=wa[i+2*ido+1];
675
        }
Leo Singer's avatar
Leo Singer committed
676
      i+=2*ido;
677 678 679
      }
    l1*=ip;
    }
Leo Singer's avatar
Leo Singer committed
680
  triggen_destroy(&tg);
681 682 683 684 685 686 687 688 689 690 691 692 693
  }

void cffti(size_t n, double wsave[])
  { if (n!=1) cffti1(n, wsave+2*n,(size_t*)(wsave+4*n)); }


/*----------------------------------------------------------------------
   rfftf1, rfftb1, rfftf, rfftb, rffti1, rffti. Real FFTs.
  ----------------------------------------------------------------------*/

static void rfftf1(size_t n, double c[], double ch[], const double wa[],
  const size_t ifac[])
  {
Leo Singer's avatar
Leo Singer committed
694
  size_t l1=n, nf=ifac[1], iw=n-1;
695 696
  double *p1=ch, *p2=c;

Leo Singer's avatar
Leo Singer committed
697
  for(size_t k1=1; k1<=nf;++k1)
698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726
    {
    size_t ip=ifac[nf-k1+2];
    size_t ido=n / l1;
    l1 /= ip;
    iw-=(ip-1)*ido;
    SWAP (p1,p2,double *);
    if(ip==4)
      radf4(ido, l1, p1, p2, wa+iw);
    else if(ip==2)
      radf2(ido, l1, p1, p2, wa+iw);
    else if(ip==3)
      radf3(ido, l1, p1, p2, wa+iw);
    else if(ip==5)
      radf5(ido, l1, p1, p2, wa+iw);
    else
      {
      if (ido==1)
        SWAP (p1,p2,double *);
      radfg(ido, ip, l1, ido*l1, p1, p2, wa+iw);
      SWAP (p1,p2,double *);
      }
    }
  if (p1==c)
    memcpy (c,ch,n*sizeof(double));
  }

static void rfftb1(size_t n, double c[], double ch[], const double wa[],
  const size_t ifac[])
  {
Leo Singer's avatar
Leo Singer committed
727
  size_t l1=1, nf=ifac[1], iw=0;
728 729
  double *p1=c, *p2=ch;

Leo Singer's avatar
Leo Singer committed
730
  for(size_t k1=1; k1<=nf; k1++)
731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765
    {
    size_t ip = ifac[k1+1],
           ido= n/(ip*l1);
    if(ip==4)
      radb4(ido, l1, p1, p2, wa+iw);
    else if(ip==2)
      radb2(ido, l1, p1, p2, wa+iw);
    else if(ip==3)
      radb3(ido, l1, p1, p2, wa+iw);
    else if(ip==5)
      radb5(ido, l1, p1, p2, wa+iw);
    else
      {
      radbg(ido, ip, l1, ido*l1, p1, p2, wa+iw);
      if (ido!=1)
        SWAP (p1,p2,double *);
      }
    SWAP (p1,p2,double *);
    l1*=ip;
    iw+=(ip-1)*ido;
    }
  if (p1!=c)
    memcpy (c,ch,n*sizeof(double));
  }

void rfftf(size_t n, double r[], double wsave[])
  { if(n!=1) rfftf1(n, r, wsave, wsave+n,(size_t*)(wsave+2*n)); }

void rfftb(size_t n, double r[], double wsave[])
  { if(n!=1) rfftb1(n, r, wsave, wsave+n,(size_t*)(wsave+2*n)); }

static void rffti1(size_t n, double wa[], size_t ifac[])
  {
  static const size_t ntryh[4]={4,2,3,5};

Leo Singer's avatar
Leo Singer committed
766 767
  triggen tg;
  triggen_init (&tg,n);
768 769
  size_t is=0, l1=1;
  factorize (n,ntryh,4,ifac);
Leo Singer's avatar
Leo Singer committed
770
  for (size_t k=1; k<ifac[1]; k++)
771 772 773
    {
    size_t ip=ifac[k+1],
           ido=n/(l1*ip);
Leo Singer's avatar
Leo Singer committed
774
    for (size_t j=1; j<ip; ++j)
775
      {
Leo Singer's avatar
Leo Singer committed
776 777
      for (size_t i=1; i<=(ido-1)/2; ++i)
        triggen_get(&tg,j*l1*i,&wa[is+2*i-1],&wa[is+2*i-2]);
778 779 780 781
      is+=ido;
      }
    l1*=ip;
    }
Leo Singer's avatar
Leo Singer committed
782
  triggen_destroy (&tg);
783 784 785 786
  }

void rffti(size_t n, double wsave[])
  { if (n!=1) rffti1(n, wsave+n,(size_t*)(wsave+2*n)); }