nivedita velagaleti / 3DAudio

ese 519

Dependents:   PROJECT_3D_AUDIO

fhtdit.cc@0:2076b4d80327, 2015-04-07 (annotated)

Committer:
niv17
Date:
Tue Apr 07 21:09:22 2015 +0000
Revision:
0:2076b4d80327
sonic initial april 7

Who changed what in which revision?

UserRevisionLine numberNew contents of line
niv17 0:2076b4d80327 1 // This file is part of the FXT library.
niv17 0:2076b4d80327 2 // Copyright (C) 2010, 2012 Joerg Arndt
niv17 0:2076b4d80327 3 // License: GNU General Public License version 3 or later,
niv17 0:2076b4d80327 4 // see the file COPYING.txt in the main directory.
niv17 0:2076b4d80327 5
niv17 0:2076b4d80327 6 #include "fxttypes.h"
niv17 0:2076b4d80327 7 #include "cmult.h"
niv17 0:2076b4d80327 8 #include "sumdiff.h"
niv17 0:2076b4d80327 9 #include "constants.h" // COS_1_PI_8, SIN_1_PI_8
niv17 0:2076b4d80327 10 #include "sincos.h"
niv17 0:2076b4d80327 11 #include "revbinpermute.h"
niv17 0:2076b4d80327 12
niv17 0:2076b4d80327 13 #include <cmath> // M_PI, M_SQRT1_2
niv17 0:2076b4d80327 14
niv17 0:2076b4d80327 15 // tuning parameter:
niv17 0:2076b4d80327 16 // define to use trig recurrence:
niv17 0:2076b4d80327 17 // (and possibly lose some precision, see below)
niv17 0:2076b4d80327 18 //#define USE_TRIG_REC
niv17 0:2076b4d80327 19 // with type 'long double' slight speed loss on my machine,
niv17 0:2076b4d80327 20 // with type 'double' little speed gain.
niv17 0:2076b4d80327 21
niv17 0:2076b4d80327 22 #if defined USE_TRIG_REC
niv17 0:2076b4d80327 23 //#warning 'FYI: fht(double *, ulong) uses trig recursion'
niv17 0:2076b4d80327 24 #endif
niv17 0:2076b4d80327 25
niv17 0:2076b4d80327 26 // tuning parameter:
niv17 0:2076b4d80327 27 #define INITIAL_RADIX_16 1 // 0 or 1 (default)
niv17 0:2076b4d80327 28 //
niv17 0:2076b4d80327 29 #if ( INITIAL_RADIX_16==1 )
niv17 0:2076b4d80327 30 //#warning 'FYI: INITIAL_RADIX_16 set in fht_dit(double *, ulong)'
niv17 0:2076b4d80327 31 #else
niv17 0:2076b4d80327 32 #warning 'INITIAL_RADIX_16 is NOT SET in fht_dit(double *, ulong)'
niv17 0:2076b4d80327 33 #endif
niv17 0:2076b4d80327 34
niv17 0:2076b4d80327 35
niv17 0:2076b4d80327 36 void
niv17 0:2076b4d80327 37 fht_dit_core(double *f, ulong ldn)
niv17 0:2076b4d80327 38 // Decimation in time (DIT) FHT.
niv17 0:2076b4d80327 39 // Input data must be in revbin_permuted order.
niv17 0:2076b4d80327 40 // ldn := base-2 logarithm of the array length.
niv17 0:2076b4d80327 41 {
niv17 0:2076b4d80327 42 if ( ldn<=2 )
niv17 0:2076b4d80327 43 {
niv17 0:2076b4d80327 44 if ( ldn==1 ) // two point fht
niv17 0:2076b4d80327 45 {
niv17 0:2076b4d80327 46 sumdiff(f[0], f[1]);
niv17 0:2076b4d80327 47 }
niv17 0:2076b4d80327 48 else if ( ldn==2 ) // four point fht
niv17 0:2076b4d80327 49 {
niv17 0:2076b4d80327 50 double f0, f1, f2, f3;
niv17 0:2076b4d80327 51 sumdiff(f[0], f[1], f0, f1);
niv17 0:2076b4d80327 52 sumdiff(f[2], f[3], f2, f3);
niv17 0:2076b4d80327 53 sumdiff(f0, f2, f[0], f[2]);
niv17 0:2076b4d80327 54 sumdiff(f1, f3, f[1], f[3]);
niv17 0:2076b4d80327 55 }
niv17 0:2076b4d80327 56 return;
niv17 0:2076b4d80327 57 }
niv17 0:2076b4d80327 58
niv17 0:2076b4d80327 59 const ulong n = (1UL<<ldn);
niv17 0:2076b4d80327 60 const double *fn = f + n;
niv17 0:2076b4d80327 61 ulong ldk = ldn & 1;
niv17 0:2076b4d80327 62
niv17 0:2076b4d80327 63 if ( ldk==0 ) // ldn is multiple of 2 => n is a power of 4
niv17 0:2076b4d80327 64 {
niv17 0:2076b4d80327 65 #if ( INITIAL_RADIX_16==1 )
niv17 0:2076b4d80327 66 for (double *fi=f; fi<fn; fi+=16) // radix-16 step
niv17 0:2076b4d80327 67 {
niv17 0:2076b4d80327 68 double f0, f1, f2, f3;
niv17 0:2076b4d80327 69 sumdiff(fi[0], fi[1], f0, f1);
niv17 0:2076b4d80327 70 sumdiff(fi[2], fi[3], f2, f3);
niv17 0:2076b4d80327 71 sumdiff(f0, f2, fi[0], fi[2]);
niv17 0:2076b4d80327 72 sumdiff(f1, f3, fi[1], fi[3]);
niv17 0:2076b4d80327 73
niv17 0:2076b4d80327 74 sumdiff(fi[4], fi[5], f0, f1);
niv17 0:2076b4d80327 75 sumdiff(fi[6], fi[7], f2, f3);
niv17 0:2076b4d80327 76 sumdiff(f0, f2, fi[4], fi[6]);
niv17 0:2076b4d80327 77 sumdiff(f1, f3, fi[5], fi[7]);
niv17 0:2076b4d80327 78
niv17 0:2076b4d80327 79 sumdiff(fi[8], fi[9], f0, f1);
niv17 0:2076b4d80327 80 sumdiff(fi[10], fi[11], f2, f3);
niv17 0:2076b4d80327 81 sumdiff(f0, f2, fi[8], fi[10]);
niv17 0:2076b4d80327 82 sumdiff(f1, f3, fi[9], fi[11]);
niv17 0:2076b4d80327 83
niv17 0:2076b4d80327 84 sumdiff(fi[12], fi[13], f0, f1);
niv17 0:2076b4d80327 85 sumdiff(fi[14], fi[15], f2, f3);
niv17 0:2076b4d80327 86 sumdiff(f0, f2, fi[12], fi[14]);
niv17 0:2076b4d80327 87 sumdiff(f1, f3, fi[13], fi[15]);
niv17 0:2076b4d80327 88
niv17 0:2076b4d80327 89 sumdiff(fi[0], fi[4], f0, f1);
niv17 0:2076b4d80327 90 sumdiff(fi[8], fi[12], f2, f3);
niv17 0:2076b4d80327 91 sumdiff(f0, f2, fi[0], fi[8]);
niv17 0:2076b4d80327 92 sumdiff(f1, f3, fi[4], fi[12]);
niv17 0:2076b4d80327 93
niv17 0:2076b4d80327 94 sumdiff(fi[2], fi[6], f0, f1);
niv17 0:2076b4d80327 95 f3 = M_SQRT2 * fi[14];
niv17 0:2076b4d80327 96 f2 = M_SQRT2 * fi[10];
niv17 0:2076b4d80327 97 sumdiff(f0, f2, fi[2], fi[10]);
niv17 0:2076b4d80327 98 sumdiff(f1, f3, fi[6], fi[14]);
niv17 0:2076b4d80327 99
niv17 0:2076b4d80327 100 double a, b, g0, g1, g2, g3;
niv17 0:2076b4d80327 101 sumdiff(fi[5], fi[7], a, b);
niv17 0:2076b4d80327 102 a *= M_SQRT1_2;
niv17 0:2076b4d80327 103 b *= M_SQRT1_2;
niv17 0:2076b4d80327 104 sumdiff(fi[1], a, f0, f1);
niv17 0:2076b4d80327 105 sumdiff(fi[3], b, g0, g1);
niv17 0:2076b4d80327 106 sumdiff(fi[13], fi[15], a, b);
niv17 0:2076b4d80327 107 a *= M_SQRT1_2;
niv17 0:2076b4d80327 108 b *= M_SQRT1_2;
niv17 0:2076b4d80327 109 sumdiff(fi[9], a, f2, f3);
niv17 0:2076b4d80327 110 sumdiff(fi[11], b, g2, g3);
niv17 0:2076b4d80327 111 double c1 = COS_1_PI_8; // jjkeep
niv17 0:2076b4d80327 112 double s1 = SIN_1_PI_8; // jjkeep
niv17 0:2076b4d80327 113 cmult(s1, c1, f2, g3, b, a);
niv17 0:2076b4d80327 114 sumdiff(f0, a, fi[1], fi[9]);
niv17 0:2076b4d80327 115 sumdiff(g1, b, fi[7], fi[15]);
niv17 0:2076b4d80327 116 cmult(c1, s1, g2, f3, b, a);
niv17 0:2076b4d80327 117 sumdiff(g0, a, fi[3], fi[11]);
niv17 0:2076b4d80327 118 sumdiff(f1, b, fi[5], fi[13]);
niv17 0:2076b4d80327 119 }
niv17 0:2076b4d80327 120 ldk = 4;
niv17 0:2076b4d80327 121 #else // INITIAL_RADIX_16
niv17 0:2076b4d80327 122 for (double *fi=f; fi<fn; fi+=4) // radix-4 step
niv17 0:2076b4d80327 123 {
niv17 0:2076b4d80327 124 double f0, f1, f2, f3;
niv17 0:2076b4d80327 125 sumdiff(fi[0], fi[1], f0, f1);
niv17 0:2076b4d80327 126 sumdiff(fi[2], fi[3], f2, f3);
niv17 0:2076b4d80327 127 sumdiff(f0, f2, fi[0], fi[2]);
niv17 0:2076b4d80327 128 sumdiff(f1, f3, fi[1], fi[3]);
niv17 0:2076b4d80327 129 }
niv17 0:2076b4d80327 130 ldk = 2;
niv17 0:2076b4d80327 131 #endif // INITIAL_RADIX_16
niv17 0:2076b4d80327 132 }
niv17 0:2076b4d80327 133 else // ldk==1, n is no power of 4
niv17 0:2076b4d80327 134 {
niv17 0:2076b4d80327 135 for (double *fi=f; fi<fn; fi+=8) // radix-8 step
niv17 0:2076b4d80327 136 {
niv17 0:2076b4d80327 137 double g0, f0, f1, g1;
niv17 0:2076b4d80327 138 sumdiff(fi[0], fi[1], f0, g0);
niv17 0:2076b4d80327 139 sumdiff(fi[2], fi[3], f1, g1);
niv17 0:2076b4d80327 140
niv17 0:2076b4d80327 141 sumdiff(f0, f1);
niv17 0:2076b4d80327 142 sumdiff(g0, g1);
niv17 0:2076b4d80327 143
niv17 0:2076b4d80327 144 double s1, c1, s2, c2;
niv17 0:2076b4d80327 145 sumdiff(fi[4], fi[5], s1, c1);
niv17 0:2076b4d80327 146 sumdiff(fi[6], fi[7], s2, c2);
niv17 0:2076b4d80327 147
niv17 0:2076b4d80327 148 sumdiff(s1, s2);
niv17 0:2076b4d80327 149
niv17 0:2076b4d80327 150 sumdiff(f0, s1, fi[0], fi[4]);
niv17 0:2076b4d80327 151 sumdiff(f1, s2, fi[2], fi[6]);
niv17 0:2076b4d80327 152
niv17 0:2076b4d80327 153 c1 *= M_SQRT2;
niv17 0:2076b4d80327 154 c2 *= M_SQRT2;
niv17 0:2076b4d80327 155
niv17 0:2076b4d80327 156 sumdiff(g0, c1, fi[1], fi[5]);
niv17 0:2076b4d80327 157 sumdiff(g1, c2, fi[3], fi[7]);
niv17 0:2076b4d80327 158 }
niv17 0:2076b4d80327 159 ldk = 3;
niv17 0:2076b4d80327 160 }
niv17 0:2076b4d80327 161
niv17 0:2076b4d80327 162
niv17 0:2076b4d80327 163 for ( ; ldk<ldn; ldk+=2)
niv17 0:2076b4d80327 164 {
niv17 0:2076b4d80327 165 ulong k = 1UL<<ldk;
niv17 0:2076b4d80327 166 ulong kh = k >> 1;
niv17 0:2076b4d80327 167 ulong k2 = k << 1;
niv17 0:2076b4d80327 168 ulong k3 = k2 + k;
niv17 0:2076b4d80327 169 ulong k4 = k2 << 1;
niv17 0:2076b4d80327 170
niv17 0:2076b4d80327 171 for (double *fi=f, *gi=f+kh; fi<fn; fi+=k4, gi+=k4)
niv17 0:2076b4d80327 172 {
niv17 0:2076b4d80327 173 double f0, f1, f2, f3;
niv17 0:2076b4d80327 174 sumdiff(fi[0], fi[k], f0, f1);
niv17 0:2076b4d80327 175 sumdiff(fi[k2], fi[k3], f2, f3);
niv17 0:2076b4d80327 176 sumdiff(f0, f2, fi[0], fi[k2]);
niv17 0:2076b4d80327 177 sumdiff(f1, f3, fi[k], fi[k3]);
niv17 0:2076b4d80327 178
niv17 0:2076b4d80327 179 sumdiff(gi[0], gi[k], f0, f1);
niv17 0:2076b4d80327 180 f3 = M_SQRT2 * gi[k3];
niv17 0:2076b4d80327 181 f2 = M_SQRT2 * gi[k2];
niv17 0:2076b4d80327 182 sumdiff(f0, f2, gi[0], gi[k2]);
niv17 0:2076b4d80327 183 sumdiff(f1, f3, gi[k], gi[k3]);
niv17 0:2076b4d80327 184 }
niv17 0:2076b4d80327 185
niv17 0:2076b4d80327 186 double tt = M_PI_4/(double)kh; // jjkeep
niv17 0:2076b4d80327 187 #if defined USE_TRIG_REC
niv17 0:2076b4d80327 188 double s1 = 0.0, c1 = 1.0; // jjkeep
niv17 0:2076b4d80327 189 double al = sin(0.5*tt); // jjkeep
niv17 0:2076b4d80327 190 al *= (2.0*al);
niv17 0:2076b4d80327 191 double be = sin(tt); // jjkeep
niv17 0:2076b4d80327 192 #endif // USE_TRIG_REC
niv17 0:2076b4d80327 193
niv17 0:2076b4d80327 194 for (ulong i=1; i<kh; i++)
niv17 0:2076b4d80327 195 {
niv17 0:2076b4d80327 196 #if defined USE_TRIG_REC
niv17 0:2076b4d80327 197 {
niv17 0:2076b4d80327 198 double t1 = c1; // jjkeep
niv17 0:2076b4d80327 199 c1 -= (al*t1+be*s1);
niv17 0:2076b4d80327 200 s1 -= (al*s1-be*t1);
niv17 0:2076b4d80327 201 }
niv17 0:2076b4d80327 202 #else
niv17 0:2076b4d80327 203 double s1, c1; // jjkeep
niv17 0:2076b4d80327 204 SinCos(tt*(double)i, &s1, &c1); // jjkeep
niv17 0:2076b4d80327 205 #endif // USE_TRIG_REC
niv17 0:2076b4d80327 206
niv17 0:2076b4d80327 207 double c2, s2; // jjkeep
niv17 0:2076b4d80327 208 csqr(c1, s1, c2, s2);
niv17 0:2076b4d80327 209
niv17 0:2076b4d80327 210 for (double *fi=f+i, *gi=f+k-i; fi<fn; fi+=k4, gi+=k4)
niv17 0:2076b4d80327 211 {
niv17 0:2076b4d80327 212 double a, b, g0, f0, f1, g1, f2, g2, f3, g3;
niv17 0:2076b4d80327 213 cmult(s2, c2, fi[k], gi[k], b, a);
niv17 0:2076b4d80327 214 sumdiff(fi[0], a, f0, f1);
niv17 0:2076b4d80327 215 sumdiff(gi[0], b, g0, g1);
niv17 0:2076b4d80327 216
niv17 0:2076b4d80327 217 cmult(s2, c2, fi[k3], gi[k3], b, a);
niv17 0:2076b4d80327 218 sumdiff(fi[k2], a, f2, f3);
niv17 0:2076b4d80327 219 sumdiff(gi[k2], b, g2, g3);
niv17 0:2076b4d80327 220
niv17 0:2076b4d80327 221 cmult(s1, c1, f2, g3, b, a);
niv17 0:2076b4d80327 222 sumdiff(f0, a, fi[0], fi[k2]);
niv17 0:2076b4d80327 223 sumdiff(g1, b, gi[k], gi[k3]);
niv17 0:2076b4d80327 224
niv17 0:2076b4d80327 225 cmult(c1, s1, g2, f3, b, a);
niv17 0:2076b4d80327 226 sumdiff(g0, a, gi[0], gi[k2]);
niv17 0:2076b4d80327 227 sumdiff(f1, b, fi[k], fi[k3]);
niv17 0:2076b4d80327 228 }
niv17 0:2076b4d80327 229 }
niv17 0:2076b4d80327 230 }
niv17 0:2076b4d80327 231 }
niv17 0:2076b4d80327 232 // -------------------------
niv17 0:2076b4d80327 233
niv17 0:2076b4d80327 234
niv17 0:2076b4d80327 235 void
niv17 0:2076b4d80327 236 fht_dit(double *f, ulong ldn)
niv17 0:2076b4d80327 237 // Fast Hartley Transform.
niv17 0:2076b4d80327 238 // Split-radix decimation in time (DIT) algorithm.
niv17 0:2076b4d80327 239 // ldn := base-2 logarithm of the array length.
niv17 0:2076b4d80327 240 {
niv17 0:2076b4d80327 241 if ( ldn<=2 )
niv17 0:2076b4d80327 242 {
niv17 0:2076b4d80327 243 if ( ldn==1 ) // two point fht
niv17 0:2076b4d80327 244 {
niv17 0:2076b4d80327 245 sumdiff(f[0], f[1]);
niv17 0:2076b4d80327 246 }
niv17 0:2076b4d80327 247 else if ( ldn==2 ) // four point fht
niv17 0:2076b4d80327 248 {
niv17 0:2076b4d80327 249 double f0, f1, f2, f3;
niv17 0:2076b4d80327 250 sumdiff(f[0], f[2], f0, f1);
niv17 0:2076b4d80327 251 sumdiff(f[1], f[3], f2, f3);
niv17 0:2076b4d80327 252 sumdiff(f0, f2, f[0], f[2]);
niv17 0:2076b4d80327 253 sumdiff(f1, f3, f[1], f[3]);
niv17 0:2076b4d80327 254 }
niv17 0:2076b4d80327 255
niv17 0:2076b4d80327 256 return;
niv17 0:2076b4d80327 257 }
niv17 0:2076b4d80327 258
niv17 0:2076b4d80327 259 revbin_permute(f, 1UL<<ldn);
niv17 0:2076b4d80327 260 fht_dit_core(f, ldn);
niv17 0:2076b4d80327 261 }
niv17 0:2076b4d80327 262 // -------------------------