transforms.cc

各种工程计算的库函数

/*

    DFT, DCT and DST routines
    Copyright (C) 1996-2001 Takuya OOURA
    Copyright (C) 1999-2003 Jussi Laako

    This program 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.

    This program 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 this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
    Version 1.0 : Converted T.OOURA's functions to C++ class
    Version 1.1 : Updated to match T.OOURA's Oct/99 code and all versions
    Version 1.2 : Some conversion and precision updates
    Version 1.3 : Updated to match T.OOURA's Oct/01 code

*/


/* This is decimation-in-frequency split-radix transform */


#include <stdio.h>
#include <stdlib.h>
#ifdef USE_INTEL_MATH
    #include <mathimf.h>
#else
    #include <math.h>
#endif
#include <float.h>

#include "dsp/TransformS.hh"


#ifdef USE_CDFT_THREADS

#include <pthread.h>


void * cftrec1f_th_cb(void *p)
{
    clTransformS *TS = (clTransformS *) ((cdft_arg_t *) p)->klass;
    
    return TS->cftrec1f_th(p);
}


void * cftrec2f_th_cb(void *p)
{
    clTransformS *TS = (clTransformS *) ((cdft_arg_t *) p)->klass;
    
    return TS->cftrec2f_th(p);
}


void * cftrec1_th_cb(void *p)
{
    clTransformS *TS = (clTransformS *) ((cdft_arg_t *) p)->klass;
    
    return TS->cftrec1_th(p);
}


void * cftrec2_th_cb(void *p)
{
    clTransformS *TS = (clTransformS *) ((cdft_arg_t *) p)->klass;
    
    return TS->cftrec2_th(p);
}


void clTransformS::cdft_thread_create(cdft_thread_t *thp, 
    void * (*func)(void *), cdft_arg_t *argp)
{
    argp->klass = (void *) this;
    if (pthread_create(thp, NULL, func, (void *) argp) != 0) {
        fprintf(stderr, "clTransformS::cdft_thread_create() error\n");
        exit(1);
    }
}


void clTransformS::cdft_thread_wait(cdft_thread_t th)
{
    if (pthread_join(th, NULL) != 0) {
        fprintf(stderr, "clTransformS::cdft_thread_wait() error\n");
        exit(1);
    }
}


#endif /* USE_CDFT_THREADS */


void clTransformS::makeipt(long nw, long *ip)
{
    long j, l, m, m2, p, q;
    
    ip[2] = 0;
    ip[3] = 16;
    m = 2;
    for (l = nw; l > 32; l >>= 2) {
        m2 = m << 1;
        q = m2 << 3;
        for (j = m; j < m2; j++) {
            p = ip[j] << 2;
            ip[m + j] = p;
            ip[m2 + j] = p + q;
        }
        m = m2;
    }
}


// -----


void clTransformS::cdft(long n, long isgn, float *a, long *ip, float *w)
{
    long nw;
    
    nw = ip[0];
    if (n > (nw << 2)) {
        nw = n >> 2;
        makewt(nw, ip, w);
    }
    if (isgn >= 0) {
        cftfsub(n, a, ip, nw, w);
    } else {
        cftbsub(n, a, ip, nw, w);
    }
}


void clTransformS::rdft(long n, long isgn, float *a, long *ip, float *w)
{
    long nw, nc;
    float xi;
    
    nw = ip[0];
    if (n > (nw << 2)) {
        nw = n >> 2;
        makewt(nw, ip, w);
    }
    nc = ip[1];
    if (n > (nc << 2)) {
        nc = n >> 2;
        makect(nc, ip, w + nw);
    }
    if (isgn >= 0) {
        if (n > 4) {
            cftfsub(n, a, ip, nw, w);
            rftfsub(n, a, nc, w + nw);
        } else if (n == 4) {
            cftfsub(n, a, ip, nw, w);
        }
        xi = a[0] - a[1];
        a[0] += a[1];
        a[1] = xi;
    } else {
        a[1] = 0.5f * (a[0] - a[1]);
        a[0] -= a[1];
        if (n > 4) {
            rftbsub(n, a, nc, w + nw);
            cftbsub(n, a, ip, nw, w);
        } else if (n == 4) {
            cftbsub(n, a, ip, nw, w);
        }
    }
}


