← Back to branch summary

~ubuntu-branches/ubuntu/hoary/kdemultimedia/hoary

« back to all changes in this revision

Viewing changes to noatun/noatun/library/noatunarts/fft.c

  • Committer: Bazaar Package Importer
  • Author(s): Martin Schulze
  • Date: 2003-01-22 15:00:51 UTC
  • Revision ID: james.westby@ubuntu.com-20030122150051-uihwkdoxf15mi1tn
Tags: upstream-2.2.2
Import upstream version 2.2.2

Show diffs side-by-side

added added

removed removed

Lines of Context:
noatun/noatun/library/noatunarts/fft.c
 
1
 
 
2
#ifdef HAVE_CONFIG_H
 
3
#include <config.h>
 
4
#endif
 
5
#include <stdlib.h>
 
6
#include <stdio.h>
 
7
#include <math.h>
 
8
#include "fft.h"
 
9
 
 
10
#define TRUE  1
 
11
#define FALSE 0
 
12
 
 
13
/*
 
14
 band pass filter for the Eq.  This is a modification of Kai Lassfolk's work, as
 
15
 removed from the Sound Processing Kit:
 
16
 
 
17
    Sound Processing Kit - A C++ Class Library for Audio Signal Processing
 
18
    Copyright (C) 1995-1998 Kai Lassfolk
 
19
 
 
20
    This library is free software; you can redistribute it and/or
 
21
    modify it under the terms of the GNU Library General Public
 
22
    License as published by the Free Software Foundation; either
 
23
    version 2 of the License, or (at your option) any later version.
 
24
 
 
25
    This library is distributed in the hope that it will be useful,
 
26
    but WITHOUT ANY WARRANTY; without even the implied warranty of
 
27
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 
28
    Library General Public License for more details.
 
29
 
 
30
    You should have received a copy of the GNU Library General Public
 
31
    License along with this library; if not, write to the Free
 
32
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
33
*/
 
34
 
 
35
#define SAMPLERATE 44100
 
36
 
 
37
#ifndef M_PI
 
38
#define M_PI DDC_PI
 
39
#endif
 
40
 
 
41
void BandPassInit(struct BandPassInfo *ip, float center, float bw)
 
42
{
 
43
        ip->center = center;
 
44
        ip->bandwidth = bw;
 
45
 
 
46
        ip->C = 1.0 / tan(M_PI * bw / SAMPLERATE);
 
47
        ip->D = 2 * cos(2 * M_PI * center / SAMPLERATE);
 
48
 
 
49
        ip->a[0] = 1.0 / (1.0 + ip->C);
 
50
        ip->a[1] = 0.0;
 
51
        ip->a[2] = -ip->a[0];
 
52
 
 
53
        ip->b[0] = -ip->C * ip->D * ip->a[0];
 
54
        ip->b[1] = (ip->C - 1.0) * ip->a[0];
 
55
 
 
56
        ip->bufferX[0] = ip->bufferX[1] = 0.0;
 
57
        ip->bufferY[0] = ip->bufferY[1] = 0.0;
 
58
}
 
59
void BandPassSSE(struct BandPassInfo *ip, float *inbuffer, float *outbuffer, unsigned long samples)
 
