src/fht.cpp

   1 // FHT - Fast Hartley Transform Class
   2 //
   3 // Copyright (C) 2004  Melchior FRANZ - mfranz@kde.org
   4 //
   5 // This program is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU General Public License as
   7 // published by the Free Software Foundation; either version 2 of the
   8 // License, or (at your option) any later version.
   9 //
  10 // This program is distributed in the hope that it will be useful, but
  11 // WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
  18 //
  19 // $Id$
  20
  21
  22 #include "fht.h"
  23
  24 #include <math.h>
  25 #include <string.h>
  26
  27 FHT::FHT(int n) :
  28     m_buf(0),
  29     m_tab(0),
  30     m_log(0)
  31 {
  32     if (n < 3) {
  33         m_num = 0;
  34         m_exp2 = -1;
  35         return;
  36     }
  37     m_exp2 = n;
  38     m_num = 1 << n;
  39     if (n > 3) {
  40         m_buf = new float[m_num];
  41         m_tab = new float[m_num * 2];
  42         makeCasTable();
  43     }
  44 }
  45
  46
  47 FHT::~FHT()
  48 {
  49     delete[] m_buf;
  50     delete[] m_tab;
  51     delete[] m_log;
  52 }
  53
  54
  55 void FHT::makeCasTable(void)
  56 {
  57     float d, *costab, *sintab;
  58     int ul, ndiv2 = m_num / 2;
  59
  60     for (costab = m_tab, sintab = m_tab + m_num / 2 + 1, ul = 0; ul < m_num; ul++) {
  61         d = M_PI * ul / ndiv2;
  62         *costab = *sintab = cos(d);
  63
  64         costab += 2, sintab += 2;
  65         if (sintab > m_tab + m_num * 2)
  66             sintab = m_tab + 1;
  67     }
  68 }
  69
  70
  71 float* FHT::copy(float *d, float *s)
  72 {
  73     return (float *)memcpy(d, s, m_num * sizeof(float));
  74 }
  75
  76
  77 float* FHT::clear(float *d)
  78 {
  79     return (float *)memset(d, 0, m_num * sizeof(float));
  80 }
  81
  82
  83 void FHT::scale(float *p, float d)
  84 {
  85     for (int i = 0; i < (m_num / 2); i++)
  86         *p++ *= d;
  87 }
  88
  89
  90 void FHT::ewma(float *d, float *s, float w)
  91 {
  92     for (int i = 0; i < (m_num / 2); i++, d++, s++)
  93         *d = *d * w + *s * (1 - w);
  94 }
  95
  96
  97 void FHT::logSpectrum(float *out, float *p)
  98 {
  99     int n = m_num / 2, i, j, k, *r;
 100     if (!m_log) {
 101         m_log = new int[n];
 102         float f = n / log10((double)n);
 103         for (i = 0, r = m_log; i < n; i++, r++) {
 104             j = int(rint(log10(i + 1.0) * f));
 105             *r = j >= n ? n - 1 : j;
 106         }
 107     }
 108     semiLogSpectrum(p);
 109     *out++ = *p = *p / 100;
 110     for (k = i = 1, r = m_log; i < n; i++) {
 111         j = *r++;
 112         if (i == j)
 113             *out++ = p[i];
 114         else {
 115             float base = p[k - 1];
 116             float step = (p[j] - base) / (j - (k - 1));
 117             for (float corr = 0; k <= j; k++, corr += step)
 118                 *out++ = base + corr;
 119         }
 120     }
 121 }
 122
 123
 124 void FHT::semiLogSpectrum(float *p)
 125 {
 126     float e;
 127     power2(p);
 128     for (int i = 0; i < (m_num / 2); i++, p++) {
 129         e = 10.0 * log10(sqrt(*p * .5));
 130         *p = e < 0 ? 0 : e;
 131     }
 132 }
 133
 134
 135 void FHT::spectrum(float *p)
 136 {
 137     power2(p);
 138     for (int i = 0; i < (m_num / 2); i++, p++)
 139         *p = (float)sqrt(*p * .5);
 140 }
 141
 142
 143 void FHT::power(float *p)
 144 {
 145     power2(p);
 146     for (int i = 0; i < (m_num / 2); i++)
 147         *p++ *= .5;
 148 }
 149
 150
 151 void FHT::power2(float *p)
 152 {
 153     int i;
 154     float *q;
 155     _transform(p, m_num, 0);
 156
 157     *p = (*p * *p), *p += *p, p++;
 158
 159     for (i = 1, q = p + m_num - 2; i < (m_num / 2); i++, --q)
 160         *p = (*p * *p) + (*q * *q), p++;
 161 }
 162
 163
 164 void FHT::transform(float *p)
 165 {
 166     if (m_num == 8)
 167         transform8(p);
 168     else
 169         _transform(p, m_num, 0);
 170 }
 171
 172
 173 void FHT::transform8(float *p)
 174 {
 175     float a, b, c, d, e, f, g, h, b_f2, d_h2;
 176     float a_c_eg, a_ce_g, ac_e_g, aceg, b_df_h, bdfh;
 177
 178     a = *p++, b = *p++, c = *p++, d = *p++;
 179     e = *p++, f = *p++, g = *p++, h = *p;
 180     b_f2 = (b - f) * M_SQRT2;
 181     d_h2 = (d - h) * M_SQRT2;
 182
 183     a_c_eg = a - c - e + g;
 184     a_ce_g = a - c + e - g;
 185     ac_e_g = a + c - e - g;
 186     aceg = a + c + e + g;
 187
 188     b_df_h = b - d + f - h;
 189     bdfh = b + d + f + h;
 190
 191     *p = a_c_eg - d_h2;
 192     *--p = a_ce_g - b_df_h;
 193     *--p = ac_e_g - b_f2;
 194     *--p = aceg - bdfh;
 195     *--p = a_c_eg + d_h2;
 196     *--p = a_ce_g + b_df_h;
 197     *--p = ac_e_g + b_f2;
 198     *--p = aceg + bdfh;
 199 }
 200
 201
 202 void FHT::_transform(float *p, int n, int k)
 203 {
 204     if (n == 8) {
 205         transform8(p + k);
 206         return;
 207     }
 208
 209     int i, j, ndiv2 = n / 2;
 210     float a, *t1, *t2, *t3, *t4, *ptab, *pp;
 211
 212     for (i = 0, t1 = m_buf, t2 = m_buf + ndiv2, pp = &p[k]; i < ndiv2; i++)
 213         *t1++ = *pp++, *t2++ = *pp++;
 214
 215     memcpy(p + k, m_buf, sizeof(float) * n);
 216
 217     _transform(p, ndiv2, k);
 218     _transform(p, ndiv2, k + ndiv2);
 219
 220     j = m_num / ndiv2 - 1;
 221     t1 = m_buf;
 222     t2 = t1 + ndiv2;
 223     t3 = p + k + ndiv2;
 224     ptab = m_tab;
 225     pp = p + k;
 226
 227     a = *ptab++ * *t3++;
 228     a += *ptab * *pp;
 229     ptab += j;
 230
 231     *t1++ = *pp + a;
 232     *t2++ = *pp++ - a;
 233
 234     for (i = 1, t4 = p + k + n; i < ndiv2; i++, ptab += j) {
 235         a = *ptab++ * *t3++;
 236         a += *ptab * *--t4;
 237
 238         *t1++ = *pp + a;
 239         *t2++ = *pp++ - a;
 240     }
 241     memcpy(p + k, m_buf, sizeof(float) * n);
 242 }
 243