void clTransformS::ddct(long n, long isgn, float *a, long *ip, float *w)
{
    long j, nw, nc;
    float xr;
    
    nw = ip[0];
    if (n > (nw << 2)) {
        nw = n >> 2;
        makewt(nw, ip, w);
    }
    nc = ip[1];
    if (n > nc) {
        nc = n;
        makect(nc, ip, w + nw);
    }
    if (isgn < 0) {
        xr = a[n - 1];
        for (j = n - 2; j >= 2; j -= 2) {
            a[j + 1] = a[j] - a[j - 1];
            a[j] += a[j - 1];
        }
        a[1] = a[0] - xr;
        a[0] += xr;
        if (n > 4) {
            rftbsub(n, a, nc, w + nw);
            cftbsub(n, a, ip, nw, w);
        } else if (n == 4) {
            cftbsub(n, a, ip, nw, w);
        }
    }
    dctsub(n, a, nc, w + nw);
    if (isgn >= 0) {
        if (n > 4) {
            cftfsub(n, a, ip, nw, w);
            rftfsub(n, a, nc, w + nw);
        } else if (n == 4) {
            cftfsub(n, a, ip, nw, w);
        }
        xr = a[0] - a[1];
        a[0] += a[1];
        for (j = 2; j < n; j += 2) {
            a[j - 1] = a[j] - a[j + 1];
            a[j] += a[j + 1];
        }
        a[n - 1] = xr;
    }
}


void clTransformS::ddst(long n, long isgn, float *a, long *ip, float *w)
{
    long j, nw, nc;
    float xr;
    
    nw = ip[0];
    if (n > (nw << 2)) {
        nw = n >> 2;
        makewt(nw, ip, w);
    }
    nc = ip[1];
    if (n > nc) {
        nc = n;
        makect(nc, ip, w + nw);
    }
    if (isgn < 0) {
        xr = a[n - 1];
        for (j = n - 2; j >= 2; j -= 2) {
            a[j + 1] = -a[j] - a[j - 1];
            a[j] -= a[j - 1];
        }
        a[1] = a[0] + xr;
        a[0] -= xr;
        if (n > 4) {
            rftbsub(n, a, nc, w + nw);
            cftbsub(n, a, ip, nw, w);
        } else if (n == 4) {
            cftbsub(n, a, ip, nw, w);
        }
    }
    dstsub(n, a, nc, w + nw);
    if (isgn >= 0) {
        if (n > 4) {
            cftfsub(n, a, ip, nw, w);
            rftfsub(n, a, nc, w + nw);
        } else if (n == 4) {
            cftfsub(n, a, ip, nw, w);
        }
        xr = a[0] - a[1];
        a[0] += a[1];
        for (j = 2; j < n; j += 2) {
            a[j - 1] = -a[j] - a[j + 1];
            a[j] -= a[j + 1];
        }
        a[n - 1] = -xr;
    }
}


void clTransformS::dfct(long n, float *a, float *t, long *ip, float *w)
{
    long j, k, l, m, mh, nw, nc;
    float xr, xi, yr, yi;
    
    nw = ip[0];
    if (n > (nw << 3)) {
        nw = n >> 3;
        makewt(nw, ip, w);
    }
    nc = ip[1];
    if (n > (nc << 1)) {
        nc = n >> 1;
        makect(nc, ip, w + nw);
    }
    m = n >> 1;
    yi = a[m];
    xi = a[0] + a[n];
    a[0] -= a[n];
    t[0] = xi - yi;
    t[m] = xi + yi;
    if (n > 2) {
        mh = m >> 1;
        for (j = 1; j < mh; j++) {
            k = m - j;
            xr = a[j] - a[n - j];
            xi = a[j] + a[n - j];
            yr = a[k] - a[n - k];
            yi = a[k] + a[n - k];
            a[j] = xr;
            a[k] = yr;
            t[j] = xi - yi;
            t[k] = xi + yi;
        }
        t[mh] = a[mh] + a[n - mh];
        a[mh] -= a[n - mh];
        dctsub(m, a, nc, w + nw);
        if (m > 4) {
            cftfsub(m, a, ip, nw, w);
            rftfsub(m, a, nc, w + nw);
        } else if (m == 4) {
            cftfsub(m, a, ip, nw, w);
        }
        a[n - 1] = a[0] - a[1];
        a[1] = a[0] + a[1];
        for (j = m - 2; j >= 2; j -= 2) {
            a[2 * j + 1] = a[j] + a[j + 1];
            a[2 * j - 1] = a[j] - a[j + 1];
        }
        l = 2;
        m = mh;
        while (m >= 2) {
            dctsub(m, t, nc, w + nw);
            if (m > 4) {
                cftfsub(m, t, ip, nw, w);
                rftfsub(m, t, nc, w + nw);
            } else if (m == 4) {
                cftfsub(m, t, ip, nw, w);
            }
            a[n - l] = t[0] - t[1];
            a[l] = t[0] + t[1];
            k = 0;
            for (j = 2; j < m; j += 2) {
                k += l << 2;
                a[k - l] = t[j] - t[j + 1];
                a[k + l] = t[j] + t[j + 1];
            }
            l <<= 1;
            mh = m >> 1;
            for (j = 0; j < mh; j++) {
                k = m - j;
                t[j] = t[m + k] - t[m + j];
                t[k] = t[m + k] + t[m + j];
            }
            t[mh] = t[m + mh];
            m = mh;
        }
        a[l] = t[0];
        a[n] = t[2] - t[1];
        a[0] = t[2] + t[1];
    } else {
        a[1] = a[0];
        a[2] = t[0];
        a[0] = t[1];
    }
}