60
{
 
61
#ifdef HAVE_X86_SSE
 
62
        __asm__(
 
63
                "testl      %0, %0                \n"
 
64
                "jz         .l5                   \n" /* if (!samples) */
 
65
                
 
66
                "movl       %1, %%ecx             \n"
 
67
                "movups     0x10(%%ecx), %%xmm2   \n" /* ip->a[0] */
 
68
                "shufps     $0x00, %%xmm2, %%xmm2 \n" /* ip->a[0] all over xmm3 */
 
69
                "movups     0x14(%%ecx), %%xmm4   \n" /* xmm4 = {ip->a[1], ip->a[2], ip->b} */
 
70
                "movups     0x24(%%ecx), %%xmm5   \n" /* xmm5 = {ip->bufferX, ip->bufferY} */
 
71
                "xorl       %%ecx, %%ecx          \n" /* i = 0 */
 
72
                "movl       $1, %%edx             \n" /* j = 1 */
 
73
                "prefetcht0 (%2)                  \n"
 
74
                ".l1:                             \n"
 
75
                
 
76
                "decl       %%edx                 \n" /* --j */
 
77
                "jnz        .l4                   \n" /* if (j) */
 
78
 
 
79
                /* only load single values if less than four remain in inbuffer */
 
80
                "testl      $0xfffffffc, %0       \n"
 
81
                "jnz        .l2                   \n"
 
82
                "movss      (%2, %%ecx, 4), %%xmm3\n"
 
83
                "movl       $1, %%edx             \n"
 
84
                "jmp        .l3                   \n"
 
85
                ".l2:                             \n"
 
86
                /* {inbuffer[i], inbuffer[i+1], inbuffer[i+2], inbuffer[i+3]} * ip->a[0] */
 
87
                "movups     (%2, %%ecx, 4), %%xmm3\n"
 
88
                "movl       $3, %%edx             \n" /* j = 3 */
 
89
                ".l3:                             \n"
 
90
                "movaps     %%xmm3, %%xmm6        \n"
 
91
                "mulps      %%xmm2, %%xmm3        \n"
 
92
                ".l4:                             \n"
 
93
                
 
94
                /* {ip->a[1], ip->a[2], ip->b} * {ip->bufferX, ip->bufferY} */
 
95
                "movaps     %%xmm4, %%xmm0        \n"
 
96
                "mulps      %%xmm5, %%xmm0        \n"
 
97
                "movaps     %%xmm0, %%xmm1        \n"
 
98
                /* xmm0 = {xmm0[0] + xmm0[1], <unused>, xmm0[2] + xmm0[3], <unused>} */
 
99
                "shufps     $0xb1, %%xmm0, %%xmm1 \n"
 
100
                "addps      %%xmm1, %%xmm0        \n"
 
101
                /* xmm0[0] -= xmm0[2] */
 
102
                "movhlps    %%xmm0, %%xmm1        \n"
 
103
                "subss      %%xmm1, %%xmm0        \n"
 
104
                "addss      %%xmm3, %%xmm0        \n" /* xmm0[0] += inbuffer[i] * ip->a[0] */
 
105
                "movss      %%xmm0, (%3, %%ecx, 4)\n" /* outbuffer[i] = xmm0[0] */
 
106
 
 
107
                /* xmm5 = {inbuffer[i], xmm5[0], outbuffer[i], xmm5[2]} */              
 
108
                "shufps     $0x24, %%xmm5, %%xmm0 \n"
 
109
                "shufps     $0x81, %%xmm0, %%xmm5 \n"
 
110
                "movss      %%xmm6, %%xmm5        \n"
 
111
 
 
112
                /* right-shift xmm3 (inbuffer * ip->a[0]) and xmm6 (inbuffer) */
 
113
                "shufps     $0x39, %%xmm3, %%xmm3 \n"
 
114
                "shufps     $0x39, %%xmm6, %%xmm6 \n"
 
115
                
 
116
                "incl       %%ecx                 \n" /* ++i */
 
117
                "decl       %0                    \n"
 
118
                "jnz        .l1                   \n"
 
119
                
 
120
                "movl       %1,%%ecx              \n"
 
121
                "movups     %%xmm5, 0x24(%%ecx)   \n" /* {ip->bufferX, ip->bufferY} = xmm5 */
 
122
                "emms                             \n"
 
123
                ".l5:                             \n"
 
124
        :
 
125
        : "r" (samples),  /* %0 */
 
126
          "m" (ip),       /* %1 */
 
127
          "r" (inbuffer), /* %2 */
 
128
          "r" (outbuffer) /* %3 */
 
129
        : "ecx", "edx");
 
130
#endif
 
131
}
 
132
        
 
133
void BandPass(struct BandPassInfo *ip, float *inbuffer, float *outbuffer, unsigned long samples)
 
134
{
 
135
        unsigned long i;
 
136
        for (i=0; i<samples; ++i)
 
137
        {
 
138
            outbuffer[i] = ip->a[0] * inbuffer[i] + ip->a[1] * ip->bufferX[0] + ip->a[2]
 
139
                               * ip->bufferX[1] - ip->b[0] * ip->bufferY[0] - ip->b[1]
 
140
                                           * ip->bufferY[1];
 
141
 
 
142
                ip->bufferX[1] = ip->bufferX[0];
 
143
                ip->bufferX[0] = inbuffer[i];
 
144
                ip->bufferY[1] = ip->bufferY[0];
 
145
                ip->bufferY[0] = outbuffer[i];
 
146
        }
 
147
}
 
