home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / numana01.zip / SRC / FOURIER.MOD < prev    next >
Text File  |  1996-08-16  |  20KB  |  522 lines
  1. IMPLEMENTATION MODULE Fourier;
  2.  
  3.         (********************************************************)
  4.         (*                                                      *)
  5.         (*              Fast Fourier Transform                  *)
  6.         (*                                                      *)
  7.         (*  Programmer:         P. Moylan                       *)
  8.         (*  Last edited:        31 July 1996                    *)
  9.         (*  Status:             Working, but I'd like to check  *)
  10.         (*              further variants, also get a better     *)
  11.         (*              scrambling algorithm.                   *)
  12.         (*                                                      *)
  13.         (*      Desirable additions:                            *)
  14.         (*       - special cases for real data.                 *)
  15.         (*       - convolutions.                                *)
  16.         (*                                                      *)
  17.         (********************************************************)
  18.  
  19. FROM SYSTEM IMPORT
  20.     (* proc *)  ADR;
  21.  
  22. FROM LongComplexMath IMPORT
  23.     (* proc *)  scalarMult, exp;
  24.  
  25. FROM MiscM2 IMPORT
  26.     (* const*)  PI,
  27.     (* proc *)  Error, Cos, Sin, Sqrt;
  28.  
  29. (************************************************************************)
  30. (*                      MISCELLANEOUS UTILITIES                         *)
  31. (*                                                                      *)
  32. (*  Not all of the next few procedures are used; they're there mainly   *)
  33. (*  as intermediate steps in my development work, and might be          *)
  34. (*  removed from a final version.                                       *)
  35. (*                                                                      *)
  36. (************************************************************************)
  37.  
  38. (*
  39. PROCEDURE Log2 (N: CARDINAL): CARDINAL;
  40.  
  41.     (* Logarithm to base 2 (rounded down to the next-lower integral     *)
  42.     (* value, in case N is not a power of 2.)  The result is in the     *)
  43.     (* range 0..31.                                                     *)
  44.  
  45.     VAR test, result: CARDINAL;
  46.  
  47.     BEGIN
  48.         test := MAX(CARDINAL) DIV 2 + 1;  result := 31;
  49.         WHILE test > N DO
  50.             DEC (result);  test := test DIV 2;
  51.         END (*WHILE*);
  52.         RETURN result;
  53.     END Log2;
  54. *)
  55. (************************************************************************)
  56. (*
  57. PROCEDURE BitRev (num, bits: CARDINAL): CARDINAL;
  58.  
  59.     (* Takes the last "bits" bits of "num", and reverses their order.   *)
  60.     (* The legal range of "bits" is 0..31.                              *)
  61.  
  62.     VAR j, result: CARDINAL;
  63.  
  64.     BEGIN
  65.         result := 0;
  66.         FOR j := 1 TO bits DO
  67.             result := 2*result;
  68.             IF ODD(num) THEN INC(result) END(*IF*);
  69.             num := num DIV 2;
  70.         END (*FOR*);
  71.         RETURN result;
  72.     END BitRev;
  73. *)
  74. (************************************************************************)
  75. (*
  76. PROCEDURE Scramble (N, m: CARDINAL;  VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  77.  
  78.     (* Re-sorts a data array of size N = 2^m into bit-reversed order,   *)
  79.     (* i.e. for all j data[j] is swapped with data[BitRev(j,m)].        *)
  80.  
  81.     (* For the moment this is a "proof of concept" version which uses   *)
  82.     (* a crude method for doing the scrambling.  It works, but I've     *)
  83.     (* found a faster method - see Shuffle below.                       *)
  84.  
  85.     VAR j, newpos: CARDINAL;  temp: LONGCOMPLEX;
  86.  
  87.     BEGIN
  88.         FOR j := 1 TO N-1 DO
  89.             newpos := BitRev (j, m);
  90.             IF newpos > j THEN
  91.                 temp := data[j];  data[j] := data[newpos];
  92.                 data[newpos] := temp;
  93.             END (*IF*);
  94.         END (*FOR*);
  95.     END Scramble;
  96. *)
  97. (************************************************************************)
  98. (*
  99. PROCEDURE W(r, N: CARDINAL): LONGCOMPLEX;
  100.  
  101.     (* Computes W^r as needed for FFT calculations.             *)
  102.     (* W = exp(-2*pi*i/N), therefore W^r = exp(-2*pi*i*r/N)     *)
  103.     (* W^r = cos(t) - i sin(t), where t = 2*pi*r/N.             *)
  104.  
  105.     (* This procedure is not at present being used, but is here for     *)
  106.     (* new development work.                                            *)
  107.  
  108.     CONST TwoPi = 2.0*PI;
  109.  
  110.     VAR theta: LONGREAL;
  111.  
  112.     BEGIN
  113.         theta := TwoPi*VAL(LONGREAL,r)/VAL(LONGREAL,N);
  114.         RETURN CMPLX(Cos(theta), -Sin(theta));
  115.     END W;
  116. *)
  117. (************************************************************************)
  118. (*                      THE SHUFFLING ALGORITHM                         *)
  119. (*                                                                      *)
  120. (*  The following two procedures work together to put an array into     *)
  121. (*  "bit-reversed" order: for all j, data[j] is swapped with data[k],   *)
  122. (*  where k is obtained by writing j as a binary number and then        *)
  123. (*  reversing the order of its bits.  The method we use here is         *)
  124. (*  admittedly rather obscure - it was derived by a succession of       *)
  125. (*  program transformations starting from a more readable but highly    *)
  126. (*  recursive algorithm - but it has a significant speed advantage      *)
  127. (*  over more obvious ways of doing the job.                            *)
  128. (*                                                                      *)
  129. (************************************************************************)
  130.  
  131. PROCEDURE Commit (start, step, size: CARDINAL;
  132.                                 VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  133.  
  134.     (* Swaps all elements in seq with their bit-reversed partners.      *)
  135.     (* The difference between this procedure and Shuffle is that here   *)
  136.     (* we are sure that every element in seq will be involved in a      *)
  137.     (* swap, whereas in Shuffle we are dealing with a sequence where    *)
  138.     (* some elements will stay in place.                                *)
  139.  
  140.     (* The sequence seq is defined to be the sequence of size elements  *)
  141.     (* whose first element is at location (start + step DIV 2), and     *)
  142.     (* subsequent elements occur at subscript increments of step.       *)
  143.  
  144.     VAR N, destination, depth, oldsize: CARDINAL;
  145.         Stack: ARRAY [1..12] OF CARDINAL;
  146.         temp: LONGCOMPLEX;
  147.  
  148.     BEGIN
  149.         N := step*size;
  150.         destination := start + N;
  151.         INC (start, step DIV 2);
  152.         depth := 0;
  153.  
  154.         LOOP
  155.             (* Swap one pair of elements. *)
  156.  
  157.             temp := data[start];
  158.             data[start] := data[destination];
  159.             data[destination] := temp;
  160.  
  161.             (* Find the next subsequence to work on. *)
  162.  
  163.             INC (depth);
  164.             oldsize := size;
  165.             N := N DIV size;
  166.             step := step*size;
  167.             size := 1;
  168.  
  169.             LOOP
  170.                 IF oldsize > 1 THEN
  171.                     EXIT (*LOOP*);
  172.                 END(*IF*);
  173.  
  174.                 DEC (depth);
  175.                 IF depth = 0 THEN
  176.                     RETURN;
  177.                 END (*IF*);
  178.  
  179.                 oldsize := Stack[depth];
  180.                 DEC (destination, N);
  181.                 step := step DIV 2;
  182.                 DEC (start, step);
  183.                 N := 2*N;
  184.                 size := 2*size;
  185.  
  186.             END (*LOOP*);
  187.  
  188.             INC (start, step DIV 2);
  189.             INC (destination, N);
  190.             Stack[depth] := oldsize DIV 2;
  191.  
  192.         END (*LOOP*);
  193.  
  194.     END Commit;
  195.  
  196. (************************************************************************)
  197.  
  198. PROCEDURE Shuffle (size: CARDINAL;  VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  199.  
  200.     (* Shuffles an array into bit-reversed order.  The actual work of   *)
  201.     (* moving subsequences is done by procedure Commit.  The present    *)
  202.     (* procedure has the job of working out which subsequences to pass  *)
  203.     (* on to Commit.  This culling procedure means that we step right   *)
  204.     (* past elements that don't need to be moved, and that those that   *)
  205.     (* do need to be moved are moved exactly once.                      *)
  206.  
  207.     VAR start, step, newstep, level, scale: CARDINAL;
  208.         Stack: ARRAY [1..7] OF CARDINAL;
  209.  
  210.         (* The upper stack bound needs to be Log2(size) DIV 2. *)
  211.  
  212.     BEGIN
  213.         start := 0;  step := 1;  level := 0;
  214.  
  215.         LOOP
  216.             IF size < 4 THEN
  217.                 IF level <= 1 THEN
  218.                     RETURN;
  219.                 END (*IF*);
  220.                 scale := 2*Stack[level];
  221.                 newstep := step DIV scale;
  222.                 INC (start, 2*(size-1)*step + 3*(step + newstep) DIV 2);
  223.                 step := newstep;
  224.                 size := scale*scale*size;
  225.                 DEC (level);
  226.                 Stack[level] := 2*Stack[level];
  227.             ELSE
  228.                 INC (level);  Stack[level] := 1;
  229.             END (*IF*);
  230.  
  231.             step := 2*step;
  232.             size := size DIV 4;
  233.  
  234.             (* For the current subsequence, swap all odd elements in    *)
  235.             (* the first half with even elements in the second half.    *)
  236.  
  237.             Commit (start, step, size, data);
  238.  
  239.         END (*LOOP*);
  240.  
  241.     END Shuffle;
  242.  
  243. (************************************************************************)
  244. (*                 "DECIMATION IN TIME" ALGORITHM                       *)
  245. (************************************************************************)
  246.  
  247. PROCEDURE FFTDTSN (Direct: BOOLEAN;  N: CARDINAL;
  248.                         VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  249.  
  250.     (* Fast Fourier transform of an array of N complex data points.     *)
  251.     (* The result is returned in the same array.  N must be a power of  *)
  252.     (* two.  The algorithm is essentially a form of one of the          *)
  253.     (* Cooley-Tukey transforms, but I've set up the computations in     *)
  254.     (* such a way that there are no sine/cosine calculations.  This     *)
  255.     (* should, I think, improve both speed and accuracy.                *)
  256.  
  257.     (* This version assumes scrambled input data.                       *)
  258.  
  259.     (* Status: working. *)
  260.  
  261.     VAR gap, j, k, pos1, pos2, blocksize, groups: CARDINAL;
  262.         temp1, temp2, multiplier, mstep: LONGCOMPLEX;  scale, re, im: LONGREAL;
  263.  
  264.     BEGIN
  265.         (* For extra speed, we pull out the first pass as a special     *)
  266.         (* case.                                                        *)
  267.  
  268.         IF N >= 2 THEN
  269.             FOR pos1 := 0 TO N-2 BY 2 DO
  270.                 temp1 := data[pos1];
  271.                 temp2 := data[pos1+1];
  272.                 data[pos1] := temp1 + temp2;
  273.                 data[pos1+1] := temp1 - temp2;
  274.             END (*FOR*);
  275.         END (*IF*);
  276.  
  277.         (* Now for the second and subsequent passes. *)
  278.  
  279.         groups := N DIV 4;  gap := 2;
  280.         IF Direct THEN
  281.             mstep := CMPLX (0.0, -1.0);
  282.         ELSE
  283.             mstep := CMPLX (0.0, +1.0);
  284.         END (*IF*);
  285.  
  286.         WHILE groups >= 1 DO
  287.             multiplier := CMPLX (1.0, 0.0);
  288.             blocksize := 2*gap;
  289.             FOR j := 0 TO gap-1 DO
  290.                 pos1 := j;
  291.                 pos2 := pos1 + gap;
  292.                 FOR k := 0 TO groups-1 DO
  293.                     temp1 := data[pos1];
  294.                     temp2 := multiplier * data[pos2];
  295.                     data[pos1] := temp1 + temp2;
  296.                     data[pos2] := temp1 - temp2;
  297.                     INC (pos1, blocksize);
  298.                     INC (pos2, blocksize);
  299.                 END (*FOR*);
  300.                 multiplier := multiplier * mstep;
  301.             END (*FOR*);
  302.             groups := groups DIV 2;
  303.  
  304.             (* This next calculation gets the square root of mstep.     *)
  305.             (* It should be faster than the more general square root    *)
  306.             (* calculation.                                             *)
  307.  
  308.             re := Sqrt (0.5*(1.0 + RE(mstep)));
  309.             im := 0.5 * IM(mstep) / re;
  310.             mstep := CMPLX (re, im);
  311.             gap := blocksize;
  312.  
  313.         END (*WHILE*);
  314.  
  315.         (* For an inverse transform, the results have to be scaled by   *)
  316.         (* a factor 1/N.                                                *)
  317.  
  318.         IF NOT Direct THEN
  319.             scale := 1.0 / VAL(LONGREAL, N);
  320.             FOR k := 0 TO N-1 DO
  321.                 data[k] := scalarMult (scale, data[k]);
  322.             END (*FOR*);
  323.         END (*IF*);
  324.  
  325.     END FFTDTSN;
  326.  
  327. (************************************************************************)
  328.  
  329. PROCEDURE FFT3 (Direct: BOOLEAN;  N: CARDINAL;
  330.                         VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  331.  
  332.     (* Fast Fourier transform of an array of N complex data points.     *)
  333.     (* The result is returned in the same array.  N must be a power of  *)
  334.     (* two.                                                             *)
  335.  
  336.     (* Status: Working. *)
  337.  
  338.     BEGIN
  339.         Shuffle (N, data);
  340.         FFTDTSN (Direct, N, data);
  341.     END FFT3;
  342.  
  343. (************************************************************************)
  344. (*              "DECIMATION IN FREQUENCY" ALGORITHM                     *)
  345. (************************************************************************)
  346.  
  347. PROCEDURE FFT8G (Direct: BOOLEAN;  N: CARDINAL;
  348.                         VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  349.  
  350.     (* Fast Fourier transform of an array of N complex data points.     *)
  351.     (* The result is returned in the same array.  N must be a power of  *)
  352.     (* two.  The output is in scrambled order.                          *)
  353.  
  354.     (* Status: Working. *)
  355.  
  356.     VAR k, pos1, pos2, g, groups, halfN: CARDINAL;
  357.         scale, temp1, temp2, WW: LONGCOMPLEX;  theta: LONGREAL;
  358.  
  359.     BEGIN
  360.         IF N <= 1 THEN RETURN END(*IF*);
  361.         groups := 1;  halfN := N DIV 2;
  362.         theta := PI/VAL(LONGREAL,halfN);
  363.         IF Direct THEN
  364.             WW := CMPLX (Cos(theta), -Sin(theta));
  365.         ELSE
  366.             WW := CMPLX (Cos(theta), Sin(theta));
  367.         END (*IF*);
  368.         WHILE N > 1 DO
  369.             scale := CMPLX (1.0, 0.0);
  370.             FOR k := 0 TO halfN-1 DO
  371.                 pos1 := k;
  372.                 FOR g := 0 TO groups-1 DO
  373.                     pos2 := pos1 + halfN;
  374.                     temp1 := data[pos1];
  375.                     temp2 := data[pos2];
  376.                     data[pos1] := temp1 + temp2;
  377.                     data[pos2] := scale * (temp1 - temp2);
  378.                     INC (pos1, N);
  379.                 END (*FOR*);
  380.                 scale := scale*WW;
  381.             END (*FOR*);
  382.             groups := 2*groups;  N := halfN;  halfN := N DIV 2;
  383.             WW := WW * WW;
  384.         END (*WHILE*);
  385.  
  386.         (* For an inverse transform, the results have to be scaled by   *)
  387.         (* a factor 1/N.  By now groups = original N.                   *)
  388.  
  389.         IF NOT Direct THEN
  390.             theta := 1.0 / VAL(LONGREAL, groups);
  391.             FOR k := 0 TO groups-1 DO
  392.                 data[k] := scalarMult (theta, data[k]);
  393.             END (*FOR*);
  394.         END (*IF*);
  395.  
  396.     END FFT8G;
  397.  
  398. (************************************************************************)
  399.  
  400. PROCEDURE FFT8 (Direct: BOOLEAN;  N: CARDINAL;
  401.                                 VAR (*INOUT*) data: ARRAY OF LONGCOMPLEX);
  402.  
  403.     (* Fast Fourier transform of an array of N complex data points.     *)
  404.     (* The result is returned in the same array.  N must be a power of  *)
  405.     (* two.  This is based on the Sande variant of the FFT.             *)
  406.  
  407.     (* Status: Working. *)
  408.  
  409.     BEGIN
  410.         FFT8G (Direct, N, data);
  411.         Shuffle (N, data);
  412.     END FFT8;
  413.  
  414. (************************************************************************)
  415. (*              NAIVE FORM OF THE DISCRETE FOURIER TRANSFORM            *)
  416. (************************************************************************)
  417.  
  418. PROCEDURE SlowFT (Direct: BOOLEAN;  N: CARDINAL;
  419.                                 VAR (*IN*) X: ARRAY OF LONGCOMPLEX;
  420.                                 VAR (*OUT*) A: ARRAY OF LONGCOMPLEX);
  421.  
  422.     (* For comparison: a Fourier transform by the slow method. *)
  423.  
  424.     VAR r, k: CARDINAL;  temp: LONGREAL;
  425.         sum, power, weight: LONGCOMPLEX;
  426.  
  427.     BEGIN
  428.         FOR r := 0 TO N-1 DO
  429.             sum := CMPLX (0.0, 0.0);
  430.             FOR k := 0 TO N-1 DO
  431.                 temp := 2.0*PI*VAL(LONGREAL,r)*VAL(LONGREAL,k)/VAL(LONGREAL,N);
  432.                 IF Direct THEN
  433.                     power := CMPLX (0.0, -temp);
  434.                 ELSE
  435.                     power := CMPLX (0.0, temp);
  436.                 END (*IF*);
  437.                 weight := exp (power);
  438.                 sum := sum + weight * X[k];
  439.             END (*FOR*);
  440.             temp := 1.0/VAL(LONGREAL,N);
  441.             IF Direct THEN
  442.                 A[r] := sum;
  443.             ELSE
  444.                 A[r] := scalarMult (temp, sum);
  445.             END (*IF*);
  446.         END (*FOR*);
  447.     END SlowFT;
  448.  
  449. (************************************************************************)
  450. (*                      FFT ON REAL INPUT DATA                          *)
  451. (************************************************************************)
  452.  
  453. PROCEDURE RealFFT (Direct: BOOLEAN;  N: CARDINAL;
  454.                                 VAR (*INOUT*) data: ARRAY OF LONGREAL);
  455.  
  456.     (* Fast Fourier transform of an array of N real data points.        *)
  457.     (* N must be a power of two.  The complex result is returned in the *)
  458.     (* same array, by storing each complex number in two successive     *)
  459.     (* elements of data (the real part first, then the imaginary part). *)
  460.     (* This means that we can return only (N DIV 2) answers, but this   *)
  461.     (* is usually acceptable: because of the symmetries for real input  *)
  462.     (* data, it suffices to confine our attention to the positive half  *)
  463.     (* of the frequency spectrum.                                       *)
  464.  
  465.     (* Status: working.  *)
  466.  
  467.     TYPE CxPtr = POINTER TO ARRAY [0..2048] OF LONGCOMPLEX;
  468.  
  469.     CONST C1 = 0.5;
  470.  
  471.     VAR p: CxPtr;  k1, k2: CARDINAL;
  472.         sumr, sumi, diffr, diffi, WR, WI, KR, KI, C2: LONGREAL;
  473.  
  474.     BEGIN
  475.         IF N < 2 THEN RETURN END(*IF*);
  476.         p := ADR(data);
  477.         N := N DIV 2;
  478.         sumr := PI / VAL(LONGREAL, N);
  479.         IF Direct THEN
  480.             C2 := -0.5;
  481.             FFT3 (TRUE, N, p^);
  482.         ELSE
  483.             C2 := 0.5;
  484.             sumr := -sumr;
  485.         END (*IF*);
  486.  
  487.         IF N > 2 THEN
  488.             KR := Sin(0.5*sumr);  KR := 2.0*KR*KR;  WR := 1.0 - KR;
  489.             KI := Sin(sumr);  WI := -KI;
  490.             k2 := 2*(N-1);
  491.             FOR k1 := 2 TO N-2 BY 2 DO
  492.                 sumr := C1*(data[k1] + data[k2]);
  493.                 sumi := C1*(data[k1+1] - data[k2+1]);
  494.                 diffr := -C2*(data[k1+1] + data[k2+1]);
  495.                 diffi := C2*(data[k1] - data[k2]);
  496.                 data[k1] := sumr + WR*diffr - WI*diffi;
  497.                 data[k1+1] := sumi + WR*diffi + WI*diffr;
  498.                 data[k2] := sumr - WR*diffr + WI*diffi;
  499.                 data[k2+1] := -sumi + WR*diffi + WI*diffr;
  500.                 sumr := WR - WR*KR + WI*KI;
  501.                 WI := WI - WR*KI - WI*KR;
  502.                 WR := sumr;
  503.                 DEC (k2, 2);
  504.             END (*FOR*);
  505.         END (*IF*);
  506.         IF N > 1 THEN data[N+1] := -data[N+1] END(*IF*);
  507.         sumr := data[0];
  508.         data[0] := sumr + data[1];
  509.         data[1] := sumr - data[1];
  510.  
  511.         IF NOT Direct THEN
  512.             data[0] := 0.5*data[0];  data[1] := 0.5*data[1];
  513.             FFT3 (FALSE, N, p^);
  514.         END (*IF*);
  515.  
  516.     END RealFFT;
  517.  
  518. (************************************************************************)
  519.  
  520. END Fourier.
  521.  
  522.