void clTransformS::dfst(long n, float *a, float *t, long *ip, float *w)
{
    long j, k, l, m, mh, nw, nc;
    float xr, xi, yr, yi;
    
    nw = ip[0];
    if (n > (nw << 3)) {
        nw = n >> 3;
        makewt(nw, ip, w);
    }
    nc = ip[1];
    if (n > (nc << 1)) {
        nc = n >> 1;
        makect(nc, ip, w + nw);
    }
    if (n > 2) {
        m = n >> 1;
        mh = m >> 1;
        for (j = 1; j < mh; j++) {
            k = m - j;
            xr = a[j] + a[n - j];
            xi = a[j] - a[n - j];
            yr = a[k] + a[n - k];
            yi = a[k] - a[n - k];
            a[j] = xr;
            a[k] = yr;
            t[j] = xi + yi;
            t[k] = xi - yi;
        }
        t[0] = a[mh] - a[n - mh];
        a[mh] += a[n - mh];
        a[0] = a[m];
        dstsub(m, a, nc, w + nw);
        if (m > 4) {
            cftfsub(m, a, ip, nw, w);
            rftfsub(m, a, nc, w + nw);
        } else if (m == 4) {
            cftfsub(m, a, ip, nw, w);
        }
        a[n - 1] = a[1] - a[0];
        a[1] = a[0] + a[1];
        for (j = m - 2; j >= 2; j -= 2) {
            a[2 * j + 1] = a[j] - a[j + 1];
            a[2 * j - 1] = -a[j] - a[j + 1];
        }
        l = 2;
        m = mh;
        while (m >= 2) {
            dstsub(m, t, nc, w + nw);
            if (m > 4) {
                cftfsub(m, t, ip, nw, w);
                rftfsub(m, t, nc, w + nw);
            } else if (m == 4) {
                cftfsub(m, t, ip, nw, w);
            }
            a[n - l] = t[1] - t[0];
            a[l] = t[0] + t[1];
            k = 0;
            for (j = 2; j < m; j += 2) {
                k += l << 2;
                a[k - l] = -t[j] - t[j + 1];
                a[k + l] = t[j] - t[j + 1];
            }
            l <<= 1;
            mh = m >> 1;
            for (j = 1; j < mh; j++) {
                k = m - j;
                t[j] = t[m + k] + t[m + j];
                t[k] = t[m + k] - t[m + j];
            }
            t[0] = t[m + mh];
            m = mh;
        }
        a[l] = t[0];
    }
    a[0] = 0;
}


/* -------- initializing routines -------- */


void clTransformS::makewt(long nw, long *ip, float *w)
{
    long j, nwh, nw0, nw1;
    float delta, wn4r, wk1r, wk1i, wk3r, wk3i;
    
    ip[0] = nw;
    ip[1] = 1;
    if (nw > 2) {
        nwh = nw >> 1;
#       ifndef TRANSFORM_EXT_PREC
        delta = atanf(1.0f) / nwh;
        wn4r = cosf(delta * nwh);
#       else
        delta = (float) atan(1.0) / nwh;
        wn4r = (float) cos(delta * nwh);
#       endif
        w[0] = 1;
        w[1] = wn4r;
        if (nwh == 4) {
#           ifndef TRANSFORM_EXT_PREC
            w[2] = cosf(delta * 2.0f);
            w[3] = sinf(delta * 2.0f);
#           else
            w[2] = (float) cos(delta * 2.0);
            w[3] = (float) sin(delta * 2.0);
#           endif
        } else if (nwh > 4) {
            makeipt(nw, ip);
#           ifndef TRANSFORM_EXT_PREC
            w[2] = 0.5f / cosf(delta * 2.0f);
            w[3] = 0.5f / cosf(delta * 6.0f);
#           else
            w[2] = (float) (0.5 / cos(delta * 2.0));
            w[3] = (float) (0.5 / cos(delta * 6.0));
#           endif
            for (j = 4; j < nwh; j += 4) {
#               ifndef TRANSFORM_EXT_PREC
                w[j] = cosf(delta * j);
                w[j + 1] = sinf(delta * j);
                w[j + 2] = cosf(3.0f * delta * j);
                w[j + 3] = -sinf(3.0f * delta * j);
#               else
                w[j] = (float) cos(delta * j);
                w[j + 1] = (float) sin(delta * j);
                w[j + 2] = (float) cos(3.0 * delta * j);
                w[j + 3] = (float) -sin(3.0 * delta * j);
#               endif
            }