148
 
 
149
 
 
150
/*============================================================================
 
151
 
 
152
    fftmisc.c  -  Don Cross <dcross@intersrv.com>
 
153
 
 
154
    http://www.intersrv.com/~dcross/fft.html
 
155
 
 
156
    Helper routines for Fast Fourier Transform implementation.
 
157
    Contains common code for fft_float() and fft_double().
 
158
 
 
159
    See also:
 
160
        fourierf.c
 
161
        fourierd.c
 
162
        ..\include\fourier.h
 
163
 
 
164
    Revision history:
 
165
 
 
166
1998 September 19 [Don Cross]
 
167
    Improved the efficiency of IsPowerOfTwo().
 
168
    Updated coding standards.
 
169
 
 
170
============================================================================*/
 
171
 
 
172
 
 
173
#define BITS_PER_WORD   (sizeof(unsigned) * 8)
 
174
 
 
175
 
 
176
static int IsPowerOfTwo ( unsigned x )
 
177
{
 
178
    if ( x < 2 )
 
179
        return FALSE;
 
180
 
 
181
    if ( x & (x-1) )        /* Thanks to 'byang' for this cute trick! */
 
182
        return FALSE;
 
183
 
 
184
    return TRUE;
 
185
}
 
186
 
 
187
 
 
188
static unsigned NumberOfBitsNeeded ( unsigned PowerOfTwo )
 
189
{
 
190
    unsigned i;
 
191
 
 
192
    if ( PowerOfTwo < 2 )
 
193
    {
 
194
        fprintf (
 
195
            stderr,
 
196
            ">>> Error in fftmisc.c: argument %d to NumberOfBitsNeeded is too small.\n",
 
197
            PowerOfTwo );
 
198
 
 
199
        exit(1);
 
200
    }
 
201
 
 
202
    for ( i=0; ; i++ )
 
203
    {
 
204
        if ( PowerOfTwo & (1 << i) )
 
205
            return i;
 
206
    }
 
207
}
 
208
 
 
209
 
 
210
 
 
211
static unsigned ReverseBits ( unsigned index, unsigned NumBits )
 
212
{
 
213
    unsigned i, rev;
 
214
 
 
215
    for ( i=rev=0; i < NumBits; i++ )
 
216
    {
 
217
        rev = (rev << 1) | (index & 1);
 
218
        index >>= 1;
 
219
    }
 
220
 
 
221
    return rev;
 
222
}
 
223
 
 
224
 
 
225
static double Index_to_frequency ( unsigned NumSamples, unsigned Index )
 
226
{
 
227
    if ( Index >= NumSamples )
 
228
        return 0.0;
 
229
    else if ( Index <= NumSamples/2 )
 
230
        return (double)Index / (double)NumSamples;
 
231
 
 
232
    return -(double)(NumSamples-Index) / (double)NumSamples;
 
233
}
 
234
 
 
235
#undef TRUE
 
236
#undef FALSE
 
237
#undef BITS_PER_WORD
 
238
/*============================================================================
 
239
 
 
240
    fourierf.c  -  Don Cross <dcross@intersrv.com>
 
241
 
 
242
    http://www.intersrv.com/~dcross/fft.html
 
243
 
 
244
    Contains definitions for doing Fourier transforms
 
245
    and inverse Fourier transforms.
 
246
 
 
247
    This module performs operations on arrays of 'float'.
 
248
 
 
249
    Revision history:
 
250
 
 
251
1998 September 19 [Don Cross]
 
252
    Updated coding standards.
 
253
    Improved efficiency of trig calculations.
 
254
 
 
255
============================================================================*/
 
256
 
 
257
#include <stdlib.h>
 
258
#include <stdio.h>
 
259
#include <math.h>
 
260
 
 
261
#include "fft.h"
 
262
 
 
263
#define CHECKPOINTER(p)  CheckPointer(p,#p)
 
264
 
 
265
static void CheckPointer ( const void *p, const char *name )
 
266
{
 
267
    if ( p == NULL )
 
268
    {
 
269
        fprintf ( stderr, "Error in fft_float():  %s == NULL\n", name );
 
270
        exit(1);
 
271
    }
 
272
}
 
273
 
 
274
 
 
275
void fft_float (
 
276
    unsigned  NumSamples,
 
277
    int       InverseTransform,
 
278
    float    *RealIn,
 
279
    float    *ImagIn,
 
280
    float    *RealOut,
 
281
    float    *ImagOut )
 
282
{
 
283
    unsigned NumBits;    /* Number of bits needed to store indices */
 
284
    unsigned i, j, k, n;
 
285
    unsigned BlockSize, BlockEnd;
 
286
 
 
287
    double angle_numerator = 2.0 * DDC_PI;
 
288
    double tr, ti;     /* temp real, temp imaginary */
 
289
 
 
290
    if ( !IsPowerOfTwo(NumSamples) )
 
291
    {
 
292
        fprintf (
 
293
            stderr,
 
294
            "Error in fft():  NumSamples=%u is not power of two\n",
 
295
            NumSamples );
 
296
 
 
297
        exit(1);
 
298
    }
 
299
 
 
300
    if ( InverseTransform )
 
301
        angle_numerator = -angle_numerator;
 
302
 
 
303
    CHECKPOINTER ( RealIn );
 
304
    CHECKPOINTER ( RealOut );
 
305
    CHECKPOINTER ( ImagOut );
 
306
 
 
307
    NumBits = NumberOfBitsNeeded ( NumSamples );
 
308
 
 
309
    /*
 
310
    **   Do simultaneous data copy and bit-reversal ordering into outputs...
 
311
    */
 
312
 
 
313
    for ( i=0; i < NumSamples; i++ )
 
314
    {
 
315
        j = ReverseBits ( i, NumBits );
 
316
        RealOut[j] = RealIn[i];
 
317
        ImagOut[j] = (ImagIn == NULL) ? 0.0 : ImagIn[i];
 
318
    }
 
319
 
 
320
    /*
 
321
    **   Do the FFT itself...
 
322
    */
 
323
 
 
324
    BlockEnd = 1;
 
325
    for ( BlockSize = 2; BlockSize <= NumSamples; BlockSize <<= 1 )
 
326
    {
 
327
        double delta_angle = angle_numerator / (double)BlockSize;
 
328
        double sm2 = sin ( -2 * delta_angle );
 
329
        double sm1 = sin ( -delta_angle );
 
330
        double cm2 = cos ( -2 * delta_angle );
 
331
        double cm1 = cos ( -delta_angle );
 
332
        double w = 2 * cm1;
 
333
        double ar[3], ai[3];
 
334
 
 
335
        for ( i=0; i < NumSamples; i += BlockSize )
 
336
        {
 
337
            ar[2] = cm2;
 
338
            ar[1] = cm1;
 
339
 
 
340
            ai[2] = sm2;
 
341
            ai[1] = sm1;
 
342
 
 
343
            for ( j=i, n=0; n < BlockEnd; j++, n++ )
 
344
            {
 
345
                ar[0] = w*ar[1] - ar[2];
 
346
                ar[2] = ar[1];
 
347
                ar[1] = ar[0];
 
348
 
 
349
                ai[0] = w*ai[1] - ai[2];
 
350
                ai[2] = ai[1];
 
351
                ai[1] = ai[0];
 
352
 
 
353
                k = j + BlockEnd;
 
354
                tr = ar[0]*RealOut[k] - ai[0]*ImagOut[k];
 
355
                ti = ar[0]*ImagOut[k] + ai[0]*RealOut[k];
 
356
 
 
357
                RealOut[k] = RealOut[j] - tr;
 
358
                ImagOut[k] = ImagOut[j] - ti;
 
359
 
 
360
                RealOut[j] += tr;
 
361
                ImagOut[j] += ti;
 
362
            }
 
363
        }
 
364
 
 
365
        BlockEnd = BlockSize;
 
366
    }
 
367
 
 
368
    /*
 
369
    **   Need to normalize if inverse transform...
 
370
    */
 
371
 
 
372
    if ( InverseTransform )
 
373
    {
 
374
        double denom = (double)NumSamples;
 
375
 
 
376
        for ( i=0; i < NumSamples; i++ )
 
377
        {
 
378
            RealOut[i] /= denom;
 
379
            ImagOut[i] /= denom;
 
380
        }
 
381
    }
 
382
}
 
383
 
 
384